Bollard Bowing Example #1

Sketch Code Listing:

/*
   NRG2019_Bollard_Bowling_Ex_1.ino
   R. Baas
   2019-09-30

   This example sketch completes the Bollard Bowling Challenge
   (https://coderedrobotics.com/wp-content/uploads/2019/09/NRG-Bollard-Bowling-2019.pdf)

*/

/////////////////////////////// DEFINE CONSTANTS AND VARIABLES /////////////////////////////////

// Define Rover states by assigning names to constant integer numbers.  
// This makes it easier to follow the logic of each state.
const int startUp             = 0;  // State when rover is powered up in the start box.
const int followLineLook4X    = 1;  // State to follow line and look for cross lane.
const int turnRight90         = 2;  // State to turn right 90 degrees after finding cross lane.
const int followLineLook4End  = 3;  // etc.
const int ultrasonic2Wall     = 4;  // etc....
const int backAndRight180     = 5;
const int turnLeft90          = 6;
const int foundX              = 7;
const int foundEnd            = 8;
const int finishedXlanes      = 9;
const int followLineLook4End2 = 10;
const int foundEnd2           = 11;
const int backAndRight180_2   = 12;
const int followLineLook4X2   = 13;
const int foundX2             = 14;
const int goForward           = 15;
const int goLeft              = 16;
const int goRight             = 17;

const int totalXlanes = 3;  // Total number of cross lanes to navigate.

// Define a variable the holds a value between 0 and 254 that will be used for the speed of the motors.
// How does the drive motor speed affect the ability to follow the lane and navigate the course?
int carSpeed;

// Define variables that change as the program progresses.
int roverState;     // This variable tracks the rover state.
int XlanesDone = 0; // This variable tracks the number of cross lanes navigated.


////////////////// PAY ATTENTION TO THESE!!! //////////////////////////
// Define variables that determine points earned for pushing bollards and moving the robot car.
int lineFollowSpeed  = 110;
int turnSpeed        = 120;

int bollardPushSpeed = 80;
int bollardPushDist  = 20;

int endBoxDist       = 20;
///////////////////////////////////////////////////////////////////////


// Define the digital input pins connected to the line sensors. HIGH = True and LOW = False.
// NOTE!! If using new, single board line sensor, uncomment the next three lines and comment out the current lines.
//#define LT_R !digitalRead(10)
//#define LT_M !digitalRead(4)
//#define LT_L !digitalRead(2)

// Comment out the next three lines if using the newer single board line sensor.
#define LT_R digitalRead(10)
#define LT_M digitalRead(4)
#define LT_L digitalRead(2)

// Define the analog output pins that indicate the drive motors speeds.
#define ENA 5
#define ENB 6

// Define ultrasonic sensor pins.
int Echo = A4;
int Trig = A5;

// Define the digital output pins that determing the forwards or backwards
// rotation of the drive wheels.
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11

//////////////////////// END OF DEFINE CONSTANTS AND VARIABLES /////////////////////////////////


////////////////// START OF USER FUNCTION DEFINITIONS //////////////////////////////////////////

// Function to drive the wheels forward.
// Note the keywords HIGH and LOW.  These are "built-in constants" mean ON and OFF.
void forward() {
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("go forward!");
}

// Function to drive the wheels backwards.
void back() {
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("go back!");
}

// Function to drive the right wheels forwards and the left wheels backwards.
// Turns the robot right.
void right() {
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("go right!");
}

// Function to drive the left wheels forwards and the right wheels backwards.
// Turns the robot left.
void left() {
  analogWrite(ENA, carSpeed);
  analogWrite(ENB, carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("go left!");
}

// Function to stop all wheel rotation and stop the robot.
void allstop() {
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  Serial.println("Stop!");
}

// Functon to measure distance using the ultrasonic sensor. Return an integer representing centimeters to the backstop.
int getDistance() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  return (int)pulseIn(Echo, HIGH) / 58; // Return distance in centimeters.
}

// Function to follow the lane (line).  Return an integer result every time the function is called.
// This function must be called repeatedly (such as from a while loop) in order to follow the line.
int followLine() {
  int result;
  if (!LT_L && LT_M && !LT_R) {
    result = goForward;
    forward();
  }
  else if (LT_L && !LT_M && !LT_R) {
    result = goLeft;
    left();
    //while (LT_L);
  }
  else if (!LT_L && !LT_M && LT_R) {
    result = goRight;
    right();
    //while (LT_R);
  }
  else if (LT_L && LT_M && LT_R) {
    result = foundX;
    allstop();
  }
  else if (!LT_L && !LT_M && !LT_R) {
    result = foundEnd;
    allstop();
  }
  else {
    result = goForward; // If the sensors do not match any of the above, continue driving forward.
    forward();
  }
  return result;
}

// Function to turn left or right once a cross lane is found. Call the function with
// an integer "dir" = to 0 or 1.  dir = 0 turn left.  dir = 1 turn right.
// Need to turn right when starting a cross lane and left when finishing a cross lane.
void foundXfunc(int dir) {
  forward();
  delay(50);
  if (dir == 1) {
    right();
  }
  else {
    left();
  }
  delay(500);
  while (!LT_M);  // Keep turning left or right until the line is seen by the middle sensor again.
}

// Function to turn around after pushing a bollard. This function can be used
// on both ends of the cross lane.
void backAndRight180func() {
  back();
  delay(300);   // Backup for 300 msec.

  right();
  delay(500);   // Right for 500 msec. to clear the original lane from the sensors.

  right();
  while (!LT_M);  // Keep turning right until the line is seen by the middle sensor again.
}

// Function to drive forward up to a certian distance away from a vertical surface.  Used for bollard pushing
// and stopping in the end box.  Call the function with an integer (targetDist) for the distance to target.
void driveUltrasonic(int targetDist) {
  int distance;
  distance = getDistance();
  while (distance > targetDist) {
    forward();
    distance = getDistance();
  }
  allstop();
}

////////////////// END OF USER FUNCTION DEFINITIONS ////////////////////////////////////


////////////////// START OF STANDARD SETUP() FUNCTON ////////////////////////////
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  roverState = startUp;

}
////////////////// END OF STANDARD SETUP() FUNCTON ////////////////////////////


////////////////// START OF STANDARD LOOP() FUNCTON ////////////////////////////
void loop() {
  // put your main code here, to run repeatedly:
  Serial.println(roverState);

  // Use a Swith Case structure (https://www.arduino.cc/reference/en/language/structure/control-structure/switchcase/)
  // to process Rover States.  Process the Case that matches the current roverState integer value.  "break;" drops out of the current
  // Case and start the loop (and the switch statement) over again.
  switch (roverState) {
    case startUp:
      roverState = followLineLook4X;
      break;

    case followLineLook4X:
      carSpeed = lineFollowSpeed;   // Why set the carSpeed here?

      // Repeatedly call the followLine() function until the foundX state is returned.
      while (roverState != foundX) {
        roverState = followLine();
      }
      allstop();
      delay(500);
      break;

    case foundX:
      carSpeed = turnSpeed; // Why set the carSpeed here?
      foundXfunc(1);    // 1 = turn right.
      roverState = followLineLook4End;
      break;

    case followLineLook4End:
      carSpeed = lineFollowSpeed; // Why set the carSpeed here?

      // Repeatedly call the followLine() function until the foundEnd state is returned.
      while (roverState != foundEnd) {
        roverState = followLine();
      }
      break;

    case foundEnd:
      carSpeed = bollardPushSpeed;  // Why set the carSpeed here?
      driveUltrasonic(bollardPushDist);
      roverState = backAndRight180;
      carSpeed = 110;
      allstop();
      delay(500);
      break;

    case backAndRight180:
      carSpeed = turnSpeed; // Why set the carSpeed here?
      backAndRight180func();
      roverState = followLineLook4End2;
      break;

    case followLineLook4End2:
      carSpeed = lineFollowSpeed; // Why set the carSpeed here?

      // Repeatedly call the followLine() function until the foundEnd state is returned.
      while (roverState != foundEnd) {
        roverState = followLine();
      }
      roverState = foundEnd2;
      break;

    case foundEnd2:
      carSpeed = bollardPushSpeed;
      driveUltrasonic(bollardPushDist);
      roverState = backAndRight180_2;
      break;

    case backAndRight180_2:
      carSpeed = turnSpeed;
      backAndRight180func();
      roverState = followLineLook4X2;
      break;

    case followLineLook4X2:
      carSpeed = lineFollowSpeed;
      while (roverState != foundX) {
        roverState = followLine();
      }
      roverState = foundX2;
      break;

    case foundX2:
      carSpeed = turnSpeed;
      foundXfunc(0);
      XlanesDone = XlanesDone + 1;  // Increment the number of cross lanes done by one.

      // If the number of cross lanes done is less than three, look for the next cross lane.
      // Otherwise look for the end at the end box.
      if (XlanesDone == totalXlanes) {
        roverState = finishedXlanes;
      }
      else {
        roverState = followLineLook4X;
      }
      break;

    case finishedXlanes:
      carSpeed = lineFollowSpeed;

      // Repeatedly call the followLine() function until the foundEnd state is returned.
      while (roverState != foundEnd) {
        roverState = followLine();
      }

      // Drive to the backstop in the end box.
      driveUltrasonic(endBoxDist);
      while (1) {};   // Force into an infinite loop to effectively stop the program.
  } // End of switch()
}   // End of loop()
////////////////// END OF STANDARD LOOP() FUNCTON ////////////////////////////