C++ Robot Lab 5 – Turning Algorithm

View Exclusively

Table of Contents

Introduction

Your robot can now follow a path and stop at the start of a room (takeAStep) by implementing a simple algorithm that controls the motors based on inputs from the sensors. In this lab, we are focusing on how to turn the robot to face the desired direction. When this lab is completed, you will be able to control your robot for all situations it can encounter in the maze and will be ready to develop the control algorithm for navigating your path through the maze in Labs 6, 7, and 8. The critical issue for the turning algorithm is keeping track of the robot’s position with the sensor data that is available. This requires continuous readings from the sensor(s) and implementation of Finite State Machines (FSMs). Along the way we will write three (3) new functions:

  • turnLeft()
  • turnRight()
  • turnAround()

What is New

Data Types

const uint8_t variable  // Qualify variable as a constant
static uint8_t variable // Qualify variable that will persist after a function return

C++ Code

do {
  statement 
  statement 
  . . . 
  statement 
} while (expression);  // Repeat while expression remains true

Serial Monitor Commands

Serial.begin(9600);        // Configure baud rate 
Serial.print(“text”);      // Print the text to the serial monitor
Serial.println(variable);  // Print the variable to the serial monitor and start a new line

Timing Control

delay(time_value);         // Delay program execution by the time_value milliseconds

Build the Framework

Open Lab4 in the Arduino IDE. Save As Lab 5. At the end of takeAStep() add the following three (3) functions.

void loop()
{
  takeAStep();
}

void takeAStep(){
  // code from lab 1, 2, 3 and 4
}

void turnLeft(){
  /* Initialization */
  
  /* Turn algorithm */

  /* Write to the Motors */

}

void turnRight(){

}

void turnAround(){

}

int sensorRead(int sensor_pin, int current_level){
  // code from lab 3
}

takeAStep

You have now completed the education of your robot in how to take a step. Now it is time to teach it how to turn. Unfortunately, although yes, your robot can take a step it will not play nice with any new functions called from within loop. To solve this problem, in this section you will “encapsulate” the takeAStep function within a do-while loop.

The takeAStep() function works because it is continuously called from within loop. put another way, we are calling and returning from the takeAStep() function until the step is completed.  This causes a problem if we want to add additional functionality to our robot. In this instance functions turnLeft, turnRight, and turnAround – and that is the problem. If for instance, we were to follow takeAStep with turnRight, the program would run the line following algorithm once, then run the turnRight once, resulted in a really confused robot. Before we solve the problem, lets take a closer look at what is happening. If we were to tunnel down into the C++ code which implements the loop method from within the Arduino scripting language, we would find this while statement.

while(1) {
  loop();
}

Because the while condition is always true (1), the statements within the while block, in this case loop(), will be repeatedly called until the Arduino is reset. And here in lies the solution to our problem. To keep our takeAStep function from returning, we can “encapsulate” it within its’ own loop. However, in place of a condition which is always true, we will add a condition that will allow our function to return when the robot has completed a single step. Lets take a look at the solution.

void takeAStep()
{
  /* Initialization */ 
       :
  /* Finite State Machine */
  static int state = 0;    // fsm state
  do {
    /* Read the IR sensors */
       :
    /* Run the line following algorithm */
    switch(state) {
       :
    /* Write to the motors */
    analogWrite(PWMA, motorLeft);
    analogWrite(PWMB, motorRight); 
  } while(state != 0);
}

The do-while looping construct works best for our solution, because we want to make at least one pass through the FSM. On that first pass we move from state 0 to state 1, this means that the while test will fail (we will loop) until state 3 is completed and we return to state 0.

Add the do-while loop to the takeAStep() function.

Test Your Code

Before you added the do-while loop, your robot could take a step. Your robot should now be able to do exactly the same thing, so how do you know if your program is working? The answer is pretty simple. Move your test code in state 2: // hop, which turns ON the BLINK led after your call to takeAstep(). If everything is working the led will stay OFF until the robot completes the first step and then will turn ON.  Once you know your code is working you may comment out (//) or delete the test code.

Configure Motors

It is finally time to start teaching your robot how to turn. First, review “Controlling the Motors” in Lab 1. Up to this time, the DRV8848 motor driver has been configured for the right and left motor to move the robot forward. In the Initialization section of functions turnLeft() and turnRight(), configure the motors for the desired turn as defined in Table 1 “Motor Control Setting.”

Table 1. Motor Control Settings
Action Input
Left Motor Right Motor
AIN1 AIN2 BIN1 BIN2
Motors OFF 0 0 0 0
Forward 0 1 0 1
Spin Right (CW) 0 1 1 0
Spin Left (CCW) 1 0 0 1
Reverse 1 0 1 0
Brake 1 1 1 1

Spin Test

Unless you add a sensor (one of the design challenges) your robot will be executing an open-loop turn. Consequently, your robot will need to be able to turn about a point without drifting in any one direction. The tuning of the motors to eliminate drift is the objective of the spin test.

In setUp(), comment out takeAStep()and add a call to turnLeft(). Add the following code to turnLeft().

void turnLeft() {
  /* Initialization */
     :
  /* Turn algorithm */
     :
  /* Write to the motors */
  analogWrite(AIN1, motorLeftHIGH);
  analogWrite(AIN2, 0);
  analogWrite(BIN1, 0);
  analogWrite(BIN2, motorRightHIGH);
}

Compile and upload your code. Your robot should start spinning counter-clockwise. If your robot can complete at least two (2) revolutions without drifting then you can skip to the next section, otherwise, you will need to tune your motors employing the techniques learned in Lab 2 “Motor Control and Fast Pulse Width Modulation.”

Replace motorLeftHIGH and motorRightHIGH with numerical values that keep your robot spinning in place.  To discover these numerical values, using one of the techniques you employed in Lab 2, to tune the motors to track in a straight line, teach your robot to spin in place. These techniques included: trial-and-error, wheel encoder, a potentiometer, and binary search.

If you are unable to tune your robot to spin about a point, you may need to implement one of the hardware sensor solutions discussed in the design challenge section of the lab.

Finite State Machine Turning

In Lab 4 you implemented a four (4) state FSM to teach your robot to walk, hop, and skip. In this lab, you will again implement a four (4) state FSM to teach your robot how to turn. Implement the following pseudo-code. I would recommend looking to your takeAStep FSM for help.

/* Turn left algorithm */
switch  (state) {
  case 0:
  /* hold in reset state for some period of time */
    delay(2000); // robot moved to maze
    state = 1; // continue to state 1
    break;
  case 1: 
  /* loop on this state until right sensor enters the room in front of the robot */
  case 2:
  /* continue rotation through the room */
  case 3:
  /* rotation is complete when the right sensor enters the room to the left of the robot */
} // end switch

Once you have your robot turning left, encapsulate your finite state machine within a do-while loop.  Enable (remove comments) takeAStep and run your code. Your robot should now take a step and then turn left. After a short delay between actions (takeAStep – turnLeft – takeAStep …) your robot now autonomously walks around in circles (well actually squares). should traverse a square.

Repeat the steps above to teach your robot to turn right.

Turn Around

After testing your robot; determine in which direction it most reliably turns (left or right).  Call the corresponding function twice within turnAround() to make a U-turn.

Lab 5 Design Challenge

You can skip the remainder of this lab if you are happy receiving a passing or even a good grade on the lab. If you want to receive an excellent grade you will need to accept the challenge(s). Specifically, the maximum grade you can receive on the prelab 2 plus lab 2 if you do not accept the challenge(s) is 20 points out of 25 (80%).

These challenge may be completed at any time up to the last day of class.

Wheel Encoder

  • If your robot includes a wheel encoder,  implemented your turn using the encoder and the pin_change() function. The pin_change() function is included with the StatPak.ino file.
  • Replace software polling-based pin_change() function with a hardware pin change interrupt.

If you had trouble tuning your robot to spin about a point without drifting you may want to accept this challenge. Specifically, you are going to add a sensor to help your robot spin about a point. The following list contains only a few of the many possibilities in order to point you in the right direction. It is important to note that using a combination of more than one sensor for your algorithm greatly improves accuracy/functionality.

  • Rotary Encoder (not a wheel encoder)
    • Used to determine the position of a rotating shaft (usually a motor), through either a magnetic or optical sensor
    • Here is a basic tutorial from the Arduino website including example code
    • Note: pull up resistors are commonly required for certain types of rotary encoders
  • Gyroscope
    • Measures 3D angular velocity
  •  IMU (Inertial measurement unit)
    • Usually, a combination of an accelerometer and a gyroscope together, although it can include a magnetometer
    • Measures both acceleration and angular velocity
    • Requires complex mathematical algorithms to process data
    • When used correctly, can provide very detailed position information
  • Magnetometer
    • Measures magnetic fields (commonly Earth’s magnetic field)

Howtomechatronics has a useful tutorial on these concepts

*There are many more possibilities than the options listed here, and creativity will be rewarded.

Lab 5 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…

  1. FSM sketch with comments.
  2. The sketch of your own implementation.

Trash

Tip: An intersection is present when both sensors return 1 (true). You may also want to review C++ logical operators.

So how does this work? The first line creates a Boolean variable named start with an initial value of true. The statements within the do-while loop, which teaches the robot how to follow a line, will therefore be executed until an intersection is detected at which time the loop condition is set to state = 0 and the program returns to the Arduino loop method.

Write the Algorithm

Translate your algorithm into code using C++ conditional expressions. Start by watching this tutorial to learn/review C++ conditionals. Place your code in the main loop() section of your Arduino program.

Hint: The program can be implemented using a number of methods including:

  • Binary operators (|, &, <<, >>, ~)
  • If-else statement
  • Conditional operator (?)
  • Arduino map() function

You have now completed the education of your robot in how to take a step. Now it is time to teach it how to turn. Unfortunately, although yes, your robot can take a step it will not play nice with any new functions called from within loop. To solve this problem we will “encapsulate” the function within a within a do-while loop.

before we even start we will need to encapsulate takeAStep() within a do-while loop so we can add functions turnLeft, turnRight, and turnAround().

Before we can take this step (pun intended) however, we will need to solve a problem.  The takeAStep() function works because it is continuously called from within loop. put another way, we are calling and returning from the takeAStep() function until the step is completed.  This causes a problem if we want to add additional functionality to our robot. In this instance functions turnLeft, turnRight, and turnAround – and that is the problem. If for instance, if we were to follow takeAStep with turnRight, the program would run the line following code once, then run the turnRight code once, resulting in a really confused robot. Before we solve the problem, lets take a closer look at what is happening. If we were to tunnel down into the C++ code which implements the loop method from within the Arduino scripting language, we would find this while statement.

while(1) {
  loop();
}

Because the while condition is always true (1), the statements within the while block, in this case loop(), will be repeatedly called until the Arduino is reset. And here in lies the solution to our problem. To keep our takeAStep function from returning, we can “encapsulate” it within its’ own loop. However, in place of a condition which is always true, we will add a condition that will allow our function to return when the robot has completed a single step. Lets take a look at the solution.

This lab is designed to guide you through developing the turning algorithm that will allow your robot to navigate the maze. You will be starting with using only the four IR sensors and a finite state machine algorithm before moving onto a more complex method with one of the following (various types of encoders, gyroscope, accelerometer, color sensors, etc). At the end, you will be choosing the most efficient method that will consistently execute the turns to use in Labs 5, 6, and 7.

Your robot can now follow a line and stop at an intersection (takeAStep) by implementing a simple algorithm which controls the motors based on inputs from the sensors. In this lab, we are focusing on developing the code to cross intersections (hop) and turn the robot to face a desired direction (turnLeft, turnRight, turnAround). When this lab is completed, you will be able to control your robot for all situations it can encounter in the maze and will be ready to develop the control algorithm for navigating your path through the maze in Labs 5, 6 and 7. The critical issue for the turning algorithm is keeping track of the robot’s position with the sensor data that is available. This requires continuous readings from the sensor(s) and implementation of Finite State Machines (FSMs). Along the way we will write four (4) new functions:

  • takeAStep()
  • turnLeft()
  • turnRight()
  • turnAround()

Mehtods for Turning

While there are many methods for executing a turn at an intersection, we will be focusing one which requires developing a finite state machine that will accurately identify the position of the robot. You will have a chance to implement other turning methods as part of the design challenge.

FSM

The core concept behind the finite state machine solution is to translate the physical position of the robot when it is over the intersection to specific states that it will go through as it executes the turn. Some things to consider for this is what the initial position should be when the turn is executed. We can start at the moment the robot detects the intersection or after it has moved past the intersection. The initial position will determine the way the robot needs to turn in order to be aligned with the line after the turn.

Initializing the Motors

For our FSM solution to work the turn must start when the robot’s wheels are over the intersection.

Our first attempt at this solution will assume that the turn starts when the robot first detects the intersection. This means that all four IR sensors should be detecting the black line. If we are trying to handle a left turn, it should run the left motor at the minimum speed and the right motor at full speed. That way the robot will arc towards the left and should align with the line after the turn. There will be several transitions that the IR sensors should detect as the robot takes this path and we can associate each with a different state for the finite state machine.

The following algorithm assumes a line following and not an edge following robot with four IR sensors. If you are using the 3DoT IR Shield (Figure 1) the outer sensors use analog inputs and may be used with the AnalogRead() function to more accurately detect when a transition has occurred.

Figure 1 – Optical Sensor to Motor Driver Interface

The first state will be when the two outer sensors detect the black line of the intersection. The second state will be when the outer sensor on the right side leaves the black line and hits the white space. To make things easier to detect. During this time, the left outer sensors should also be transitioning to the white space as well. Eventually, the outer sensors will hit the vertical line and that would indicate that the turn is complete.

Apply what we discussed in lab to develop this finite state machine and verify that it works as intended. Please note any changes or modifications that you needed to make.