Brick Beating Code
/***************** Headers/Constants *****************/
#include <Timers.h>
// Possible states - enum used so expanding is easy
enum states{
WAITING, // 0
SEARCHING, // 1
FREE_SPINNING, // 2
DRIVING_TO_TAPE, // 3
DRIVING_ALONG_TAPE, // 4
DRIVING_TO_OPPONENT, // 5
PUSHING, // 6
SPINNING_ON_TAPE, // 7
SPINNING_OFF_TAPE, // 8
};
// Cardinal Directions of IR beacon sensors - all relative to robot (Front = North)
#define NORTH_EAST B10000000
#define NORTH B01000000
#define NORTH_WEST B00100000
#define WEST B00010000
#define SOUTH_WEST B00001000
#define SOUTH B00000100
#define SOUTH_EAST B00000010
#define EAST B00000001
#define ERROR_SIGNAL -1
// Tape sensor locations
#define FRONT_TAPE B01000000
#define BACK_TAPE B00000100
#define LEFT_TAPE B00010000
#define RIGHT_TAPE B00000001
// Front bumper signals (L = Left, M = Middle, R = Right)
#define L_BUMP B00000001
#define LM_BUMP B00000010
#define M_BUMP B00000100
#define RM_BUMP B00001000
#define R_BUMP B00010000
#define LMR_BUMP B00100000
// Movement directions
#define IN_FRONT 0
#define LEFT_TURN 1
#define RIGHT_TURN 2
// Movement speeds
#define FAST 10 //???
// Timers
#define BEACON_TIMER 1
#define BEACON_SAMPLING_RATE 1000/8 // 125ms - full sweep every second
#define TAPE_TIMER 2
#define TAPE_SAMPLING_RATE 1000/100 // 10ms - 25 full sweeps every second
//I/O pins - to be easily changed
/* PIN MAPPING
D0 - Tape mux
D1 - Tape mux
int D2 - Tape in
int~D3 - Beacon in
D4 -
~D5 - Beacon mux
~D6 - Beacon mux
D7 - Beacon mux
D8 - Left Motor PWR
~D9 - Left Motor DIR
~D10 - Right Motor PWR
~D11 - Right Motor DIR
D12 -
D13 -
A0 - 850Hz signal
A1 - 4kHz signal
A2 -
A3 -
A4 -
A5 -
*/
#define TAPE_MUX_BIT_0 0
#define TAPE_MUX_BIT_1 (TAPE_MUX_BIT_0 + 1)
#define TAPE_IN 2
// Tape sensor input is through interrupt
#define BEACON_MUX_BIT_0 5
#define BEACON_MUX_BIT_1 (BEACON_MUX_BIT_0 + 1)
#define BEACON_MUX_BIT_2 (BEACON_MUX_BIT_1 + 1)
#define BEACON_IN 3
#define MOTOR_LEFT_PRW 8
#define MOTOR_LEFT_DIR 9
#define MOTOR_RIGHT_PWR 10
#define MOTOR_RIGHT_DIR 11
/******************** Prototypes *********************/
void moveForward(unsigned char moveSpeed); // moves bot forward
void spin(unsigned char turnDirection); // turns bot
void push(unsigned char pushTechnique); // makes bot push
void stopMoving(); // stops all self-induced motor motion
unsigned char startBPed(); // returns if start button was pressed
unsigned char getDirectionToOpponent(); // returns opponents relative direction
unsigned char getopponentSensorData(); // returns in opponent sensor input
//void sampleBeaconSensor(); // advances beacon mux then reads sensor
//void readBeaconSensor();
void tapeTriggered();
void beaconTriggered();
/**************** Brick State Diagram ****************/
static unsigned int wallSensorData;
static unsigned int opponentSensorData;
static unsigned int tapeSensorData;
static unsigned int bumpSensorInput;
//static unsigned int 850_HIGH_THRESHOLD; // variable so it can be a dynamic threshold
//static unsigned int 4000_HIGH_THRESHOLD; // variable so it can be a dynamic threshold
static volatile unsigned long lastBeaconPulse = 0;
static volatile unsigned char blackTapeFound = false; // need bette system
void setup() {
Serial.println("Starting Palpatine Machine...");
Serial.begin(9600);
// Pin Initializations - couldn't figure out how to generalize this using port manipulation
// Outputs
pinMode(TAPE_MUX_BIT_0, OUTPUT);
pinMode(TAPE_MUX_BIT_1, OUTPUT);
pinMode(BEACON_MUX_BIT_0, OUTPUT);
pinMode(BEACON_MUX_BIT_1, OUTPUT);
pinMode(BEACON_MUX_BIT_2, OUTPUT);
pinMode(MOTOR_LEFT_PRW, OUTPUT);
pinMode(MOTOR_LEFT_DIR, OUTPUT);
pinMode(MOTOR_RIGHT_PWR, OUTPUT);
pinMode(MOTOR_RIGHT_DIR, OUTPUT);
// Inputs
pinMode(BEACON_IN, INPUT);
pinMode(TAPE_IN, INPUT);
// Interrupts
attachInterrupt(0, tapeTriggered, RISING);
attachInterrupt(1, beaconTriggered, RISING);
}
void loop() {
static unsigned char state = WAITING;
//*** Read sensors ***//
if (TMRArd_IsTimerExpired(BEACON_TIMER) == TMRArd_EXPIRED) sampleBeaconSensor();
if (TMRArd_IsTimerExpired(TAPE_TIMER) == TMRArd_EXPIRED) sampleTapeSensor();
//*** End sensors ***//
switch(state) {
/* WAITING - occurs at start
Robot is waiting for the start signal to enter state machine
Good time for initializations
Events: start button pressed
Action: start state machine
*/
case WAITING:
if (startBPed()) state = SEARCHING;
break;
/* SEARCHING - occurs when robot is unsure of what to do (confused state)
First thing robot does to orient itself
Also happens when plan is interrupted (robot gets lost)
Events: wall found
Action: start spinning right to orient bot
*/
case SEARCHING:
TMRArd_InitTimer(BEACON_TIMER, BEACON_SAMPLING_RATE);
if (wallSensorData) {
stopMoving();
spin(RIGHT_TURN);
state = FREE_SPINNING;
}
break;
/* FREE_SPINNING - occurs when robot reorienting after search
Events: wall is to the left
Action: orient bot so wall is on the left
Action: drive forward to edge of platform
*/
case FREE_SPINNING:
if (wallSensorData & WEST) {
stopMoving();
moveForward(FAST);
state = DRIVING_TO_TAPE;
}
break;
/* DRIVING_TO_TAPE - occurs when robot is moving to edge
Events: tape reached
Action: turn on tape in direction of opponent
*/
case DRIVING_TO_TAPE:
if ((tapeSensorData & LEFT_TAPE) || (tapeSensorData & RIGHT_TAPE)) {
stopMoving();
spin(getDirectionToOpponent());
state = SPINNING_ON_TAPE;
}
break;
/* SPINNING_ON_TAPE - occurs when robot is turning while at edge (on tape)
Events: bot is looking in direction of tape
Action: drive forward along tape
*/
case SPINNING_ON_TAPE:
if (tapeSensorData & FRONT_TAPE) {
stopMoving();
moveForward(FAST);
state = DRIVING_ALONG_TAPE;
}
break;
/* DRIVING_ALONG_TAPE - occurs when robot is at edge (on tape) and opponent is at side
Events: opponent is directly to the side
Action: turn toward opponent
*/
case DRIVING_ALONG_TAPE:
if ((opponentSensorData & WEST) || (opponentSensorData & EAST)) {
stopMoving();
spin(getDirectionToOpponent());
state = SPINNING_OFF_TAPE;
}
break;
/* SPINNING_OFF_TAPE - occurs when robot is at edge (on tape) and turning toward opponent
Events: bot has turned 90º and is (theoretically) facing into arena
Action: move forward toward opponent
*/
case SPINNING_OFF_TAPE:
if (getDirectionToOpponent() == IN_FRONT) {
stopMoving();
moveForward(FAST);
state = DRIVING_TO_OPPONENT;
}
break;
/* DRIVING_TO_OPPONENT - occurs when robot is approaching opponent from edge
Events: bot hits something (theoretically the opponent)
Action: PUSH!!!
*/
case DRIVING_TO_OPPONENT:
if (getDirectionToOpponent() != IN_FRONT) {
spin(getDirectionToOpponent()); // opponent is no longer at ahead
state = SPINNING_OFF_TAPE; // not ideal
}
if (bumpSensorInput) {
state = PUSHING;
}
break;
/* PUSHING - occurs when robot is in contact with opponent
Events: bumper sensors are pressed
Action: adjust push to keep opponent in front
*/
case PUSHING:
switch (bumpSensorInput) {
case L_BUMP:
case LM_BUMP:
// push harder on left side
break;
case M_BUMP:
// push as hard as possible - evenly
break;
case RM_BUMP:
case R_BUMP:
// push harder on right side
break;
case LMR_BUMP:
// pushing against wall???
break;
default: // no bump sensed - loss of contact
state = SEARCHING;
break;
}
break;
// ----------- //
default:
state = SEARCHING;
break;
}
}
/***************** Movement Methods ******************/
/* stopMoving - does xyz */
void stopMoving() {
// implement stop here - may be different than braking - set all motor speeds to 0
}
/* moveForward - does xyz */
void moveForward(unsigned char moveSpeed) {
// implement forward movement here -> all motors moving at same speed
}
/* spin - does xyz */
void spin(unsigned char turnDirection) {
// implement spinning here - turnDirection = LEFT_TURN or RIGHT_TURN
}
/* push - does xyz */
void push(unsigned char pushTechnique) {
// implement pushing here
}
/************** Beacon Sensing Methods ***************/
/* sampleBeaconSensor - advances the beacon mux port then reads the current sensor */
void sampleBeaconSensor() {
// wanted to use port manipulation, but couldn't figure out how to make it generalized so we could change the pin #s
static unsigned char sensorDirection = NORTH;
// read current sensor
// readBeaconSensor(sensorDirection);
// advance mux and change which sensor is read
switch (sensorDirection) { // checks in clockwise direction
case NORTH:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = NORTH_EAST;
break;
case NORTH_EAST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = EAST;
break;
case EAST:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = SOUTH_EAST;
break;
case SOUTH_EAST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = SOUTH;
break;
case SOUTH:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = SOUTH_WEST;
break;
case SOUTH_WEST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = WEST;
break;
case WEST:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = NORTH_WEST;
break;
case NORTH_WEST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = NORTH;
break;
default:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = NORTH;
break;
}
// reset sampling timer
TMRArd_ClearTimerExpired(BEACON_TIMER);
TMRArd_InitTimer(BEACON_TIMER, BEACON_SAMPLING_RATE);
}
/* getDirectionToOpponent - does xyz */
unsigned char getDirectionToOpponent() {
switch (opponentSensorData) {
case NORTH_EAST:
case EAST:
case SOUTH_EAST:
return RIGHT_TURN; // opponent is to right of bot
case NORTH_WEST:
case WEST:
case SOUTH_WEST:
return LEFT_TURN; // opponent is to left of bot
case SOUTH: // could go either direction, arbitrarily decided left
return LEFT_TURN;
case NORTH:
return IN_FRONT;
// Don't turn????
default:
return ERROR_SIGNAL; // opponent not found or in multiple locations
//THIS CANNOT RETURN AN ERROR SIGNAL!!!! - IT IS UNSIGNED
// CHANGE IT TO A SIGNED CHAR
break;
}
}
void beaconTriggered() {
lastBeaconPulse = TMRArd_GetTime();
}
/* getopponentSensorData - returns xyz */
//unsigned char readBeaconSensor(unsigned char sensorNumber) {
// int signalAt850 = analogRead(BEACON_850_IN);
// int signalAt4k = analogRead(BEACON_4000_IN);
//
// if (signalAt850 > 850_HIGH_THRESHOLD) opponentSensorData = (opponentSensorData | sensorNumber); // set bit high
// else opponentSensorData = (opponentSensorData & (~sensorNumber); // set bit low
//
// if (signalAt4k > 4000_HIGH_THRESHOLD) wallSensorData = (wallSensorData | sensorNumber); // set bit high
// else wallSensorData = (wallSensorData & (~sensorNumber); // set bit low
//}
/*************** Tape Sensing Methods ****************/
void sampleTapeSensor() {
static unsigned char sensorDirection = FRONT_TAPE;
if (blackTapeFound == true) {
blackTapeFound = false;
tapeSensorData = (tapeSensorData | sensorDirection); // set bit high
} else tapeSensorData = (tapeSensorData & (~sensorDirection)); // set bit low
switch(sensorDirection) { // samples in clockwise direction
case FRONT_TAPE:
digitalWrite(TAPE_MUX_BIT_0, HIGH);
digitalWrite(TAPE_MUX_BIT_1, LOW);
sensorDirection = LEFT_TAPE;
break;
case RIGHT_TAPE:
digitalWrite(TAPE_MUX_BIT_0, LOW);
digitalWrite(TAPE_MUX_BIT_1, HIGH);
sensorDirection = BACK_TAPE;
break;
case BACK_TAPE:
digitalWrite(TAPE_MUX_BIT_0, HIGH);
digitalWrite(TAPE_MUX_BIT_1, HIGH);
sensorDirection = LEFT_TAPE;
break;
case LEFT_TAPE:
digitalWrite(TAPE_MUX_BIT_0, LOW);
digitalWrite(TAPE_MUX_BIT_1, LOW);
sensorDirection = FRONT_TAPE;
break;
default:
digitalWrite(TAPE_MUX_BIT_0, LOW);
digitalWrite(TAPE_MUX_BIT_1, LOW);
sensorDirection = FRONT_TAPE;
break;
}
// reset sampling timer
TMRArd_ClearTimerExpired(TAPE_TIMER);
TMRArd_InitTimer(TAPE_TIMER, TAPE_SAMPLING_RATE);
}
void tapeTriggered() {
blackTapeFound = true;
}
/************** Manual Control Methods ***************/
/* startBPed - returns xyz */
unsigned char startBPed() {
// implement check for start button press
}
#include <Timers.h>
// Possible states - enum used so expanding is easy
enum states{
WAITING, // 0
SEARCHING, // 1
FREE_SPINNING, // 2
DRIVING_TO_TAPE, // 3
DRIVING_ALONG_TAPE, // 4
DRIVING_TO_OPPONENT, // 5
PUSHING, // 6
SPINNING_ON_TAPE, // 7
SPINNING_OFF_TAPE, // 8
};
// Cardinal Directions of IR beacon sensors - all relative to robot (Front = North)
#define NORTH_EAST B10000000
#define NORTH B01000000
#define NORTH_WEST B00100000
#define WEST B00010000
#define SOUTH_WEST B00001000
#define SOUTH B00000100
#define SOUTH_EAST B00000010
#define EAST B00000001
#define ERROR_SIGNAL -1
// Tape sensor locations
#define FRONT_TAPE B01000000
#define BACK_TAPE B00000100
#define LEFT_TAPE B00010000
#define RIGHT_TAPE B00000001
// Front bumper signals (L = Left, M = Middle, R = Right)
#define L_BUMP B00000001
#define LM_BUMP B00000010
#define M_BUMP B00000100
#define RM_BUMP B00001000
#define R_BUMP B00010000
#define LMR_BUMP B00100000
// Movement directions
#define IN_FRONT 0
#define LEFT_TURN 1
#define RIGHT_TURN 2
// Movement speeds
#define FAST 10 //???
// Timers
#define BEACON_TIMER 1
#define BEACON_SAMPLING_RATE 1000/8 // 125ms - full sweep every second
#define TAPE_TIMER 2
#define TAPE_SAMPLING_RATE 1000/100 // 10ms - 25 full sweeps every second
//I/O pins - to be easily changed
/* PIN MAPPING
D0 - Tape mux
D1 - Tape mux
int D2 - Tape in
int~D3 - Beacon in
D4 -
~D5 - Beacon mux
~D6 - Beacon mux
D7 - Beacon mux
D8 - Left Motor PWR
~D9 - Left Motor DIR
~D10 - Right Motor PWR
~D11 - Right Motor DIR
D12 -
D13 -
A0 - 850Hz signal
A1 - 4kHz signal
A2 -
A3 -
A4 -
A5 -
*/
#define TAPE_MUX_BIT_0 0
#define TAPE_MUX_BIT_1 (TAPE_MUX_BIT_0 + 1)
#define TAPE_IN 2
// Tape sensor input is through interrupt
#define BEACON_MUX_BIT_0 5
#define BEACON_MUX_BIT_1 (BEACON_MUX_BIT_0 + 1)
#define BEACON_MUX_BIT_2 (BEACON_MUX_BIT_1 + 1)
#define BEACON_IN 3
#define MOTOR_LEFT_PRW 8
#define MOTOR_LEFT_DIR 9
#define MOTOR_RIGHT_PWR 10
#define MOTOR_RIGHT_DIR 11
/******************** Prototypes *********************/
void moveForward(unsigned char moveSpeed); // moves bot forward
void spin(unsigned char turnDirection); // turns bot
void push(unsigned char pushTechnique); // makes bot push
void stopMoving(); // stops all self-induced motor motion
unsigned char startBPed(); // returns if start button was pressed
unsigned char getDirectionToOpponent(); // returns opponents relative direction
unsigned char getopponentSensorData(); // returns in opponent sensor input
//void sampleBeaconSensor(); // advances beacon mux then reads sensor
//void readBeaconSensor();
void tapeTriggered();
void beaconTriggered();
/**************** Brick State Diagram ****************/
static unsigned int wallSensorData;
static unsigned int opponentSensorData;
static unsigned int tapeSensorData;
static unsigned int bumpSensorInput;
//static unsigned int 850_HIGH_THRESHOLD; // variable so it can be a dynamic threshold
//static unsigned int 4000_HIGH_THRESHOLD; // variable so it can be a dynamic threshold
static volatile unsigned long lastBeaconPulse = 0;
static volatile unsigned char blackTapeFound = false; // need bette system
void setup() {
Serial.println("Starting Palpatine Machine...");
Serial.begin(9600);
// Pin Initializations - couldn't figure out how to generalize this using port manipulation
// Outputs
pinMode(TAPE_MUX_BIT_0, OUTPUT);
pinMode(TAPE_MUX_BIT_1, OUTPUT);
pinMode(BEACON_MUX_BIT_0, OUTPUT);
pinMode(BEACON_MUX_BIT_1, OUTPUT);
pinMode(BEACON_MUX_BIT_2, OUTPUT);
pinMode(MOTOR_LEFT_PRW, OUTPUT);
pinMode(MOTOR_LEFT_DIR, OUTPUT);
pinMode(MOTOR_RIGHT_PWR, OUTPUT);
pinMode(MOTOR_RIGHT_DIR, OUTPUT);
// Inputs
pinMode(BEACON_IN, INPUT);
pinMode(TAPE_IN, INPUT);
// Interrupts
attachInterrupt(0, tapeTriggered, RISING);
attachInterrupt(1, beaconTriggered, RISING);
}
void loop() {
static unsigned char state = WAITING;
//*** Read sensors ***//
if (TMRArd_IsTimerExpired(BEACON_TIMER) == TMRArd_EXPIRED) sampleBeaconSensor();
if (TMRArd_IsTimerExpired(TAPE_TIMER) == TMRArd_EXPIRED) sampleTapeSensor();
//*** End sensors ***//
switch(state) {
/* WAITING - occurs at start
Robot is waiting for the start signal to enter state machine
Good time for initializations
Events: start button pressed
Action: start state machine
*/
case WAITING:
if (startBPed()) state = SEARCHING;
break;
/* SEARCHING - occurs when robot is unsure of what to do (confused state)
First thing robot does to orient itself
Also happens when plan is interrupted (robot gets lost)
Events: wall found
Action: start spinning right to orient bot
*/
case SEARCHING:
TMRArd_InitTimer(BEACON_TIMER, BEACON_SAMPLING_RATE);
if (wallSensorData) {
stopMoving();
spin(RIGHT_TURN);
state = FREE_SPINNING;
}
break;
/* FREE_SPINNING - occurs when robot reorienting after search
Events: wall is to the left
Action: orient bot so wall is on the left
Action: drive forward to edge of platform
*/
case FREE_SPINNING:
if (wallSensorData & WEST) {
stopMoving();
moveForward(FAST);
state = DRIVING_TO_TAPE;
}
break;
/* DRIVING_TO_TAPE - occurs when robot is moving to edge
Events: tape reached
Action: turn on tape in direction of opponent
*/
case DRIVING_TO_TAPE:
if ((tapeSensorData & LEFT_TAPE) || (tapeSensorData & RIGHT_TAPE)) {
stopMoving();
spin(getDirectionToOpponent());
state = SPINNING_ON_TAPE;
}
break;
/* SPINNING_ON_TAPE - occurs when robot is turning while at edge (on tape)
Events: bot is looking in direction of tape
Action: drive forward along tape
*/
case SPINNING_ON_TAPE:
if (tapeSensorData & FRONT_TAPE) {
stopMoving();
moveForward(FAST);
state = DRIVING_ALONG_TAPE;
}
break;
/* DRIVING_ALONG_TAPE - occurs when robot is at edge (on tape) and opponent is at side
Events: opponent is directly to the side
Action: turn toward opponent
*/
case DRIVING_ALONG_TAPE:
if ((opponentSensorData & WEST) || (opponentSensorData & EAST)) {
stopMoving();
spin(getDirectionToOpponent());
state = SPINNING_OFF_TAPE;
}
break;
/* SPINNING_OFF_TAPE - occurs when robot is at edge (on tape) and turning toward opponent
Events: bot has turned 90º and is (theoretically) facing into arena
Action: move forward toward opponent
*/
case SPINNING_OFF_TAPE:
if (getDirectionToOpponent() == IN_FRONT) {
stopMoving();
moveForward(FAST);
state = DRIVING_TO_OPPONENT;
}
break;
/* DRIVING_TO_OPPONENT - occurs when robot is approaching opponent from edge
Events: bot hits something (theoretically the opponent)
Action: PUSH!!!
*/
case DRIVING_TO_OPPONENT:
if (getDirectionToOpponent() != IN_FRONT) {
spin(getDirectionToOpponent()); // opponent is no longer at ahead
state = SPINNING_OFF_TAPE; // not ideal
}
if (bumpSensorInput) {
state = PUSHING;
}
break;
/* PUSHING - occurs when robot is in contact with opponent
Events: bumper sensors are pressed
Action: adjust push to keep opponent in front
*/
case PUSHING:
switch (bumpSensorInput) {
case L_BUMP:
case LM_BUMP:
// push harder on left side
break;
case M_BUMP:
// push as hard as possible - evenly
break;
case RM_BUMP:
case R_BUMP:
// push harder on right side
break;
case LMR_BUMP:
// pushing against wall???
break;
default: // no bump sensed - loss of contact
state = SEARCHING;
break;
}
break;
// ----------- //
default:
state = SEARCHING;
break;
}
}
/***************** Movement Methods ******************/
/* stopMoving - does xyz */
void stopMoving() {
// implement stop here - may be different than braking - set all motor speeds to 0
}
/* moveForward - does xyz */
void moveForward(unsigned char moveSpeed) {
// implement forward movement here -> all motors moving at same speed
}
/* spin - does xyz */
void spin(unsigned char turnDirection) {
// implement spinning here - turnDirection = LEFT_TURN or RIGHT_TURN
}
/* push - does xyz */
void push(unsigned char pushTechnique) {
// implement pushing here
}
/************** Beacon Sensing Methods ***************/
/* sampleBeaconSensor - advances the beacon mux port then reads the current sensor */
void sampleBeaconSensor() {
// wanted to use port manipulation, but couldn't figure out how to make it generalized so we could change the pin #s
static unsigned char sensorDirection = NORTH;
// read current sensor
// readBeaconSensor(sensorDirection);
// advance mux and change which sensor is read
switch (sensorDirection) { // checks in clockwise direction
case NORTH:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = NORTH_EAST;
break;
case NORTH_EAST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = EAST;
break;
case EAST:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = SOUTH_EAST;
break;
case SOUTH_EAST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = SOUTH;
break;
case SOUTH:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = SOUTH_WEST;
break;
case SOUTH_WEST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = WEST;
break;
case WEST:
digitalWrite(BEACON_MUX_BIT_0, HIGH);
digitalWrite(BEACON_MUX_BIT_1, HIGH);
digitalWrite(BEACON_MUX_BIT_2, HIGH);
sensorDirection = NORTH_WEST;
break;
case NORTH_WEST:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = NORTH;
break;
default:
digitalWrite(BEACON_MUX_BIT_0, LOW);
digitalWrite(BEACON_MUX_BIT_1, LOW);
digitalWrite(BEACON_MUX_BIT_2, LOW);
sensorDirection = NORTH;
break;
}
// reset sampling timer
TMRArd_ClearTimerExpired(BEACON_TIMER);
TMRArd_InitTimer(BEACON_TIMER, BEACON_SAMPLING_RATE);
}
/* getDirectionToOpponent - does xyz */
unsigned char getDirectionToOpponent() {
switch (opponentSensorData) {
case NORTH_EAST:
case EAST:
case SOUTH_EAST:
return RIGHT_TURN; // opponent is to right of bot
case NORTH_WEST:
case WEST:
case SOUTH_WEST:
return LEFT_TURN; // opponent is to left of bot
case SOUTH: // could go either direction, arbitrarily decided left
return LEFT_TURN;
case NORTH:
return IN_FRONT;
// Don't turn????
default:
return ERROR_SIGNAL; // opponent not found or in multiple locations
//THIS CANNOT RETURN AN ERROR SIGNAL!!!! - IT IS UNSIGNED
// CHANGE IT TO A SIGNED CHAR
break;
}
}
void beaconTriggered() {
lastBeaconPulse = TMRArd_GetTime();
}
/* getopponentSensorData - returns xyz */
//unsigned char readBeaconSensor(unsigned char sensorNumber) {
// int signalAt850 = analogRead(BEACON_850_IN);
// int signalAt4k = analogRead(BEACON_4000_IN);
//
// if (signalAt850 > 850_HIGH_THRESHOLD) opponentSensorData = (opponentSensorData | sensorNumber); // set bit high
// else opponentSensorData = (opponentSensorData & (~sensorNumber); // set bit low
//
// if (signalAt4k > 4000_HIGH_THRESHOLD) wallSensorData = (wallSensorData | sensorNumber); // set bit high
// else wallSensorData = (wallSensorData & (~sensorNumber); // set bit low
//}
/*************** Tape Sensing Methods ****************/
void sampleTapeSensor() {
static unsigned char sensorDirection = FRONT_TAPE;
if (blackTapeFound == true) {
blackTapeFound = false;
tapeSensorData = (tapeSensorData | sensorDirection); // set bit high
} else tapeSensorData = (tapeSensorData & (~sensorDirection)); // set bit low
switch(sensorDirection) { // samples in clockwise direction
case FRONT_TAPE:
digitalWrite(TAPE_MUX_BIT_0, HIGH);
digitalWrite(TAPE_MUX_BIT_1, LOW);
sensorDirection = LEFT_TAPE;
break;
case RIGHT_TAPE:
digitalWrite(TAPE_MUX_BIT_0, LOW);
digitalWrite(TAPE_MUX_BIT_1, HIGH);
sensorDirection = BACK_TAPE;
break;
case BACK_TAPE:
digitalWrite(TAPE_MUX_BIT_0, HIGH);
digitalWrite(TAPE_MUX_BIT_1, HIGH);
sensorDirection = LEFT_TAPE;
break;
case LEFT_TAPE:
digitalWrite(TAPE_MUX_BIT_0, LOW);
digitalWrite(TAPE_MUX_BIT_1, LOW);
sensorDirection = FRONT_TAPE;
break;
default:
digitalWrite(TAPE_MUX_BIT_0, LOW);
digitalWrite(TAPE_MUX_BIT_1, LOW);
sensorDirection = FRONT_TAPE;
break;
}
// reset sampling timer
TMRArd_ClearTimerExpired(TAPE_TIMER);
TMRArd_InitTimer(TAPE_TIMER, TAPE_SAMPLING_RATE);
}
void tapeTriggered() {
blackTapeFound = true;
}
/************** Manual Control Methods ***************/
/* startBPed - returns xyz */
unsigned char startBPed() {
// implement check for start button press
}