C++ Robot Lab 4 – Take A Step

Table of Contents

Introduction

Your robot can now follow a path. In this lab, we are focusing on developing the code to detect a room boundary and continue until the robot’s wheels enter the next room (hop). When this lab is completed, you will be ready to develop the control algorithm for turning in the maze. The critical issue for the takeAStep algorithm is keeping track of the robot’s position with the sensor data that is available. This requires readings from the sensor(s) and implementation of a Finite State Machine (FSM) using a C++ switch-case statement. Your FSM will have three (3) states.

walk
hop
stop

What is New

C++ Code

switch(expression) {
   case constant:
      statement(s);
      break;  
   case constant:
      statement(s);
      break;
   ...
   default: //optional
   statement(s);
}

Arduino Built-In Functions

millis()      // number of milliseconds since program started running.

Walk

Open Lab3 in the Arduino IDE. Save As Lab 4. Create a new function named walk. The walk will allow us to reduce the path following code to only the two lines as shown. For reference, my lab 3 “path following algorithm” originally used 16 lines (9 C++ statements) of code.

void takeAStep() {
  /* Initilization */
      :
  /* Read the IR sensors */
      :
  /* To follow a path the ir sensor controls the opposite motor */
  analogWrite(AIN1, 0); // left motor
  analogWrite(AIN2, walk(sensorRight,motorLeftHIGH, motorLeftLOW));
  analogWrite(BIN1, 0); // right motor
  analogWrite(BIN2, walk(sensorLeft,motorRightHIGH, motorRightLOW));
  /* Write to the motors */ 
     :
}

int walk(uint8_t sensor, uint8_t motorHIGH, uint8_t motorLOW) {
  uint8_t motorSpeed;
      :
  return motorSpeed; 
}

The new walk function accepts three arguments and returns the PWM signal to be sent to the left or right motor (motorLeft or motorRight). The first walk argument is sensor which is either the left or right sensor reading (LOW or HIGH). With one exception; the structure of walk is functionally equivalent to your original algorithm, with the sensor value used in a conditional expression setting the speed (motorSpeed) of the opposite motor to its corresponding motorHIGH (parameters motorLeftHIGH, or motorRightHIGH) or motorLOW (parameters motorLeftLOW, or motorRightLOW) value accordingly. The one exception is how the new walk function, operates at the end of a room. Before the robot would only slow down as it passed over a room boundary. We now want to detect a room boundary using one of both of our inner IR sensors and once detected pass over the room boundary and then stop inside the next room.

Before you continue, verify that your robot’s performance has not changed.

Build the Framework

To take a step, we are going to upgrade our path following algorithm to a three (3) state, finite state machine (FSM). We will be using a C++ switch-case statement to implement the FSM. An if-else-if construct could have been used just as easily. A simple switch-case contains a variable (the switch) which is compared to a series of constants (the cases). The block of statements within a case is run on a match. For reasons, that date back to antiquity, statement blocks after the case are also run. To prevent this unfortunate behavior case statements nearly always end with a break instruction.

void setup

void setup(){
     :
  delay(5000);   // 5 second delay to set robot in the maze
}
     :
void takeAStep() {
  /* Initialization */ 
       :
  /* Finite State Machine */
  static int state = 0;     // fsm state 
  /* Read the IR sensors */
       :    
  /* Run the path following algorithm */
  switch(state) {
    /* follow path until room wall (i.e., inner IR) detected */  
    case 0:  // walk
      statements
      break;
   /* move forward (open loop) until inside room  
    case 1:  // hop
      statements
      break;
    case 2:  // stop
   } // end switch 

  /* Write to the motors */ 
       :
}

Text in gray is from the previous labs. Let’s take a closer look at each section.

/* Initialization */

As you write your code for each case, within the line following the algorithm you may need to declare a few new variables. Place these declaration statements in the initialization section of takeAStep.

/* Finite State Machine */

The beginning of the finite state machine section initializes the FSM and switches state to zero.

/* Run the path following algorithm */

To complete a single step the FSM sequentially moves through two states (or cases), to be defined in the following sections.

state 0: walk
state 1: hop
state 2: stop

Variables sensorLeft,sensorRight,motorLeft, and motorRight typically hold the input (sensor) and output (motor) values for each case.

state 0: walk

Write the code needed to teach your robot to walk until the end of a room is reached; the inner sensor(s) reads white. Before the end of a room is reached you can simply re-purpose your original walking code. This is the path following block-of-code from Section 3 without the definition of motorLeft and motorRight (both were defined as integers in the header block). Unlike the previous version, where your robot kept walking down the hallway, instruct your robot to proceed to the next state.

if (inner sensor(s) in the room){
  // walking code
      :
} else {
 // start hopping
 analogWrite(AIN1, 0); // "true" A ==> left motor (simply a convention)
 analogWrite(AIN2, motorLeftHIGH);
 analogWrite(BIN1, 0); // right motor
 analogWrite(BIN2, motorRightHIGH);
 state = 1;
}

state 1: hop

At the end of the last state, the motors were set to their ON position. During the hop state, your robot should proceed in an open-loop fashion. Continue doing nothing, until the inner sensor(s) detects the next room (white).

if (inner sensor(s) in the next room){
  switch to state 2
}

state 2: stop

At the end of the last state, the motors were still in their ON position. In this final state, end the mission by turning off the motors (brake) and turn ON the LED_BUILTIN led.

analogWrite(AIN1, 255); // left motor
analogWrite(AIN2, 255);
analogWrite(BIN1, 255); // right motor
analogWrite(BIN2, 255);

Test Your Code

Upload your code. Verify that the robot follows the path and hops into the next room – BLINK led turns ON.

Lab 4 Deliverable(s)

All labs should represent your own work – DO NOT COPY.

Submit all of the files in your sketch folder. Make sure that the code compiles without any errors.

Checklist

Remember before you turn in your lab…

FSM sketch with explanatory comments.

C++ Robot Lab 4 – Take A Step