#include <Metro.h>
#include <Servo.h>
Metro debugMetro(250); // print every 200ms
Servo myservo;
const unsigned long RUN_LIMIT_MS = 120000UL; // 2 minutes
unsigned long runStartMs = 0;
unsigned long stateStartMs = 0;
bool runActive = false;
bool shootingComplete = false;
const uint16_t NEAR_WALL_CM = 35;
const uint16_t PARALLEL_TOL_CM = 4;
unsigned long timer = 30000;
// Latest ultrasonic readings (cm)
int back_L_cm = 0, left_cm = 0, back_R_cm = 0;
const int trigPin_back_L = 23;
const int echoPin_back_L = 25;
const int trigPin_left = 27;
const int echoPin_left = 29;
const int trigPin_back_R = 31;
const int echoPin_back_R = 33;
// Line sensor pins
const int line0Pin = A0;
const int line1Pin = A1;
const int line2Pin = A2;
// Line sensor readings
int lineL = 0, lineM = 0, lineR = 0;
const int TAPE_TH = 650;
const int TAPE_LTH = 350;
// State machine
typedef enum {
// orientation phase
STATE_COUNTERCLOCKWISE,
// tape phase
STATE_TAPE_MOVE_RIGHT,
STATE_TAPE_MOVE_FORWARD_1,
STATE_TAPE_MOVE_FORWARD_2,
STATE_TAPE_DONE,
// SHOOTING PHASE
STATE_SHOOTING,
STATE_MOVE_BACKWARDS,
STATE_MOVE_LEFT,
STATE_RELOAD,
// GAME OVER
STATE_EMPTY
} States_t;
States_t currentState;
States_t prevState;
// Motion settings
const int speed = 255;
// RIGHT MOTOR PINS
const int enableA = 9;
const int in1 = 30;
const int in2 = 32;
// BACK MOTOR PINS
const int enableB = 6;
const int in3 = 34;
const int in4 = 36;
// LEFT MOTOR PINS
const int enableA_ = 10;
const int in1_ = 38;
const int in2_ = 24;
// FRONT MOTOR PINS
const int enableB_ = 11;
const int in3_ = 26;
const int in4_ = 28;
//FLYWHEEL MOTOR PINS
const int enableA_front = 7;
const int in1_front = 40;
const int in2_front = 42;
// const int enableB_back = 8;
// const int in3_back = 44;
// const int in4_back = 46;
// -------- Prototypes --------
void readLineSensors();
void readUltrasonicSensors();
void CalculateUltrasonicDistance();
bool eventUSInitialised();
void checkGlobalEvents(void);
void respToUSInitialised();
bool eventMovedBackwards();
void respToMovedBackwards();
bool eventMovedLeft();
void respToMovedLeft();
bool eventMovedRight();
void respToMovedRight();
bool eventReachedHogLine();
void respToReachedHogLine();
void moveForward(int, int);
void moveBackward(int, int);
void motorStop(int, int);
void botBackward(int);
void botLeft(int);
void botForward(int);
void botRight(int);
void botStop(void);
void botAnticlockwise(int);
void setup() {
Serial.begin(9600);
myservo.attach(7);
myservo.write(90); //stops it
pinMode(line0Pin, INPUT);
pinMode(line1Pin, INPUT);
pinMode(line2Pin, INPUT);
pinMode(trigPin_back_L, OUTPUT); pinMode(echoPin_back_L, INPUT);
pinMode(trigPin_left, OUTPUT); pinMode(echoPin_left, INPUT);
pinMode(trigPin_back_R, OUTPUT); pinMode(echoPin_back_R, INPUT);
pinMode(enableA, OUTPUT); pinMode(in1, OUTPUT); pinMode(in2, OUTPUT);
pinMode(enableB, OUTPUT); pinMode(in3, OUTPUT); pinMode(in4, OUTPUT);
pinMode(enableA_, OUTPUT); pinMode(in1_, OUTPUT); pinMode(in2_, OUTPUT);
pinMode(enableB_, OUTPUT); pinMode(in3_, OUTPUT); pinMode(in4_, OUTPUT);
prevState = STATE_EMPTY; //this state is a fill in state so that the motor gets trigerred
currentState = STATE_COUNTERCLOCKWISE;
botStop();
runStartMs = millis();
runActive = true;
//Serial.println("setup complete");
}
void loop() {
checkGlobalEvents();
switch (currentState) {
// ---------------- ORIENTATION PHASE ----------------
case STATE_COUNTERCLOCKWISE: {
if (prevState != STATE_COUNTERCLOCKWISE) {
botAnticlockwise(250);
prevState = STATE_COUNTERCLOCKWISE;
}
}
break;
case STATE_TAPE_MOVE_RIGHT:
if (prevState != STATE_TAPE_MOVE_RIGHT) {
botRight(speed);
prevState = STATE_TAPE_MOVE_RIGHT;
}
break;
case STATE_TAPE_MOVE_FORWARD_1:
if (prevState != STATE_TAPE_MOVE_FORWARD_1) {
botForward(speed);
prevState = STATE_TAPE_MOVE_FORWARD_1;
}
break;
case STATE_TAPE_MOVE_FORWARD_2:
if (prevState != STATE_TAPE_MOVE_FORWARD_2) {
botForward(speed);
prevState = STATE_TAPE_MOVE_FORWARD_2;
}
break;
case STATE_SHOOTING:
if (prevState != STATE_SHOOTING) {
myservo.write(-100);
delay(200);
myservo.write(90);
delay(500);
shootingComplete = true;
prevState = STATE_SHOOTING;
}
break;
case STATE_MOVE_BACKWARDS: {
if (prevState != STATE_MOVE_BACKWARDS) {
botBackward(speed);
prevState = STATE_MOVE_BACKWARDS;
}
}
break;
case STATE_MOVE_LEFT:
if (prevState != STATE_MOVE_LEFT) {
botLeft(speed);
Serial.print("Put in instruction to move left!");
prevState = STATE_MOVE_LEFT;
}
break;
case STATE_RELOAD:
if (prevState != STATE_RELOAD) {
botStop();
prevState = STATE_RELOAD;
}
break;
}
if (debugMetro.check()) {
Serial.print("state="); Serial.print(currentState);
Serial.print(" | ULTRA: BL:"); Serial.print(back_L_cm); Serial.print(", L:");
Serial.print(left_cm); Serial.print(", BR: "); Serial.print(back_R_cm);
Serial.print(" | Line: "); Serial.print(lineL); Serial.print(",");
Serial.print(lineM); Serial.print(","); Serial.println(lineR);
}
}
void checkGlobalEvents(void){
if (eventUSInitialised()) {respToUSInitialised();}
if (eventMovedRight()) {respToMovedRight();}
if (eventTimerExpired()) {respToTimerExpired();}
if (eventReachedHogLine()) {respToReachedHogLine();}
if (eventDoneShooting()) {respToDoneShooting();}
if (eventMovedBackwards()) {respToMovedBackwards();}
if (eventMovedLeft()) {respToMovedLeft();}
if (eventReloadFinished()) {respToReloadFinished();}
}
bool eventUSInitialised() {
if (currentState != STATE_COUNTERCLOCKWISE) return false;
if (currentState == STATE_COUNTERCLOCKWISE) { //should run if we are rotating anticlockwise
readUltrasonicSensors();
bool valid = (back_L_cm > 0) && (back_R_cm > 0) && (left_cm > 0);
int diff = (back_L_cm > back_R_cm) ? (back_L_cm - back_R_cm) : (back_R_cm - back_L_cm);
bool nearWalls =
(back_L_cm < NEAR_WALL_CM) &&
(back_R_cm < NEAR_WALL_CM) &&
(left_cm < NEAR_WALL_CM);
bool parallel = (diff < PARALLEL_TOL_CM);
if (valid && nearWalls && parallel) {
botStop();
Serial.print("Robot in correct position, now going to move back");
Serial.print("state!="); Serial.print(currentState);
Serial.print(" | ULTRA: BL!!:"); Serial.print(back_L_cm); Serial.print(", L:!!!");
Serial.print(left_cm); Serial.print(", BR:!!! "); Serial.print(back_R_cm);
return true;
}
else {
return false;
}
}
}
void respToUSInitialised() {
currentState = STATE_TAPE_MOVE_RIGHT;
prevState = STATE_COUNTERCLOCKWISE;
}
bool eventMovedRight() {
if (currentState != STATE_TAPE_MOVE_RIGHT) return false;
if (currentState == STATE_TAPE_MOVE_RIGHT) {
readLineSensors();
if (lineM > TAPE_TH) {
botStop(); //botStop is to reduce the motor overshooting
Serial.print("Reached the center line!");
Serial.print(" | Line:!! "); Serial.print(lineL); Serial.print(",");
Serial.print(lineM); Serial.print(","); Serial.println(lineR);
return true;
}
else {
return false;
}
}
}
void respToMovedRight() {
currentState = STATE_TAPE_MOVE_FORWARD_1;
prevState = STATE_TAPE_MOVE_RIGHT;
timer = millis();
Serial.println("Started timer");
}
bool eventTimerExpired(){
if (currentState != STATE_TAPE_MOVE_FORWARD_1) return false;
if (currentState == STATE_TAPE_MOVE_FORWARD_1) {
if (millis() - timer > 500) {
Serial.println("timer expired");
return true;
}else {
return false;
}
}
}
void respToTimerExpired(){
currentState = STATE_TAPE_MOVE_FORWARD_2;
prevState = STATE_TAPE_MOVE_FORWARD_1;
}
bool eventReachedHogLine() {
if (currentState != STATE_TAPE_MOVE_FORWARD_2) return false;
if (currentState == STATE_TAPE_MOVE_FORWARD_2) {
readLineSensors();
bool left_sensor_black = lineL > TAPE_LTH;
bool middle_sensor_black = lineM > TAPE_TH;
bool right_sensor_black = lineR > TAPE_TH;
if(left_sensor_black && !middle_sensor_black && !right_sensor_black) {
botAnticlockwise(225);
prevState = STATE_EMPTY; //so it continues to move forward
} else if (!left_sensor_black && !middle_sensor_black && right_sensor_black) {
botClockwise(225);
prevState = STATE_EMPTY;
}
if (lineL > TAPE_LTH && lineR > TAPE_TH) {
botStop();
Serial.println("Tape target reached!");
return true;
}
else {
return false;
}
}
}
void respToReachedHogLine() {
currentState = STATE_SHOOTING;
prevState = STATE_TAPE_MOVE_FORWARD_2;
}
bool eventDoneShooting() {
if (currentState != STATE_SHOOTING) return false;
if (currentState == STATE_SHOOTING) {
if (shootingComplete) {
shootingComplete = false;
return true;
}
else {
return false;
}
}
}
void respToDoneShooting() {
currentState = STATE_MOVE_BACKWARDS;
prevState = STATE_SHOOTING;
}
bool eventMovedBackwards() {
if (currentState != STATE_MOVE_BACKWARDS) return false;
if (currentState == STATE_MOVE_BACKWARDS) {
readUltrasonicSensors();
if (back_L_cm > 0 && back_L_cm <= 5) {
botStop();
Serial.print("Robot moved back, now going to move left");
Serial.print("state!="); Serial.print(currentState);
Serial.print(" | ULTRA: BL!!:"); Serial.print(back_L_cm); Serial.print(", L:!!!");
Serial.print(left_cm); Serial.print(", BR:!!! "); Serial.print(back_R_cm);
return true;
}
else {
return false;
}
}
}
void respToMovedBackwards() {
currentState = STATE_MOVE_LEFT;
prevState = STATE_MOVE_BACKWARDS;
}
bool eventMovedLeft() {
if (currentState != STATE_MOVE_LEFT) return false; //cos we need to return true or false, guard clause
if (currentState == STATE_MOVE_LEFT) {
readUltrasonicSensors();
if (left_cm <= 3) {
botStop();
Serial.print("Robot moved left, initialisation complete!");
Serial.print("state!="); Serial.print(currentState);
Serial.print(" | ULTRA: BL!!:"); Serial.print(back_L_cm); Serial.print(", L:!!!");
Serial.print(left_cm); Serial.print(", BR:!!! "); Serial.print(back_R_cm);
return true;
}
else {
return false;
}
}
}
void respToMovedLeft() {
currentState = STATE_RELOAD;
prevState = STATE_MOVE_LEFT;
timer = millis();
Serial.println("Started timer");
}
bool eventReloadFinished(){
if (currentState != STATE_RELOAD) return false;
if (currentState == STATE_RELOAD) {
if (millis() - timer > 3000) {
Serial.println("timer expired");
return true;
}else {
return false;
}
}
}
void respToReloadFinished(){
currentState = STATE_TAPE_MOVE_RIGHT;
prevState = STATE_RELOAD;
}
void readUltrasonicSensors() {
back_L_cm = calculateUltrasonicDistance(trigPin_back_L, echoPin_back_L);
left_cm = calculateUltrasonicDistance(trigPin_left, echoPin_left);
back_R_cm = calculateUltrasonicDistance(trigPin_back_R, echoPin_back_R);
}
void readLineSensors() {
lineL = analogRead(line0Pin); // left
lineM = analogRead(line1Pin); // middle
lineR = analogRead(line2Pin); // right
}
// Returns distance in cm.
// Returns -1 if no echo within timeout
int calculateUltrasonicDistance(int trigPin, int echoPin) {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Trigger: 10us HIGH pulse
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read echo pulse width (microseconds)
// Timeout set to 25,000us (~4.3m). Reduce to cut blocking/crosstalk.
unsigned long duration = pulseIn(echoPin, HIGH, 25000UL);
if (duration == 0) return -1; // timeout => invalid
// Sound speed conversion:
// distance(cm) = duration(us) / 58.0
int cm = (int)(duration / 58UL);
return cm;
}
// -------- Motor helpers --------
void moveForward(int high_pin, int low_pin) { digitalWrite(high_pin, HIGH); digitalWrite(low_pin, LOW); }
void moveBackward(int high_pin, int low_pin){ digitalWrite(low_pin, HIGH); digitalWrite(high_pin, LOW); }
void motorStop(int high_pin, int low_pin) { digitalWrite(low_pin, LOW); digitalWrite(high_pin, LOW); }
void botBackward(int speed) {
analogWrite(enableA, speed);
analogWrite(enableA_, speed);
moveBackward(in1, in2);
moveBackward(in2_, in1_);
}
void botLeft(int speed) {
analogWrite(enableB, speed);
analogWrite(enableB_, speed);
moveForward(in4, in3);
moveForward(in3_, in4_);
}
void botForward(int speed) {
analogWrite(enableA, speed);
analogWrite(enableA_, speed);
moveForward(in1, in2); // Right forward
moveForward(in2_, in1_); // Left forward
}
void botRight(int speed) {
analogWrite(enableB, speed);
analogWrite(enableB_, speed);
moveBackward(in4, in3);
moveBackward(in3_, in4_);
}
void botStop(){
motorStop(in1, in2);
motorStop(in3, in4);
motorStop(in1_, in2_);
motorStop(in3_, in4_);
}
void botAnticlockwise(int speed){
analogWrite(enableA, speed);
analogWrite(enableB, speed);
analogWrite(enableA_, speed);
analogWrite(enableB_, speed);
moveForward(in1, in2); // Right forward
moveBackward(in4, in3); // back forward
moveForward(in1_, in2_); // Left backward
moveBackward(in4_, in3_); // front backward
}
void botClockwise(int speed){
analogWrite(enableA, speed);
analogWrite(enableB, speed);
analogWrite(enableA_, speed);
analogWrite(enableB_, speed);
moveBackward(in1, in2); //R
moveForward(in4, in3); //B
moveForward(in2_, in1_); //L
moveBackward(in3_, in4_); //F
}