Final Competition Code
// Includes ------------------------------------------------
#include <Timers.h>
// Module Defines -----------------------------------------
#define FIRST_TAPE 1
#define FL_TAPE 1
#define FR_TAPE 2
#define BL_TAPE 3
#define BR_TAPE 4
#define NUM_TAPES 4
#define TAPE_ERROR 5
#define NORTH 0
#define NORTH_WEST 1
#define WEST 2
#define SOUTH_WEST 3
#define SOUTH 4
#define SOUTH_EAST 5
#define EAST 6
#define NORTH_EAST 7
#define ERROR_SIGNAL 8
// frequency constants
#define LOW_4000_THRESHOLD 3900
#define HIGH_4000_THRESHOLD 4000
#define LOW_850_THRESHOLD 700
#define HIGH_850_THRESHOLD 1000
// Arduino pins
#define TAPE_PIN_OUT1 8 // red
#define TAPE_PIN_OUT2 9 // white
#define TAPE_PIN_IN 4 // white/red
#define MOTOR_RIGHT_PWR 10 // black ~
#define MOTOR_LEFT_PWR 11 // green ~
#define MOTOR_LEFT_DIR 12 // red
#define MOTOR_RIGHT_DIR 13 // yellow -> then white to ground
#define IR_PIN_OUT1 5 // black
#define IR_PIN_OUT2 6 // white
#define IR_PIN_OUT3 7 // red
#define IR_IN 2 // interrupt - white
#define FRONT_BUMPER 3 // red
#define DIRECTION_SENSOR A0 // white
#define UNUSED_PIN2 1
#define UNUSED_PIN3 0
// timers
#define BACK_TIMER 0
#define BACK_INTERVAL 2500
#define IR_TIMER 1
#define IR_INTERVAL 10
#define TAPE_TIMER 2
#define TAPE_INTERVAL 20
#define CONFUSED_TIMER 3
#define CONFUSED_INTERVAL 10000
#define STUCK_TIMER 4
#define STUCK_INTERVAL 2000
#define DIRECTION_TIMER 5
#define DIRECTION_INTERVAL 1500
#define CONFUSED_SPIN_TIMER 6
#define CONFUSED_SPIN_INTERVAL 3000
#define OFFENSE_TIME 200 // in seconds
// motor constants
#define LEFT_FWD LOW
#define RIGHT_FWD HIGH
#define IN_FRONT 0
#define LEFT_TURN -1
#define RIGHT_TURN 1
#define FASTEST 255
#define SPIN_SPEED 180
#define BACK_SPEED 120
#define FORWARDS 0
#define BACKWARDS 1
#define TURN_SPEED 0
typedef enum states {
searching,
attacking,
avoiding,
spinning,
beserk,
lookingForWall,
hiding,
escaping
}
states;
typedef enum beserkStates {
turning,
backingUp,
movingFwd
}
beserkStates;
// Module Function Prototypes
// Module Variables
static unsigned char currentTape = FIRST_TAPE;
static unsigned char currentIR = NORTH;
static volatile unsigned long curTime;
static volatile unsigned long lastTime = 0;
static volatile unsigned long period = 0;
static signed int opponentLocation = ERROR_SIGNAL;
static signed int wallLocation = ERROR_SIGNAL;
static states currentState = searching;
static beserkStates beserkState = movingFwd;
static unsigned char beingBumped = 0;
// Arduino Main Functions
void setup() { // setup() function required for Arduino
Serial.begin(9600);
Serial.println("Yeah Brah, lez do this!");
pinMode(TAPE_PIN_OUT1, OUTPUT);
pinMode(TAPE_PIN_OUT2, OUTPUT);
pinMode(TAPE_PIN_IN, INPUT);
pinMode(IR_PIN_OUT1, OUTPUT);
pinMode(IR_PIN_OUT2, OUTPUT);
pinMode(IR_PIN_OUT3, OUTPUT);
pinMode(IR_IN, INPUT);
pinMode(FRONT_BUMPER, INPUT);
pinMode(DIRECTION_SENSOR, INPUT);
//grouding extra pins
pinMode(UNUSED_PIN2, OUTPUT);
digitalWrite(UNUSED_PIN2, LOW);
pinMode(UNUSED_PIN3, OUTPUT);
digitalWrite(UNUSED_PIN3, LOW);
pinMode(MOTOR_LEFT_PWR, OUTPUT);
pinMode(MOTOR_LEFT_DIR, OUTPUT);
pinMode(MOTOR_RIGHT_PWR, OUTPUT);
pinMode(MOTOR_RIGHT_DIR, OUTPUT);
interrupts();
attachInterrupt(IR_IN - 2, ReadBeacon, RISING);
attachInterrupt(FRONT_BUMPER - 2, toggleBump, CHANGE);
TMRArd_InitTimer(TAPE_TIMER, TAPE_INTERVAL);
TMRArd_InitTimer(IR_TIMER, IR_INTERVAL);
TMRArd_InitTimer(CONFUSED_TIMER, CONFUSED_INTERVAL);
TMRArd_InitTimer(STUCK_TIMER, 1);
}
/**************** Brick State Diagram *******************************/
void loop() { // loop() function required for Arduino
// read/update sensors
checkIR();
checkTape();
char opponentDir = getDirectionToSignal(opponentLocation);
char wallDir = getDirectionToSignal(wallLocation);
switch (currentState) {
//-------- searching -------------//
case searching:
// checkIfConfused();
if (opponentDir == LEFT_TURN || opponentDir == RIGHT_TURN) {
// Serial.print("spin to the ");
// Serial.println(opponentDir);
spin(opponentDir, SPIN_SPEED);
TMRArd_InitTimer(CONFUSED_TIMER, CONFUSED_INTERVAL);
currentState = spinning;
}
else if(opponentDir == IN_FRONT) {
// Serial.println("attack!");
allAheadFull();
currentState = attacking;
} // else Serial.println(opponentDir);
break;
//-----------------------------------------------------------//
//-------- random beserk mode! -------------//
case beserk:
// enter beserkMode by resetting opponentLocation and moving foward
// If a IR signal is found break out of beserk mode
if (opponentLocation < ERROR_SIGNAL) {
// spin toward signal
spin(opponentDir, SPIN_SPEED);
currentState = spinning;
// Serial.println("Opponent found... ATTACK");
break;
}
else if (beingBumped) {
allAheadFull();
currentState = attacking;
opponentLocation = NORTH;
// Serial.println("bump... ATTACK");
break;
}
else if (getGameTime() > 45) {
// do whatever it takes to hide ( go to wall)
spin(wallDir, SPIN_SPEED);
currentState = lookingForWall;
// Serial.println("too late in game - go hide");
break;
}
// state machine for beserk mode
switch (beserkState) {
case turning:
// if done turning, move fwd
if (TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(BACK_TIMER);
allAheadFull();
beserkState = movingFwd;
// Serial.println("beserk forwards");
}
break;
case backingUp:
// if done backing up or tape found, turn
if ((TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED) ||
(tapeSensed())) {
TMRArd_ClearTimerExpired(BACK_TIMER);
spin(LEFT_TURN, SPIN_SPEED);
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
beserkState = turning;
// Serial.println("beserk turn");
}
break;
case movingFwd:
// drive forward until signal or tape found, then react accordingly
if (tapeSensed()) {
fullStop();
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
moveBackwards(BACK_SPEED);
beserkState = backingUp;
// Serial.println("beserk backwards");
}
break;
}
break;
//-----------------------------------------------------------//
//-------- attacking -------------//
case attacking:
checkTape();
if (opponentDir != IN_FRONT) {
// Serial.println("off target!!!!! -> SPIN");
spin(opponentDir, SPIN_SPEED);
currentState = spinning;
}
if (beingBumped && (TMRArd_IsTimerExpired(DIRECTION_TIMER) == TMRArd_EXPIRED) &&
(getDirection() == BACKWARDS)) {
turnBack(wallDir, FASTEST);
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
currentState = escaping;
}
if (tapeSensed() && TMRArd_IsTimerExpired(STUCK_TIMER)) {
if (currentTape == FL_TAPE || currentTape == FR_TAPE) {
// Serial.println(" AH TAPE, hit the brakes!!!");
fullStop();
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
moveBackwards(BACK_SPEED);
currentState = avoiding;
// Serial.println("start backing up.");
}
else if (currentTape == BL_TAPE || currentTape == BR_TAPE) {
// Serial.println(" AH TAPE, EVADdeeeeee!!!");
spin(LEFT_TURN, FASTEST);
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
currentState = avoiding;
}
if (getGameTime() > OFFENSE_TIME) {
// do whatever it takes to hide ( go to wall)
spin(wallDir, SPIN_SPEED);
currentState = lookingForWall;
// Serial.println("too late in game - go hide");
break;
}
}
break;
//-----------------------------------------------------------//
//-------- spinning -------------//
case spinning:
if (tapeSensed()) TMRArd_InitTimer(STUCK_TIMER, STUCK_INTERVAL);
if (opponentDir == IN_FRONT || beingBumped) {
// Serial.println("opponent found dead ahead!");
allAheadFull();
currentState = attacking;
}
break;
//-----------------------------------------------------------//
//-------- avoiding -------------//
case avoiding:
// Serial.println("avoiding"); //println
if (TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(BACK_TIMER);
spin(opponentDir, SPIN_SPEED);
// TMRArd_InitTimer(STUCK_TIMER, STUCK_INTERVAL);
currentState = spinning;
// Serial.println("made space, spin away!");
}
break;
//-----------------------------------------------------------//
//-------- escaping -------------//
case escaping:
if (tapeSensed() || (TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED)) {
fullStop();
allAheadFull();
TMRArd_InitTimer(STUCK_TIMER, STUCK_INTERVAL);
currentState = attacking;
}
break;
//-----------------------------------------------------------//
//-------- lookingForWall -------------//
case lookingForWall:
if (wallDir == IN_FRONT) {
// Serial.println("wall's ahead!");
allAheadFull();
currentState = hiding;
}
break;
//-----------------------------------------------------------//
//-------- hiding -------------//
case hiding:
if (wallDir == IN_FRONT && beingBumped) {
fullStop();
}
else {
// Serial.println("lost contact with wall - ahead!");
spin(wallDir, SPIN_SPEED);
currentState = lookingForWall;
}
break;
//-----------------------------------------------------------//
// default:
// break;
// Serial.println("you should not be here");
}
}
// returns game time elapsed in seconds
unsigned long getGameTime() {
return (millis()/1000);
}
void checkIfConfused() {
if (TMRArd_IsTimerExpired(CONFUSED_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(CONFUSED_TIMER);
spin(LEFT_TURN, SPIN_SPEED);
TMRArd_InitTimer(CONFUSED_SPIN_TIMER, CONFUSED_SPIN_INTERVAL);
} else if (TMRArd_IsTimerExpired(CONFUSED_SPIN_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(CONFUSED_SPIN_TIMER);
fullStop();
TMRArd_InitTimer(CONFUSED_TIMER, CONFUSED_INTERVAL);
}
}
/**************** IR Methods *******************************/
void checkIR() {
if (TMRArd_IsTimerExpired(IR_TIMER) == TMRArd_EXPIRED) {
//noInterrupts();
int freq = 1000000/period;
// Serial.println(freq); // println
if ((LOW_4000_THRESHOLD < freq) && (freq < HIGH_4000_THRESHOLD)) {
wallLocation = (currentIR);
// Serial.print("IR sensor reading from : ");
// Serial.println(opponentLocation);
}
else if ((LOW_850_THRESHOLD < freq) && (freq < HIGH_850_THRESHOLD)) {
opponentLocation = currentIR;//(opponentLocation | currentIR);
// Serial.print("IR sensor reading from : ");
// Serial.println(opponentLocation);
}
else {
NextIR();
}
period = 100000000;
TMRArd_ClearTimerExpired(IR_TIMER);
TMRArd_InitTimer(IR_TIMER, IR_INTERVAL);
}
}
/* getDirectionToSignal - does xyz */
unsigned char getDirectionToSignal(int signal) {
switch (signal) {
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
break;
}
}
void NextIR() {
switch (currentIR) {
case NORTH_EAST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = NORTH;
break;
case EAST:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = NORTH_EAST;
break;
case SOUTH_EAST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = EAST;
break;
case SOUTH:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = SOUTH_EAST;
break;
case SOUTH_WEST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = SOUTH;
break;
case WEST:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = SOUTH_WEST;
break;
case NORTH_WEST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = WEST;
break;
case NORTH:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = NORTH_WEST;
break;
default:
Serial.println("direction error");
}
}
/**************** Interrupts *******************************/
void ReadBeacon() {
curTime = micros();
period = curTime - lastTime;
lastTime = curTime;
}
void toggleBump() {
if (digitalRead(FRONT_BUMPER) == HIGH) {
beingBumped = true;
TMRArd_InitTimer(DIRECTION_TIMER, DIRECTION_INTERVAL);
}
else beingBumped = false;
}
unsigned char getDirection() {
return digitalRead(DIRECTION_SENSOR);
}
/**************** Tape Sensor *******************************/
void checkTape() {
if (TMRArd_IsTimerExpired(TAPE_TIMER) == TMRArd_EXPIRED) {
currentTape++;
if (currentTape > NUM_TAPES) currentTape = FIRST_TAPE;
NextTape(currentTape);
// Serial.println(currentTape); // println
TMRArd_ClearTimerExpired(TAPE_TIMER);
TMRArd_InitTimer(TAPE_TIMER, TAPE_INTERVAL);
}
else if (tapeSensed()) {
// Serial.print("Tape sensed here: "); // println
// Serial.println(currentTape); // println
}
}
void NextTape(unsigned char next) {
switch(next) {
case FL_TAPE:
digitalWrite(TAPE_PIN_OUT1, LOW);
digitalWrite(TAPE_PIN_OUT2, LOW);
break;
case FR_TAPE:
digitalWrite(TAPE_PIN_OUT1, HIGH);
digitalWrite(TAPE_PIN_OUT2, LOW);
break;
case BL_TAPE:
digitalWrite(TAPE_PIN_OUT1, LOW);
digitalWrite(TAPE_PIN_OUT2, HIGH);
break;
case BR_TAPE:
digitalWrite(TAPE_PIN_OUT1, HIGH);
digitalWrite(TAPE_PIN_OUT2, HIGH);
break;
default:
currentTape = FL_TAPE;
break;
}
}
unsigned char tapeSensed() {
if (digitalRead(TAPE_PIN_IN) == LOW) return true;
return false;
}
/**************** Motor Methods *******************************/
void spin(signed char dir, unsigned char velocity) { // in place
// dir > 0 -> left, dir < 0 -> right
if (dir > 0) { // spin right
digitalWrite(MOTOR_LEFT_DIR, opposite(LEFT_FWD));
digitalWrite(MOTOR_RIGHT_DIR, RIGHT_FWD);
// Serial.println("spin right?");
}
else if (dir < 0) {// spin left
digitalWrite(MOTOR_LEFT_DIR, LEFT_FWD);
digitalWrite(MOTOR_RIGHT_DIR, opposite(RIGHT_FWD));
// Serial.println("spin left?");
}
else { // don't spin
// Serial.println("error");
return;
}
analogWrite(MOTOR_LEFT_PWR, velocity);
analogWrite(MOTOR_RIGHT_PWR, velocity);
}
void turnBack(signed char dir, unsigned char velocity) { // with fwd movemen
// dir > 0 -> left, dir < 0 -> right
digitalWrite(MOTOR_LEFT_DIR, opposite(LEFT_FWD));
digitalWrite(MOTOR_RIGHT_DIR, opposite(RIGHT_FWD));
if (dir > 0) { // spin to face right
analogWrite(MOTOR_LEFT_PWR, TURN_SPEED);
analogWrite(MOTOR_RIGHT_PWR, velocity);
}
else if (dir < 0) {// spin to face left
analogWrite(MOTOR_LEFT_PWR, velocity);
analogWrite(MOTOR_RIGHT_PWR, TURN_SPEED);
}
else { // don't spin
// Serial.println("error");
return;
}
}
void fullStop() {
// Serial.println("stop");
digitalWrite(MOTOR_LEFT_DIR, LEFT_FWD);
analogWrite(MOTOR_LEFT_PWR, 0);
digitalWrite(MOTOR_RIGHT_DIR, RIGHT_FWD);
analogWrite(MOTOR_RIGHT_PWR, 0);
}
void allAheadFull() {
// Serial.println("go straight");
digitalWrite(MOTOR_LEFT_DIR, LEFT_FWD);
analogWrite(MOTOR_LEFT_PWR, FASTEST);
digitalWrite(MOTOR_RIGHT_DIR, RIGHT_FWD);
analogWrite(MOTOR_RIGHT_PWR, FASTEST);
}
void moveBackwards(unsigned char velocity) {
// Serial.println("go backwards");
digitalWrite(MOTOR_LEFT_DIR, opposite(LEFT_FWD));
analogWrite(MOTOR_LEFT_PWR, velocity);
digitalWrite(MOTOR_RIGHT_DIR, opposite(RIGHT_FWD));
analogWrite(MOTOR_RIGHT_PWR, velocity);
}
unsigned char opposite(int input) {
if (input >= 1) return LOW;
else if (input <= 0) return HIGH;
return 0;
}
#include <Timers.h>
// Module Defines -----------------------------------------
#define FIRST_TAPE 1
#define FL_TAPE 1
#define FR_TAPE 2
#define BL_TAPE 3
#define BR_TAPE 4
#define NUM_TAPES 4
#define TAPE_ERROR 5
#define NORTH 0
#define NORTH_WEST 1
#define WEST 2
#define SOUTH_WEST 3
#define SOUTH 4
#define SOUTH_EAST 5
#define EAST 6
#define NORTH_EAST 7
#define ERROR_SIGNAL 8
// frequency constants
#define LOW_4000_THRESHOLD 3900
#define HIGH_4000_THRESHOLD 4000
#define LOW_850_THRESHOLD 700
#define HIGH_850_THRESHOLD 1000
// Arduino pins
#define TAPE_PIN_OUT1 8 // red
#define TAPE_PIN_OUT2 9 // white
#define TAPE_PIN_IN 4 // white/red
#define MOTOR_RIGHT_PWR 10 // black ~
#define MOTOR_LEFT_PWR 11 // green ~
#define MOTOR_LEFT_DIR 12 // red
#define MOTOR_RIGHT_DIR 13 // yellow -> then white to ground
#define IR_PIN_OUT1 5 // black
#define IR_PIN_OUT2 6 // white
#define IR_PIN_OUT3 7 // red
#define IR_IN 2 // interrupt - white
#define FRONT_BUMPER 3 // red
#define DIRECTION_SENSOR A0 // white
#define UNUSED_PIN2 1
#define UNUSED_PIN3 0
// timers
#define BACK_TIMER 0
#define BACK_INTERVAL 2500
#define IR_TIMER 1
#define IR_INTERVAL 10
#define TAPE_TIMER 2
#define TAPE_INTERVAL 20
#define CONFUSED_TIMER 3
#define CONFUSED_INTERVAL 10000
#define STUCK_TIMER 4
#define STUCK_INTERVAL 2000
#define DIRECTION_TIMER 5
#define DIRECTION_INTERVAL 1500
#define CONFUSED_SPIN_TIMER 6
#define CONFUSED_SPIN_INTERVAL 3000
#define OFFENSE_TIME 200 // in seconds
// motor constants
#define LEFT_FWD LOW
#define RIGHT_FWD HIGH
#define IN_FRONT 0
#define LEFT_TURN -1
#define RIGHT_TURN 1
#define FASTEST 255
#define SPIN_SPEED 180
#define BACK_SPEED 120
#define FORWARDS 0
#define BACKWARDS 1
#define TURN_SPEED 0
typedef enum states {
searching,
attacking,
avoiding,
spinning,
beserk,
lookingForWall,
hiding,
escaping
}
states;
typedef enum beserkStates {
turning,
backingUp,
movingFwd
}
beserkStates;
// Module Function Prototypes
// Module Variables
static unsigned char currentTape = FIRST_TAPE;
static unsigned char currentIR = NORTH;
static volatile unsigned long curTime;
static volatile unsigned long lastTime = 0;
static volatile unsigned long period = 0;
static signed int opponentLocation = ERROR_SIGNAL;
static signed int wallLocation = ERROR_SIGNAL;
static states currentState = searching;
static beserkStates beserkState = movingFwd;
static unsigned char beingBumped = 0;
// Arduino Main Functions
void setup() { // setup() function required for Arduino
Serial.begin(9600);
Serial.println("Yeah Brah, lez do this!");
pinMode(TAPE_PIN_OUT1, OUTPUT);
pinMode(TAPE_PIN_OUT2, OUTPUT);
pinMode(TAPE_PIN_IN, INPUT);
pinMode(IR_PIN_OUT1, OUTPUT);
pinMode(IR_PIN_OUT2, OUTPUT);
pinMode(IR_PIN_OUT3, OUTPUT);
pinMode(IR_IN, INPUT);
pinMode(FRONT_BUMPER, INPUT);
pinMode(DIRECTION_SENSOR, INPUT);
//grouding extra pins
pinMode(UNUSED_PIN2, OUTPUT);
digitalWrite(UNUSED_PIN2, LOW);
pinMode(UNUSED_PIN3, OUTPUT);
digitalWrite(UNUSED_PIN3, LOW);
pinMode(MOTOR_LEFT_PWR, OUTPUT);
pinMode(MOTOR_LEFT_DIR, OUTPUT);
pinMode(MOTOR_RIGHT_PWR, OUTPUT);
pinMode(MOTOR_RIGHT_DIR, OUTPUT);
interrupts();
attachInterrupt(IR_IN - 2, ReadBeacon, RISING);
attachInterrupt(FRONT_BUMPER - 2, toggleBump, CHANGE);
TMRArd_InitTimer(TAPE_TIMER, TAPE_INTERVAL);
TMRArd_InitTimer(IR_TIMER, IR_INTERVAL);
TMRArd_InitTimer(CONFUSED_TIMER, CONFUSED_INTERVAL);
TMRArd_InitTimer(STUCK_TIMER, 1);
}
/**************** Brick State Diagram *******************************/
void loop() { // loop() function required for Arduino
// read/update sensors
checkIR();
checkTape();
char opponentDir = getDirectionToSignal(opponentLocation);
char wallDir = getDirectionToSignal(wallLocation);
switch (currentState) {
//-------- searching -------------//
case searching:
// checkIfConfused();
if (opponentDir == LEFT_TURN || opponentDir == RIGHT_TURN) {
// Serial.print("spin to the ");
// Serial.println(opponentDir);
spin(opponentDir, SPIN_SPEED);
TMRArd_InitTimer(CONFUSED_TIMER, CONFUSED_INTERVAL);
currentState = spinning;
}
else if(opponentDir == IN_FRONT) {
// Serial.println("attack!");
allAheadFull();
currentState = attacking;
} // else Serial.println(opponentDir);
break;
//-----------------------------------------------------------//
//-------- random beserk mode! -------------//
case beserk:
// enter beserkMode by resetting opponentLocation and moving foward
// If a IR signal is found break out of beserk mode
if (opponentLocation < ERROR_SIGNAL) {
// spin toward signal
spin(opponentDir, SPIN_SPEED);
currentState = spinning;
// Serial.println("Opponent found... ATTACK");
break;
}
else if (beingBumped) {
allAheadFull();
currentState = attacking;
opponentLocation = NORTH;
// Serial.println("bump... ATTACK");
break;
}
else if (getGameTime() > 45) {
// do whatever it takes to hide ( go to wall)
spin(wallDir, SPIN_SPEED);
currentState = lookingForWall;
// Serial.println("too late in game - go hide");
break;
}
// state machine for beserk mode
switch (beserkState) {
case turning:
// if done turning, move fwd
if (TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(BACK_TIMER);
allAheadFull();
beserkState = movingFwd;
// Serial.println("beserk forwards");
}
break;
case backingUp:
// if done backing up or tape found, turn
if ((TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED) ||
(tapeSensed())) {
TMRArd_ClearTimerExpired(BACK_TIMER);
spin(LEFT_TURN, SPIN_SPEED);
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
beserkState = turning;
// Serial.println("beserk turn");
}
break;
case movingFwd:
// drive forward until signal or tape found, then react accordingly
if (tapeSensed()) {
fullStop();
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
moveBackwards(BACK_SPEED);
beserkState = backingUp;
// Serial.println("beserk backwards");
}
break;
}
break;
//-----------------------------------------------------------//
//-------- attacking -------------//
case attacking:
checkTape();
if (opponentDir != IN_FRONT) {
// Serial.println("off target!!!!! -> SPIN");
spin(opponentDir, SPIN_SPEED);
currentState = spinning;
}
if (beingBumped && (TMRArd_IsTimerExpired(DIRECTION_TIMER) == TMRArd_EXPIRED) &&
(getDirection() == BACKWARDS)) {
turnBack(wallDir, FASTEST);
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
currentState = escaping;
}
if (tapeSensed() && TMRArd_IsTimerExpired(STUCK_TIMER)) {
if (currentTape == FL_TAPE || currentTape == FR_TAPE) {
// Serial.println(" AH TAPE, hit the brakes!!!");
fullStop();
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
moveBackwards(BACK_SPEED);
currentState = avoiding;
// Serial.println("start backing up.");
}
else if (currentTape == BL_TAPE || currentTape == BR_TAPE) {
// Serial.println(" AH TAPE, EVADdeeeeee!!!");
spin(LEFT_TURN, FASTEST);
TMRArd_InitTimer(BACK_TIMER, BACK_INTERVAL);
currentState = avoiding;
}
if (getGameTime() > OFFENSE_TIME) {
// do whatever it takes to hide ( go to wall)
spin(wallDir, SPIN_SPEED);
currentState = lookingForWall;
// Serial.println("too late in game - go hide");
break;
}
}
break;
//-----------------------------------------------------------//
//-------- spinning -------------//
case spinning:
if (tapeSensed()) TMRArd_InitTimer(STUCK_TIMER, STUCK_INTERVAL);
if (opponentDir == IN_FRONT || beingBumped) {
// Serial.println("opponent found dead ahead!");
allAheadFull();
currentState = attacking;
}
break;
//-----------------------------------------------------------//
//-------- avoiding -------------//
case avoiding:
// Serial.println("avoiding"); //println
if (TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(BACK_TIMER);
spin(opponentDir, SPIN_SPEED);
// TMRArd_InitTimer(STUCK_TIMER, STUCK_INTERVAL);
currentState = spinning;
// Serial.println("made space, spin away!");
}
break;
//-----------------------------------------------------------//
//-------- escaping -------------//
case escaping:
if (tapeSensed() || (TMRArd_IsTimerExpired(BACK_TIMER) == TMRArd_EXPIRED)) {
fullStop();
allAheadFull();
TMRArd_InitTimer(STUCK_TIMER, STUCK_INTERVAL);
currentState = attacking;
}
break;
//-----------------------------------------------------------//
//-------- lookingForWall -------------//
case lookingForWall:
if (wallDir == IN_FRONT) {
// Serial.println("wall's ahead!");
allAheadFull();
currentState = hiding;
}
break;
//-----------------------------------------------------------//
//-------- hiding -------------//
case hiding:
if (wallDir == IN_FRONT && beingBumped) {
fullStop();
}
else {
// Serial.println("lost contact with wall - ahead!");
spin(wallDir, SPIN_SPEED);
currentState = lookingForWall;
}
break;
//-----------------------------------------------------------//
// default:
// break;
// Serial.println("you should not be here");
}
}
// returns game time elapsed in seconds
unsigned long getGameTime() {
return (millis()/1000);
}
void checkIfConfused() {
if (TMRArd_IsTimerExpired(CONFUSED_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(CONFUSED_TIMER);
spin(LEFT_TURN, SPIN_SPEED);
TMRArd_InitTimer(CONFUSED_SPIN_TIMER, CONFUSED_SPIN_INTERVAL);
} else if (TMRArd_IsTimerExpired(CONFUSED_SPIN_TIMER) == TMRArd_EXPIRED) {
TMRArd_ClearTimerExpired(CONFUSED_SPIN_TIMER);
fullStop();
TMRArd_InitTimer(CONFUSED_TIMER, CONFUSED_INTERVAL);
}
}
/**************** IR Methods *******************************/
void checkIR() {
if (TMRArd_IsTimerExpired(IR_TIMER) == TMRArd_EXPIRED) {
//noInterrupts();
int freq = 1000000/period;
// Serial.println(freq); // println
if ((LOW_4000_THRESHOLD < freq) && (freq < HIGH_4000_THRESHOLD)) {
wallLocation = (currentIR);
// Serial.print("IR sensor reading from : ");
// Serial.println(opponentLocation);
}
else if ((LOW_850_THRESHOLD < freq) && (freq < HIGH_850_THRESHOLD)) {
opponentLocation = currentIR;//(opponentLocation | currentIR);
// Serial.print("IR sensor reading from : ");
// Serial.println(opponentLocation);
}
else {
NextIR();
}
period = 100000000;
TMRArd_ClearTimerExpired(IR_TIMER);
TMRArd_InitTimer(IR_TIMER, IR_INTERVAL);
}
}
/* getDirectionToSignal - does xyz */
unsigned char getDirectionToSignal(int signal) {
switch (signal) {
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
break;
}
}
void NextIR() {
switch (currentIR) {
case NORTH_EAST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = NORTH;
break;
case EAST:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = NORTH_EAST;
break;
case SOUTH_EAST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = EAST;
break;
case SOUTH:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = SOUTH_EAST;
break;
case SOUTH_WEST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, HIGH);
currentIR = SOUTH;
break;
case WEST:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = SOUTH_WEST;
break;
case NORTH_WEST:
digitalWrite(IR_PIN_OUT1, LOW);
digitalWrite(IR_PIN_OUT2, HIGH);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = WEST;
break;
case NORTH:
digitalWrite(IR_PIN_OUT1, HIGH);
digitalWrite(IR_PIN_OUT2, LOW);
digitalWrite(IR_PIN_OUT3, LOW);
currentIR = NORTH_WEST;
break;
default:
Serial.println("direction error");
}
}
/**************** Interrupts *******************************/
void ReadBeacon() {
curTime = micros();
period = curTime - lastTime;
lastTime = curTime;
}
void toggleBump() {
if (digitalRead(FRONT_BUMPER) == HIGH) {
beingBumped = true;
TMRArd_InitTimer(DIRECTION_TIMER, DIRECTION_INTERVAL);
}
else beingBumped = false;
}
unsigned char getDirection() {
return digitalRead(DIRECTION_SENSOR);
}
/**************** Tape Sensor *******************************/
void checkTape() {
if (TMRArd_IsTimerExpired(TAPE_TIMER) == TMRArd_EXPIRED) {
currentTape++;
if (currentTape > NUM_TAPES) currentTape = FIRST_TAPE;
NextTape(currentTape);
// Serial.println(currentTape); // println
TMRArd_ClearTimerExpired(TAPE_TIMER);
TMRArd_InitTimer(TAPE_TIMER, TAPE_INTERVAL);
}
else if (tapeSensed()) {
// Serial.print("Tape sensed here: "); // println
// Serial.println(currentTape); // println
}
}
void NextTape(unsigned char next) {
switch(next) {
case FL_TAPE:
digitalWrite(TAPE_PIN_OUT1, LOW);
digitalWrite(TAPE_PIN_OUT2, LOW);
break;
case FR_TAPE:
digitalWrite(TAPE_PIN_OUT1, HIGH);
digitalWrite(TAPE_PIN_OUT2, LOW);
break;
case BL_TAPE:
digitalWrite(TAPE_PIN_OUT1, LOW);
digitalWrite(TAPE_PIN_OUT2, HIGH);
break;
case BR_TAPE:
digitalWrite(TAPE_PIN_OUT1, HIGH);
digitalWrite(TAPE_PIN_OUT2, HIGH);
break;
default:
currentTape = FL_TAPE;
break;
}
}
unsigned char tapeSensed() {
if (digitalRead(TAPE_PIN_IN) == LOW) return true;
return false;
}
/**************** Motor Methods *******************************/
void spin(signed char dir, unsigned char velocity) { // in place
// dir > 0 -> left, dir < 0 -> right
if (dir > 0) { // spin right
digitalWrite(MOTOR_LEFT_DIR, opposite(LEFT_FWD));
digitalWrite(MOTOR_RIGHT_DIR, RIGHT_FWD);
// Serial.println("spin right?");
}
else if (dir < 0) {// spin left
digitalWrite(MOTOR_LEFT_DIR, LEFT_FWD);
digitalWrite(MOTOR_RIGHT_DIR, opposite(RIGHT_FWD));
// Serial.println("spin left?");
}
else { // don't spin
// Serial.println("error");
return;
}
analogWrite(MOTOR_LEFT_PWR, velocity);
analogWrite(MOTOR_RIGHT_PWR, velocity);
}
void turnBack(signed char dir, unsigned char velocity) { // with fwd movemen
// dir > 0 -> left, dir < 0 -> right
digitalWrite(MOTOR_LEFT_DIR, opposite(LEFT_FWD));
digitalWrite(MOTOR_RIGHT_DIR, opposite(RIGHT_FWD));
if (dir > 0) { // spin to face right
analogWrite(MOTOR_LEFT_PWR, TURN_SPEED);
analogWrite(MOTOR_RIGHT_PWR, velocity);
}
else if (dir < 0) {// spin to face left
analogWrite(MOTOR_LEFT_PWR, velocity);
analogWrite(MOTOR_RIGHT_PWR, TURN_SPEED);
}
else { // don't spin
// Serial.println("error");
return;
}
}
void fullStop() {
// Serial.println("stop");
digitalWrite(MOTOR_LEFT_DIR, LEFT_FWD);
analogWrite(MOTOR_LEFT_PWR, 0);
digitalWrite(MOTOR_RIGHT_DIR, RIGHT_FWD);
analogWrite(MOTOR_RIGHT_PWR, 0);
}
void allAheadFull() {
// Serial.println("go straight");
digitalWrite(MOTOR_LEFT_DIR, LEFT_FWD);
analogWrite(MOTOR_LEFT_PWR, FASTEST);
digitalWrite(MOTOR_RIGHT_DIR, RIGHT_FWD);
analogWrite(MOTOR_RIGHT_PWR, FASTEST);
}
void moveBackwards(unsigned char velocity) {
// Serial.println("go backwards");
digitalWrite(MOTOR_LEFT_DIR, opposite(LEFT_FWD));
analogWrite(MOTOR_LEFT_PWR, velocity);
digitalWrite(MOTOR_RIGHT_DIR, opposite(RIGHT_FWD));
analogWrite(MOTOR_RIGHT_PWR, velocity);
}
unsigned char opposite(int input) {
if (input >= 1) return LOW;
else if (input <= 0) return HIGH;
return 0;
}