Getting Started

You should have already downloaded and modified the state machine template. If you haven’t yet, start with Part 1 of this guide.

In this part, we’re going to add three new states to help our robot avoid obstacles. These states will use the Time of Flight sensors to check for obstacles. Depending on what the sensors detect, the robot could enter one of three states: AVOID_LEFT, AVOID_RIGHT, and BACK.

Step 1: Add the object avoidance states

At line 31, add the following:

if (dist1 < 200 || dist2 < 200) 
{
  state = "BACK";
}
else if(dist1 < 500) 
{
  state = "AVOID_LEFT";
}
else if (dist2 < 500) 
{
  state = "AVOID_RIGHT";
}

Step 2: Clean up FOLLOW state’s if statement

Don’t forget to change the if statement for the “FOLLOW” state to an else if . This should be near line 46

else if(huskylens.available()) 

Step 3: Add the behavior code for our new states

Around line 88 (directly after the SEARCH state’s behavior) add these new if statements:

else if(state == "BACK") {
  moveBackward();
}
else if(state == "AVOID_LEFT") {
  turnLeft();
  moveForward();
}
else if(state == "AVOID_RIGHT") {
  turnRight();
  moveForward();
}

Step 4: Test your code

Does the robot behave as you expected?

How could you improve the algorithm?