Zumo 32U4 Drive to Edge

This post discusses a common issue with any sumo event or competition that involves navigating to a line.  Our goal with this post is simply to drive the robot placed on a light colored surface until it reaches a dark color.  In the case of my office, I have a light colored desk, and I want to drive it to the edge of the table and stop (preferably without going over the edge!).

This uses the light sensor calibration function from the previous post, so if you don’t have that, you would want to go back and look at it.  My main program looks like this:

#include <Zumo32U4.h>

Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4Buzzer buzzer;

#define FORWARD_SPEED 200
#define EDGE_THRESHOLD 500

unsigned int lineSensorValues[3];

void setup() {
  lineSensors.initThreeSensors();
  calLightSensors(10);
}

void loop() {
  lcd.print("Press A");
  motors.setSpeeds(0, 0);
  buttonA.waitForButton();
  lcd.clear();
  delay(200);
  drive2line(FORWARD_SPEED);
} 

The #define statements at the beginning define constants used in the program.  If there is a constant that you use through the program, it often is best to define it at the beginning so you only need to change it in one place if you every want to adjust it.

The function that is new in the program is drive2line(speed), which we need to create.  The basic flow of this is to turn the motors on, monitor the light sensors (take repeated readings) until either light sensor sees a value greater than EDGE_THRESHOLD, then turn the motors off.  Setting the speed and turning the speed to 0 is easy, and we did that in previous posts.  Reading the light sensors is also something that we’ve already done.  We’ve also already done something very similar by reading the encoders until they reach a certain value.  All we need to do is to put together what we already know.

We will be reading the light sensors inside a do… while statement.  The condition to continue the loop will simply be that both of the outer light sensors see a light colored surface (low value on reflected light).  We want to stay in the loop while the left sensor sees white and the right sensor sees white.  If either one sees a dark color, we want to exit and turn the motors off.  This can be accomplished with the following:

void drive2line(int motorSpeed) {
  motors.setSpeeds(motorSpeed,motorSpeed);
  do {
    lineSensors.readCalibrated(lineSensorValues);
  } while (lineSensorValues[0]<EDGE_THRESHOLD && lineSensorValues[2]<EDGE_THRESHOLD);
  motors.setSpeeds(0,0);
} 

Remember, the && symbol is a logical AND operation.  Assuming that you add the drive2line(speed) function, and copy the light sensor calibration function from the previous post, the robot should run the light sensor cal, then wait for the user to press the button, drive at a speed of 200 until it sees the edge of the table (or a black line), stop, then wait for the button and repeat.  Try increasing the speed and have some fun.  Just always be ready to catch the robot so it doesn’t go over the edge.

This entry was posted in Robotics and tagged , . Bookmark the permalink.