Finite State Diagram*
*an updated version will be uploaded very soon.
Code version used during competition
/**************************************************************
* File: Sumobot_v9.ino
**************************************************************/
/*---------------- Includes ---------------------------------*/
#include <Timers.h>;
#include <stdbool.h>;
/*---------------- Module Defines ---------------------------*/
//states of the Sumobot:
#define SEARCHFWD 0 //explore (move forward&right)
#define SEARCHSPIN 1 //explore (move forward&right)
#define LEFTTOBOT 2 //forward&left towards opponent
#define RIGHTTOBOT 3 //forward&right towards opponent
#define FWDTOBOT 4 //forward towards opponent
#define BACKLEFT 5 //turn slight left to avoid tape
#define BACKRIGHT 6 //turn slight right to avoid tape
#define OVERRIGHT 7 //go over tape slightly
#define OVERLEFT 8 //go over tape slightly
#define SCANWALL 10 //endgame: find wall beacon
#define LEFTTOWALL 11 //endgame: forward&left towards wall
#define RIGHTTOWALL 12 //endgame: forward&right towards wall
#define FWDTOWALL 13 //endgame: forward towards wall
/*---------------- Module Level Variables -------------------*/
//timer settings
static int TimeToEndgame = 100000; //seconds (not for brick)
static int BackupTime = 2000; //milliseconds
static int SpinTime = 2000; //milliseconds
static int FwdSearchTime = 3000; //milliseconds
static int OvershootTime = 100; //milliseconds
static int BackToFieldTime = 500; //milliseconds
static int DisplayCurrentTime = 1000; //milliseconds
static int SpeedTime = 500; //milliseconds
//timer ids
static int EndgameTimer = 0;
static int BackupTimer = 1;
static int FwdSearchTimer = 2;
static int SpinTimer = 3;
static int OvershootTimer = 4;
static int BackToFieldTimer = 5;
static int SpeedTimer = 6;
static int DisplayCurrentTimer = 7;
//motor speeds
static int FwdAttackSpeed = 75; //both motors
static int FwdSearchSpeed = 75; //both motors
static int TurnSpeedFwdHigh = 75; //motors in opposite dirs
static int TurnSpeedFwdLow = 37; //motors in opposite dirs
static int TurnSpeedHigh = 75; //motors in opposite dirs, one faster than the other
static int TurnSpeedLow = 5; //motors in opposite dirs, one slower than the other
static int BackupSpeedHigh = 75; //one motor backs up faster (to turn)
static int BackupSpeedLow = 37; //other motor backs up slower (to turn)
static int ZeroSpeed = 0; //0 speed needed for less blocking turn when one sensor on
static int Acceleration1 = 200; //ramp up both motors
//beacon sensor thresholds
static int limfreq850_lower = 400; //Hz
static int limfreq850_upper = 1800; //Hz
//define pins
int BeaconPin_R850 = 2; //INPUT pin for right 850Hz sensor (interrupt)
int BeaconPin_L850 = 3; //INPUT pin for left 850Hz sensor (interrupt)
int TapePin_Back = 7; //INPUT pin for back tape sensor
int TapePin_ROuter = 8; //INPUT pin for outer right tape sensor
int TapePin_LOuter = 9; //INPUT pin for outer left tape sensor
int MotorPin_RPulse = 10; //OUTPUT pin for right motor speed
int MotorPin_LPulse = 11; //OUTPUT pin for left motor speed
int MotorPin_RDir = 12; //OUTPUT pin for right motor direction
int MotorPin_LDir = 13; //OUTPUT pin for left motor direction
int MotorPin_Rrevs = A0; //INPUT pin for right motor revs sensor
int MotorPin_Lrevs = A1; //INPUT pin for left motor revs sensor//int TapePin_RInner = ; //INPUT pin for inner right tape sensor (not implemented)
//int TapePin_LInner = ; //INPUT pin for inner left tape sensor (not implemented)
//int AccelPin_X = A2; //INPUT pin for X-acceleration sensor (not implemented)
//int AccelPin_Y = A3; //INPUT pin for Y-acceleration sensor (not implemented)
//define interrupt variables
volatile long t0_0 = TMRArd_GetTime();
volatile long t1_0 = 0;
volatile long t0_1 = TMRArd_GetTime();
volatile long t1_1 = 0;
volatile long deltaT0 = 0;
volatile long deltaT1 = 0;
volatile int c = 0;
volatile int d = 0;
static long cmax = 4;
static long dmax = cmax;
float freq0 = 0;
float freq1 = 0;
static int loopTime = (int) cmax * 1000/850; //milliseconds
static int state;
/*---------------- Module Function Prototypes ---------------*/
unsigned char TestSearchFwdTimer(void);
unsigned char TestSearchSpinTimer(void);
unsigned char TestForKey(void);
unsigned char TestTapeRightOuter(void);
unsigned char TestTapeLeftOuter(void);
unsigned char TestTapeBack(void);
unsigned char TestBackupTimer(void);
unsigned char TestOvershootTimer(void);
unsigned char TestBackToFieldTimer(void);
unsigned char TestBotLeft(void);
unsigned char TestBotRight(void);
unsigned char TestSpeedTimer(void);
unsigned char TestDisplayCurrentTimer(void);
unsigned char TestWallLeft(void);
unsigned char TestWallRight(void);
unsigned char TestEndgameTimer(void);
void RespToSearchFwdTimer(void);
void RespToSearchSpinTimer(void);
void RespToKey(void);
void RespToTapeRightOuter(void);
void RespToTapeLeftOuter(void);
void RespToTapeBack(void);
void RespToBackupTimer(void);
void RespToBotLeft(void);
void RespToBotRight(void);
void RespToBotFwd(void);
void RespToBotGone(void);
void RespToLeftBackupTimer(void);
void RespToRightBackupTimer(void);
void RespToSpeedTimer(void);
void RespToDisplayCurrentTimer(void);
void RespToWallLeft(void);
void RespToWallRight(void);
void RespToWallFwd(void);
void RespToEndgameTimer(void);
//void RespToMotorStall(void) {} // (not implemented)
//void RespToBump(void) {} // (not implemented)
void setDeltaT0(void);
void setDeltaT1(void);
void resetFreq(void);
void setOvershootTimer(void);
/*---------------- Arduino Main Functions -------------------*/
void setup(){
//set baud rate
Serial.begin(9600);
Serial.println("THE SUMOBOT RISES...");
//set pin modes
pinMode(BeaconPin_R850,INPUT);
pinMode(BeaconPin_L850,INPUT);
pinMode(MotorPin_RDir,OUTPUT);
pinMode(MotorPin_RPulse,OUTPUT);
pinMode(MotorPin_LDir,OUTPUT);
pinMode(MotorPin_LPulse,OUTPUT);
pinMode(TapePin_ROuter,INPUT);
pinMode(TapePin_LOuter,INPUT);
pinMode(TapePin_Back,INPUT);
pinMode(MotorPin_Rrevs,INPUT);
pinMode(MotorPin_Lrevs,INPUT);
// pinMode(TapePin_LInner,INPUT);
// pinMode(TapePin_RInner,INPUT);
// pinMode(AccelPin_X,INPUT);
// pinMode(AccelPin_Y,INPUT);
//set game timer
TMRArd_InitTimer(EndgameTimer,TimeToEndgame*1000);
//set current reading interval timer
TMRArd_InitTimer(DisplayCurrentTimer, DisplayCurrentTime);
//set interrupts: attachInterrupt(interrupt, function, mode)
attachInterrupt(0, setDeltaT0, RISING); //Pin2: rising edges
attachInterrupt(1, setDeltaT1, RISING); //Pin3: rising edges
//set initial state
TMRArd_InitTimer(SpinTimer,SpinTime);
state = SEARCHSPIN;
}
void loop() {
if (TestDisplayCurrentTimer()){ RespToDisplayCurrentTimer();}
switch (state) {
case(SEARCHFWD):
// if(TestForKey()) RespToKey();
if(TestSearchFwdTimer()){ Serial.print(state); RespToSearchFwdTimer();
Serial.println(" Timer expired");}
if(TestTapeRightOuter()){ Serial.print(state); RespToTapeRightOuter();
Serial.println(" Tape right detected");}
if(TestTapeLeftOuter()){ Serial.print(state); RespToTapeLeftOuter();
Serial.println(" Tape left detected");}
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
break;
case(SEARCHSPIN):
// if(TestForKey()) RespToKey();
if(TestSearchSpinTimer()){ Serial.print(state); RespToSearchSpinTimer();
Serial.println(" Timer expired");}
if(TestTapeRightOuter()){ Serial.print(state); RespToTapeRightOuter();
Serial.println(" Tape right detected");}
if(TestTapeLeftOuter()){ Serial.print(state); RespToTapeLeftOuter();
Serial.println(" Tape left detected");}
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
if(TestEndgameTimer()){ Serial.print(state); RespToEndgameTimer();
Serial.println(" The end is near");}
break;
case (LEFTTOBOT): //aim for bot
Serial.println(" LEFTTOBOT");
if(TestTapeRightOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape right detected"); state = OVERRIGHT; break;}
if(TestTapeLeftOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape left detected"); state = OVERLEFT; break;}
if(TestBotLeft()&&TestBotRight()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if((!TestBotRight())&&(!TestBotLeft())){ Serial.print(state); RespToBotGone();
Serial.println(" No bot detected"); resetFreq(); break;}
resetFreq();
break;
case (RIGHTTOBOT): //aim for bot
Serial.println(" RIGHTTOBOT");
if(TestTapeRightOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape right detected"); state = OVERRIGHT; break;}
if(TestTapeLeftOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape left detected"); state = OVERLEFT; break;}
if(TestBotLeft()&&TestBotRight()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
if((!TestBotRight())&&(!TestBotLeft())){ Serial.print(state); RespToBotGone();
Serial.println(" No bot detected"); resetFreq(); break;}
resetFreq();
break;
case (FWDTOBOT): //aim for bot
Serial.println(" FWDTOBOT");
if(TestTapeRightOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape right detected"); state = OVERRIGHT; break;}
if(TestTapeLeftOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape left detected"); state = OVERLEFT; break;}
if(TestSpeedTimer()){ Serial.print(state); RespToSpeedTimer();
Serial.print("Speed Timer expired: ramp up");}
if(TestBotRight()&&(!TestBotLeft())){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()&&(!TestBotRight())){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
if((!TestBotRight())&&(!TestBotLeft())){ Serial.print(state); RespToBotGone();
Serial.println(" No bot detected"); resetFreq(); break;}
resetFreq();
break;
case (BACKRIGHT): //avoid tape
Serial.println(" BACKRIGHT");
if(TestBackupTimer()){ Serial.print(state); RespToBackupTimer();
Serial.println(" Timer expired"); }
if(TestTapeBack()){ Serial.print(state); RespToTapeBack();
Serial.println(" Tape back detected"); }
if(TestBackToFieldTimer()){ //don't go for bot when backing up after overshooting tape
Serial.println(TestBackToFieldTimer());
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
}
break;
case (BACKLEFT): //avoid tape
Serial.println(" BACKLEFT");
if(TestBackupTimer()){ Serial.print(state); RespToBackupTimer();
Serial.println(" Timer expired"); }
if(TestTapeBack()){ Serial.print(state); RespToTapeBack();
Serial.println(" Tape back detected"); }
if(TestBackToFieldTimer()){ //don't go for bot when backing up after overshooting tape
Serial.println(TestBackToFieldTimer());
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
}
break;
case (OVERLEFT): //aim for bot
Serial.println(" TAPE LEFT: OVERSHOOTING");
if(TestOvershootTimer()){ Serial.print(state); RespToLeftBackupTimer();
Serial.println(" Done overshooting, back up"); resetFreq(); break;}
Serial.println(TestOvershootTimer();
resetFreq();
break;
case (OVERRIGHT): //aim for bot
Serial.println(" TAPE RIGHT: OVERSHOOTING");
if(TestOvershootTimer()){ Serial.print(state); RespToRightBackupTimer();
Serial.println(" Done overshooting, back up"); resetFreq(); break;}
Serial.println(TestOvershootTimer());
resetFreq();
break;
// case(SCANWALL): //endgame: find wall
// if(TestWallLeft()&TestWallRight()) RespToWallFwd();
// if(TestWallRight()) RespToWallRight();
// if(TestWallLeft()) RespToWallLeft();}
// break;
// case (LEFTOWALL): //endgame: aim for wall
// if(TestWallLeft()&TestWallRight()) RespToWallFwd();
// if(TestWallRight()) RespToWallRight();
// if(!TestWallLeft()&!TestWallRight()) RespToWallGone();}
// break;
// case (RIGHTTOWALL): //endgame: aim for wall
// if(TestWallLeft()&TestWallRight()) RespToWallFwd();
// if(TestWallLeft()) RespToWallLeft();
// if(!TestWallLeft()&!TestWallRight()) RespToWallGone();}
// break;
// case (FWDTOWALL): //endgame: aim for wall
// if(TestWallLeft()&!TestWallRight()) RespToWallLeft();
// if(TestWallRight()&!TestWallLeft()) RespToWallRight();
// if(!TestWallLeft()&!TestWallRight()) RespToWallGone();}
// break;
}
}
//interrupt 0 (Pin2) function
void setDeltaT0(void){
c++;
if(c==cmax){
t1_0=TMRArd_GetTime();
deltaT0 = t1_0-t0_0;
t0_0=t1_0;
c=0;
}
}
//interrupt 1 (Pin3) function
void setDeltaT1(void){
d++;
if(d==dmax){
t1_1=TMRArd_GetTime();
deltaT1 = t1_1-t0_1;
t0_1=t1_1;
d=0;
}
}
/*---------------- Module Functions -------------------------*/
/****RESET INTERRUPT FREQUENCY****/
void resetFreq(void){
deltaT0 = 0;
deltaT1 = 0;
delay(loopTime);
}
/****SEARCHING****/
unsigned char TestSearchFwdTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(FwdSearchTimer);
}
unsigned char TestSearchSpinTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(SpinTimer);
}
void RespToSearchFwdTimer(void) { //start spinning
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, TurnSpeedHigh);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, TurnSpeedLow);
//start spinning timer
TMRArd_InitTimer(SpinTimer,SpinTime);
state = SEARCHSPIN;
}
void RespToSearchSpinTimer(void) { //start forward
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
/**** USER INPUT****/
unsigned char TestForKey(void) {
unsigned char KeyEventOccurred;
KeyEventOccurred = Serial.available();
return KeyEventOccurred;
}
void RespToKey(void) {
unsigned char theKey;
theKey = Serial.read();
Serial.println(theKey);
Serial.print(", ASCII=");
Serial.println(theKey,HEX);
if (theKey == 's') {
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, 0);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, 0);
}
if (theKey == 'w') {
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, 100);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, 100);
}
if (theKey == 'a') {
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, 50);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, 50);
}
if (theKey == 'd') {
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, 50);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, 50);
}
if (theKey == 'x') {
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, 50);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, 50);
}
}
/****OVERSHOOTING TAPE****/
void setOvershootTimer(void){
TMRArd_InitTimer(OvershootTimer,OvershootTime);
}
unsigned char TestOvershootTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(OvershootTimer);
}
unsigned char TestBackToFieldTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(BackToFieldTimer);
}
void RespToLeftBackupTimer(void){
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedHigh);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedLow);
//start backup-without-responding-to-beacons timer after pushing mode
TMRArd_InitTimer(BackupTimer,BackupTime);
TMRArd_InitTimer(BackToFieldTimer,BackToFieldTime);
state = BACKLEFT;
}
void RespToRightBackupTimer(void){
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedLow);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedHigh);
//start backup-without-responding-to-beacons timer after pushing mode
TMRArd_InitTimer(BackupTimer,BackupTime);
TMRArd_InitTimer(BackToFieldTimer,BackToFieldTime);
state = BACKRIGHT;
}
/****AVOIDING TAPE****/
unsigned char TestTapeRightOuter(void){
unsigned char EventOccurred;
static unsigned int TapeLevelROuter = 0;
//read tape sensor: LOW when seeing tape
TapeLevelROuter = digitalRead(TapePin_ROuter);
EventOccurred = TapeLevelROuter<1; //1 if tape sensor is LOW
return EventOccurred;
};
unsigned char TestTapeLeftOuter(void){
unsigned char EventOccurred;
static unsigned int TapeLevelLOuter = 0;
//read tape sensor: LOW when seeing tape
TapeLevelLOuter = digitalRead(TapePin_LOuter);
EventOccurred = TapeLevelLOuter<1; //1 if tape sensor is LOW
return EventOccurred;
};
unsigned char TestTapeBack(void){
unsigned char EventOccurred;
static unsigned int TapeLevelBack = 0;
//read tape sensor: LOW when seeing tape
TapeLevelBack = digitalRead(TapePin_Back);
EventOccurred = TapeLevelBack<1; //1 if tape sensor is LOW
return EventOccurred;
};
unsigned char TestBackupTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(BackupTimer);
}
void RespToTapeRightOuter(void){ //back up while turning right
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedLow);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedHigh);
//start backup timer
TMRArd_InitTimer(BackupTimer,BackupTime);
state=BACKRIGHT;
}
void RespToTapeLeftOuter(void){ //back up while turning left
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedHigh);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedLow);
//start backup timer
TMRArd_InitTimer(BackupTimer,BackupTime);
state=BACKLEFT;
}
void RespToTapeBack(void){ //go forward
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
void RespToBackupTimer(void){ //go forth & find bot
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
/****FINDING BOT****/
unsigned char TestBotRight(void){
unsigned char EventOccurred;
freq0 = 1000*cmax/deltaT0;
if((freq0>limfreq850_lower)&&(freq0<limfreq850_upper)){
EventOccurred = 1;
}
else{ //no 850Hz detected to the right
EventOccurred = 0;
}
return EventOccurred;
}
unsigned char TestBotLeft(void){
unsigned char EventOccurred;
freq1 = 1000*dmax/deltaT1;
if((freq1>limfreq850_lower)&&(freq1<limfreq850_upper)){
EventOccurred = 1;
}
else{ //no 850Hz detected to the left
EventOccurred = 0;
}
return EventOccurred;
}
unsigned char TestSpeedTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(SpeedTimer);
}
unsigned char TestDisplayCurrentTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(DisplayCurrentTimer);
}
void RespToSpeedTimer(void){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, Acceleration1);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, Acceleration1);
}
void RespToBotLeft(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, TurnSpeedFwdLow);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, TurnSpeedFwdHigh);
//switch state
state = LEFTTOBOT;
}
void RespToBotRight(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, TurnSpeedFwdHigh);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, TurnSpeedFwdLow);
//switch state
state = RIGHTTOBOT;
}
void RespToBotFwd(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdAttackSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdAttackSpeed);
//switch state
TMRArd_InitTimer(SpeedTimer,SpeedTime); //set delay before speed ramps up
state = FWDTOBOT;
}
void RespToBotGone(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
void RespToDisplayCurrentTimer(void){
static float ValueMotorL = 0;
static float ValueMotorR = 0;
static float VoltageMotorL = 0;
static float VoltageMotorR = 0;
static float CurrentMotorL = 0;
static float CurrentMotorR = 0;
ValueMotorL=analogRead(A0);
ValueMotorR=analogRead(A1);
VoltageMotorL=ValueMotorL*5/1023;
VoltageMotorR=ValueMotorR*5/1023;
CurrentMotorL=1/0.525*VoltageMotorL;
CurrentMotorR=1/0.525*VoltageMotorR;
Serial.print("ValueMotorL: ");
Serial.print(ValueMotorL,3);
Serial.print("ValueMotorR: ");
Serial.println(ValueMotorR,3);
Serial.print("VoltageMotorL: ");
Serial.print(VoltageMotorL,3);
Serial.println(" V");
Serial.print("VoltageMotorR: ");
Serial.print(VoltageMotorR,3);
Serial.println(" V");
Serial.print("CurrentMotorL: ");
Serial.print(CurrentMotorL,3);
Serial.println(" A");
Serial.print("CurrentMotorR: ");
Serial.print(CurrentMotorR,3);
Serial.println(" A");
TMRArd_InitTimer(DisplayCurrentTimer, DisplayCurrentTime);
}
///****CHECKING ENDGAME TIMER****/
unsigned char TestEndgameTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(EndgameTimer);
}
void RespToEndgameTimer(void){
//switch to endgame mode: find wall and drive into it
state = SCANWALL;
}
///****FINDING WALL****/
//
//unsigned char TestForWallBeacon(void){
// static unsigned int BeaconLevel_4000Int = 0;
//
// //read 4kHz beacon sensor
// BeaconLevel_4000Int = digitalRead(BeaconPin_4000Int);
//
// return BeaconLevel_400Int;
//}
* File: Sumobot_v9.ino
**************************************************************/
/*---------------- Includes ---------------------------------*/
#include <Timers.h>;
#include <stdbool.h>;
/*---------------- Module Defines ---------------------------*/
//states of the Sumobot:
#define SEARCHFWD 0 //explore (move forward&right)
#define SEARCHSPIN 1 //explore (move forward&right)
#define LEFTTOBOT 2 //forward&left towards opponent
#define RIGHTTOBOT 3 //forward&right towards opponent
#define FWDTOBOT 4 //forward towards opponent
#define BACKLEFT 5 //turn slight left to avoid tape
#define BACKRIGHT 6 //turn slight right to avoid tape
#define OVERRIGHT 7 //go over tape slightly
#define OVERLEFT 8 //go over tape slightly
#define SCANWALL 10 //endgame: find wall beacon
#define LEFTTOWALL 11 //endgame: forward&left towards wall
#define RIGHTTOWALL 12 //endgame: forward&right towards wall
#define FWDTOWALL 13 //endgame: forward towards wall
/*---------------- Module Level Variables -------------------*/
//timer settings
static int TimeToEndgame = 100000; //seconds (not for brick)
static int BackupTime = 2000; //milliseconds
static int SpinTime = 2000; //milliseconds
static int FwdSearchTime = 3000; //milliseconds
static int OvershootTime = 100; //milliseconds
static int BackToFieldTime = 500; //milliseconds
static int DisplayCurrentTime = 1000; //milliseconds
static int SpeedTime = 500; //milliseconds
//timer ids
static int EndgameTimer = 0;
static int BackupTimer = 1;
static int FwdSearchTimer = 2;
static int SpinTimer = 3;
static int OvershootTimer = 4;
static int BackToFieldTimer = 5;
static int SpeedTimer = 6;
static int DisplayCurrentTimer = 7;
//motor speeds
static int FwdAttackSpeed = 75; //both motors
static int FwdSearchSpeed = 75; //both motors
static int TurnSpeedFwdHigh = 75; //motors in opposite dirs
static int TurnSpeedFwdLow = 37; //motors in opposite dirs
static int TurnSpeedHigh = 75; //motors in opposite dirs, one faster than the other
static int TurnSpeedLow = 5; //motors in opposite dirs, one slower than the other
static int BackupSpeedHigh = 75; //one motor backs up faster (to turn)
static int BackupSpeedLow = 37; //other motor backs up slower (to turn)
static int ZeroSpeed = 0; //0 speed needed for less blocking turn when one sensor on
static int Acceleration1 = 200; //ramp up both motors
//beacon sensor thresholds
static int limfreq850_lower = 400; //Hz
static int limfreq850_upper = 1800; //Hz
//define pins
int BeaconPin_R850 = 2; //INPUT pin for right 850Hz sensor (interrupt)
int BeaconPin_L850 = 3; //INPUT pin for left 850Hz sensor (interrupt)
int TapePin_Back = 7; //INPUT pin for back tape sensor
int TapePin_ROuter = 8; //INPUT pin for outer right tape sensor
int TapePin_LOuter = 9; //INPUT pin for outer left tape sensor
int MotorPin_RPulse = 10; //OUTPUT pin for right motor speed
int MotorPin_LPulse = 11; //OUTPUT pin for left motor speed
int MotorPin_RDir = 12; //OUTPUT pin for right motor direction
int MotorPin_LDir = 13; //OUTPUT pin for left motor direction
int MotorPin_Rrevs = A0; //INPUT pin for right motor revs sensor
int MotorPin_Lrevs = A1; //INPUT pin for left motor revs sensor//int TapePin_RInner = ; //INPUT pin for inner right tape sensor (not implemented)
//int TapePin_LInner = ; //INPUT pin for inner left tape sensor (not implemented)
//int AccelPin_X = A2; //INPUT pin for X-acceleration sensor (not implemented)
//int AccelPin_Y = A3; //INPUT pin for Y-acceleration sensor (not implemented)
//define interrupt variables
volatile long t0_0 = TMRArd_GetTime();
volatile long t1_0 = 0;
volatile long t0_1 = TMRArd_GetTime();
volatile long t1_1 = 0;
volatile long deltaT0 = 0;
volatile long deltaT1 = 0;
volatile int c = 0;
volatile int d = 0;
static long cmax = 4;
static long dmax = cmax;
float freq0 = 0;
float freq1 = 0;
static int loopTime = (int) cmax * 1000/850; //milliseconds
static int state;
/*---------------- Module Function Prototypes ---------------*/
unsigned char TestSearchFwdTimer(void);
unsigned char TestSearchSpinTimer(void);
unsigned char TestForKey(void);
unsigned char TestTapeRightOuter(void);
unsigned char TestTapeLeftOuter(void);
unsigned char TestTapeBack(void);
unsigned char TestBackupTimer(void);
unsigned char TestOvershootTimer(void);
unsigned char TestBackToFieldTimer(void);
unsigned char TestBotLeft(void);
unsigned char TestBotRight(void);
unsigned char TestSpeedTimer(void);
unsigned char TestDisplayCurrentTimer(void);
unsigned char TestWallLeft(void);
unsigned char TestWallRight(void);
unsigned char TestEndgameTimer(void);
void RespToSearchFwdTimer(void);
void RespToSearchSpinTimer(void);
void RespToKey(void);
void RespToTapeRightOuter(void);
void RespToTapeLeftOuter(void);
void RespToTapeBack(void);
void RespToBackupTimer(void);
void RespToBotLeft(void);
void RespToBotRight(void);
void RespToBotFwd(void);
void RespToBotGone(void);
void RespToLeftBackupTimer(void);
void RespToRightBackupTimer(void);
void RespToSpeedTimer(void);
void RespToDisplayCurrentTimer(void);
void RespToWallLeft(void);
void RespToWallRight(void);
void RespToWallFwd(void);
void RespToEndgameTimer(void);
//void RespToMotorStall(void) {} // (not implemented)
//void RespToBump(void) {} // (not implemented)
void setDeltaT0(void);
void setDeltaT1(void);
void resetFreq(void);
void setOvershootTimer(void);
/*---------------- Arduino Main Functions -------------------*/
void setup(){
//set baud rate
Serial.begin(9600);
Serial.println("THE SUMOBOT RISES...");
//set pin modes
pinMode(BeaconPin_R850,INPUT);
pinMode(BeaconPin_L850,INPUT);
pinMode(MotorPin_RDir,OUTPUT);
pinMode(MotorPin_RPulse,OUTPUT);
pinMode(MotorPin_LDir,OUTPUT);
pinMode(MotorPin_LPulse,OUTPUT);
pinMode(TapePin_ROuter,INPUT);
pinMode(TapePin_LOuter,INPUT);
pinMode(TapePin_Back,INPUT);
pinMode(MotorPin_Rrevs,INPUT);
pinMode(MotorPin_Lrevs,INPUT);
// pinMode(TapePin_LInner,INPUT);
// pinMode(TapePin_RInner,INPUT);
// pinMode(AccelPin_X,INPUT);
// pinMode(AccelPin_Y,INPUT);
//set game timer
TMRArd_InitTimer(EndgameTimer,TimeToEndgame*1000);
//set current reading interval timer
TMRArd_InitTimer(DisplayCurrentTimer, DisplayCurrentTime);
//set interrupts: attachInterrupt(interrupt, function, mode)
attachInterrupt(0, setDeltaT0, RISING); //Pin2: rising edges
attachInterrupt(1, setDeltaT1, RISING); //Pin3: rising edges
//set initial state
TMRArd_InitTimer(SpinTimer,SpinTime);
state = SEARCHSPIN;
}
void loop() {
if (TestDisplayCurrentTimer()){ RespToDisplayCurrentTimer();}
switch (state) {
case(SEARCHFWD):
// if(TestForKey()) RespToKey();
if(TestSearchFwdTimer()){ Serial.print(state); RespToSearchFwdTimer();
Serial.println(" Timer expired");}
if(TestTapeRightOuter()){ Serial.print(state); RespToTapeRightOuter();
Serial.println(" Tape right detected");}
if(TestTapeLeftOuter()){ Serial.print(state); RespToTapeLeftOuter();
Serial.println(" Tape left detected");}
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
break;
case(SEARCHSPIN):
// if(TestForKey()) RespToKey();
if(TestSearchSpinTimer()){ Serial.print(state); RespToSearchSpinTimer();
Serial.println(" Timer expired");}
if(TestTapeRightOuter()){ Serial.print(state); RespToTapeRightOuter();
Serial.println(" Tape right detected");}
if(TestTapeLeftOuter()){ Serial.print(state); RespToTapeLeftOuter();
Serial.println(" Tape left detected");}
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
if(TestEndgameTimer()){ Serial.print(state); RespToEndgameTimer();
Serial.println(" The end is near");}
break;
case (LEFTTOBOT): //aim for bot
Serial.println(" LEFTTOBOT");
if(TestTapeRightOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape right detected"); state = OVERRIGHT; break;}
if(TestTapeLeftOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape left detected"); state = OVERLEFT; break;}
if(TestBotLeft()&&TestBotRight()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if((!TestBotRight())&&(!TestBotLeft())){ Serial.print(state); RespToBotGone();
Serial.println(" No bot detected"); resetFreq(); break;}
resetFreq();
break;
case (RIGHTTOBOT): //aim for bot
Serial.println(" RIGHTTOBOT");
if(TestTapeRightOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape right detected"); state = OVERRIGHT; break;}
if(TestTapeLeftOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape left detected"); state = OVERLEFT; break;}
if(TestBotLeft()&&TestBotRight()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
if((!TestBotRight())&&(!TestBotLeft())){ Serial.print(state); RespToBotGone();
Serial.println(" No bot detected"); resetFreq(); break;}
resetFreq();
break;
case (FWDTOBOT): //aim for bot
Serial.println(" FWDTOBOT");
if(TestTapeRightOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape right detected"); state = OVERRIGHT; break;}
if(TestTapeLeftOuter()){ Serial.print(state); setOvershootTimer();
Serial.println(" Tape left detected"); state = OVERLEFT; break;}
if(TestSpeedTimer()){ Serial.print(state); RespToSpeedTimer();
Serial.print("Speed Timer expired: ramp up");}
if(TestBotRight()&&(!TestBotLeft())){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()&&(!TestBotRight())){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
if((!TestBotRight())&&(!TestBotLeft())){ Serial.print(state); RespToBotGone();
Serial.println(" No bot detected"); resetFreq(); break;}
resetFreq();
break;
case (BACKRIGHT): //avoid tape
Serial.println(" BACKRIGHT");
if(TestBackupTimer()){ Serial.print(state); RespToBackupTimer();
Serial.println(" Timer expired"); }
if(TestTapeBack()){ Serial.print(state); RespToTapeBack();
Serial.println(" Tape back detected"); }
if(TestBackToFieldTimer()){ //don't go for bot when backing up after overshooting tape
Serial.println(TestBackToFieldTimer());
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
}
break;
case (BACKLEFT): //avoid tape
Serial.println(" BACKLEFT");
if(TestBackupTimer()){ Serial.print(state); RespToBackupTimer();
Serial.println(" Timer expired"); }
if(TestTapeBack()){ Serial.print(state); RespToTapeBack();
Serial.println(" Tape back detected"); }
if(TestBackToFieldTimer()){ //don't go for bot when backing up after overshooting tape
Serial.println(TestBackToFieldTimer());
if(TestBotRight()&&TestBotLeft()){ Serial.print(state); RespToBotFwd();
Serial.println(" Bot fwd detected"); resetFreq(); break;}
if(TestBotRight()){ Serial.print(state); RespToBotRight();
Serial.println(" Bot right detected"); resetFreq(); break;}
if(TestBotLeft()){ Serial.print(state); RespToBotLeft();
Serial.println(" Bot left detected"); resetFreq(); break;}
}
break;
case (OVERLEFT): //aim for bot
Serial.println(" TAPE LEFT: OVERSHOOTING");
if(TestOvershootTimer()){ Serial.print(state); RespToLeftBackupTimer();
Serial.println(" Done overshooting, back up"); resetFreq(); break;}
Serial.println(TestOvershootTimer();
resetFreq();
break;
case (OVERRIGHT): //aim for bot
Serial.println(" TAPE RIGHT: OVERSHOOTING");
if(TestOvershootTimer()){ Serial.print(state); RespToRightBackupTimer();
Serial.println(" Done overshooting, back up"); resetFreq(); break;}
Serial.println(TestOvershootTimer());
resetFreq();
break;
// case(SCANWALL): //endgame: find wall
// if(TestWallLeft()&TestWallRight()) RespToWallFwd();
// if(TestWallRight()) RespToWallRight();
// if(TestWallLeft()) RespToWallLeft();}
// break;
// case (LEFTOWALL): //endgame: aim for wall
// if(TestWallLeft()&TestWallRight()) RespToWallFwd();
// if(TestWallRight()) RespToWallRight();
// if(!TestWallLeft()&!TestWallRight()) RespToWallGone();}
// break;
// case (RIGHTTOWALL): //endgame: aim for wall
// if(TestWallLeft()&TestWallRight()) RespToWallFwd();
// if(TestWallLeft()) RespToWallLeft();
// if(!TestWallLeft()&!TestWallRight()) RespToWallGone();}
// break;
// case (FWDTOWALL): //endgame: aim for wall
// if(TestWallLeft()&!TestWallRight()) RespToWallLeft();
// if(TestWallRight()&!TestWallLeft()) RespToWallRight();
// if(!TestWallLeft()&!TestWallRight()) RespToWallGone();}
// break;
}
}
//interrupt 0 (Pin2) function
void setDeltaT0(void){
c++;
if(c==cmax){
t1_0=TMRArd_GetTime();
deltaT0 = t1_0-t0_0;
t0_0=t1_0;
c=0;
}
}
//interrupt 1 (Pin3) function
void setDeltaT1(void){
d++;
if(d==dmax){
t1_1=TMRArd_GetTime();
deltaT1 = t1_1-t0_1;
t0_1=t1_1;
d=0;
}
}
/*---------------- Module Functions -------------------------*/
/****RESET INTERRUPT FREQUENCY****/
void resetFreq(void){
deltaT0 = 0;
deltaT1 = 0;
delay(loopTime);
}
/****SEARCHING****/
unsigned char TestSearchFwdTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(FwdSearchTimer);
}
unsigned char TestSearchSpinTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(SpinTimer);
}
void RespToSearchFwdTimer(void) { //start spinning
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, TurnSpeedHigh);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, TurnSpeedLow);
//start spinning timer
TMRArd_InitTimer(SpinTimer,SpinTime);
state = SEARCHSPIN;
}
void RespToSearchSpinTimer(void) { //start forward
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
/**** USER INPUT****/
unsigned char TestForKey(void) {
unsigned char KeyEventOccurred;
KeyEventOccurred = Serial.available();
return KeyEventOccurred;
}
void RespToKey(void) {
unsigned char theKey;
theKey = Serial.read();
Serial.println(theKey);
Serial.print(", ASCII=");
Serial.println(theKey,HEX);
if (theKey == 's') {
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, 0);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, 0);
}
if (theKey == 'w') {
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, 100);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, 100);
}
if (theKey == 'a') {
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, 50);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, 50);
}
if (theKey == 'd') {
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, 50);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, 50);
}
if (theKey == 'x') {
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, 50);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, 50);
}
}
/****OVERSHOOTING TAPE****/
void setOvershootTimer(void){
TMRArd_InitTimer(OvershootTimer,OvershootTime);
}
unsigned char TestOvershootTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(OvershootTimer);
}
unsigned char TestBackToFieldTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(BackToFieldTimer);
}
void RespToLeftBackupTimer(void){
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedHigh);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedLow);
//start backup-without-responding-to-beacons timer after pushing mode
TMRArd_InitTimer(BackupTimer,BackupTime);
TMRArd_InitTimer(BackToFieldTimer,BackToFieldTime);
state = BACKLEFT;
}
void RespToRightBackupTimer(void){
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedLow);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedHigh);
//start backup-without-responding-to-beacons timer after pushing mode
TMRArd_InitTimer(BackupTimer,BackupTime);
TMRArd_InitTimer(BackToFieldTimer,BackToFieldTime);
state = BACKRIGHT;
}
/****AVOIDING TAPE****/
unsigned char TestTapeRightOuter(void){
unsigned char EventOccurred;
static unsigned int TapeLevelROuter = 0;
//read tape sensor: LOW when seeing tape
TapeLevelROuter = digitalRead(TapePin_ROuter);
EventOccurred = TapeLevelROuter<1; //1 if tape sensor is LOW
return EventOccurred;
};
unsigned char TestTapeLeftOuter(void){
unsigned char EventOccurred;
static unsigned int TapeLevelLOuter = 0;
//read tape sensor: LOW when seeing tape
TapeLevelLOuter = digitalRead(TapePin_LOuter);
EventOccurred = TapeLevelLOuter<1; //1 if tape sensor is LOW
return EventOccurred;
};
unsigned char TestTapeBack(void){
unsigned char EventOccurred;
static unsigned int TapeLevelBack = 0;
//read tape sensor: LOW when seeing tape
TapeLevelBack = digitalRead(TapePin_Back);
EventOccurred = TapeLevelBack<1; //1 if tape sensor is LOW
return EventOccurred;
};
unsigned char TestBackupTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(BackupTimer);
}
void RespToTapeRightOuter(void){ //back up while turning right
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedLow);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedHigh);
//start backup timer
TMRArd_InitTimer(BackupTimer,BackupTime);
state=BACKRIGHT;
}
void RespToTapeLeftOuter(void){ //back up while turning left
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, BackupSpeedHigh);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, BackupSpeedLow);
//start backup timer
TMRArd_InitTimer(BackupTimer,BackupTime);
state=BACKLEFT;
}
void RespToTapeBack(void){ //go forward
digitalWrite(MotorPin_LDir, HIGH);
analogWrite(MotorPin_LPulse, ZeroSpeed);
digitalWrite(MotorPin_RDir, HIGH);
analogWrite(MotorPin_RPulse, ZeroSpeed);
delay(50);
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
void RespToBackupTimer(void){ //go forth & find bot
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
/****FINDING BOT****/
unsigned char TestBotRight(void){
unsigned char EventOccurred;
freq0 = 1000*cmax/deltaT0;
if((freq0>limfreq850_lower)&&(freq0<limfreq850_upper)){
EventOccurred = 1;
}
else{ //no 850Hz detected to the right
EventOccurred = 0;
}
return EventOccurred;
}
unsigned char TestBotLeft(void){
unsigned char EventOccurred;
freq1 = 1000*dmax/deltaT1;
if((freq1>limfreq850_lower)&&(freq1<limfreq850_upper)){
EventOccurred = 1;
}
else{ //no 850Hz detected to the left
EventOccurred = 0;
}
return EventOccurred;
}
unsigned char TestSpeedTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(SpeedTimer);
}
unsigned char TestDisplayCurrentTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(DisplayCurrentTimer);
}
void RespToSpeedTimer(void){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, Acceleration1);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, Acceleration1);
}
void RespToBotLeft(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, TurnSpeedFwdLow);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, TurnSpeedFwdHigh);
//switch state
state = LEFTTOBOT;
}
void RespToBotRight(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, TurnSpeedFwdHigh);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, TurnSpeedFwdLow);
//switch state
state = RIGHTTOBOT;
}
void RespToBotFwd(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdAttackSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdAttackSpeed);
//switch state
TMRArd_InitTimer(SpeedTimer,SpeedTime); //set delay before speed ramps up
state = FWDTOBOT;
}
void RespToBotGone(){
digitalWrite(MotorPin_LDir, LOW);
analogWrite(MotorPin_LPulse, FwdSearchSpeed);
digitalWrite(MotorPin_RDir, LOW);
analogWrite(MotorPin_RPulse, FwdSearchSpeed);
//start fwdsearch timer
TMRArd_InitTimer(FwdSearchTimer,FwdSearchTime);
state = SEARCHFWD;
}
void RespToDisplayCurrentTimer(void){
static float ValueMotorL = 0;
static float ValueMotorR = 0;
static float VoltageMotorL = 0;
static float VoltageMotorR = 0;
static float CurrentMotorL = 0;
static float CurrentMotorR = 0;
ValueMotorL=analogRead(A0);
ValueMotorR=analogRead(A1);
VoltageMotorL=ValueMotorL*5/1023;
VoltageMotorR=ValueMotorR*5/1023;
CurrentMotorL=1/0.525*VoltageMotorL;
CurrentMotorR=1/0.525*VoltageMotorR;
Serial.print("ValueMotorL: ");
Serial.print(ValueMotorL,3);
Serial.print("ValueMotorR: ");
Serial.println(ValueMotorR,3);
Serial.print("VoltageMotorL: ");
Serial.print(VoltageMotorL,3);
Serial.println(" V");
Serial.print("VoltageMotorR: ");
Serial.print(VoltageMotorR,3);
Serial.println(" V");
Serial.print("CurrentMotorL: ");
Serial.print(CurrentMotorL,3);
Serial.println(" A");
Serial.print("CurrentMotorR: ");
Serial.print(CurrentMotorR,3);
Serial.println(" A");
TMRArd_InitTimer(DisplayCurrentTimer, DisplayCurrentTime);
}
///****CHECKING ENDGAME TIMER****/
unsigned char TestEndgameTimer(void){
return (unsigned char)TMRArd_IsTimerExpired(EndgameTimer);
}
void RespToEndgameTimer(void){
//switch to endgame mode: find wall and drive into it
state = SCANWALL;
}
///****FINDING WALL****/
//
//unsigned char TestForWallBeacon(void){
// static unsigned int BeaconLevel_4000Int = 0;
//
// //read 4kHz beacon sensor
// BeaconLevel_4000Int = digitalRead(BeaconPin_4000Int);
//
// return BeaconLevel_400Int;
//}