Zumo 32U4 – Squaring to an Edge or Line

This post talks about a common step for any robotics competition, namely, squaring the robot to a line or an edge.  This builds off of previous post where you worked on having the robot drive to an edge.  The program for the driving to an edge can be found at:

Zumo 32U4 Drive to Edge

For this step, we are just going to build off of that program and I assume that you already have that sketch completed.

There are many ways to square to a line.  You could drive the left side until the light sensor sees the line, then drive the right side.  However, unless the sensors are in line with the wheel base, this results in an error that requires multiple passes.  I like the use the proportional method.  The PID method is even better, but adds considerable complexity.  For this case, we will consider only the proportional.  A proportional squaring method is very similar to a proportional line follow that has been written about quite a bit for any line-following tutorial.  I’m writing this assuming that you understand the principles of proportional feedback.

In this case, we need two proportional feedback paths, one for the left motor and left light sensor, and one for the right motor and right light sensor.  As with a line follow, we calculate the error for each side, multiply each side by a gain to create a left and a right correction factor, then apply the correction factor to the motor power.  Unlike a line follower where we are adjusting the steering, we need to adjust the power of each motor so that the light sensors reach the edge of the table.  The pseudo-code looks something like this:

1. Read the value of the left and right light sensors.
2. Calculate the error of the left and right light sensors.
3. Calculate the correction factor of the left and right (correction is error times gain).
4. Apply the correction factor to the motor power.
5. Repeat as long as the left and right errors are larger than a certain tolerance.

There really is nothing difficult about this with respect to the code and there really are no new concepts.  Let’s create a function called square2line() that is type void with an input parameter of the tolerance (which should be an integer).  The tolerance is a way of calculating how close to square is close enough.  We will need some local variables, errorLeft, errorRight, corrLeft, and corrRight where corr is short for correction.  Everything else just builds off what we’ve already covered and is placed inside a do… while loop.  The condition to keep running the loop is if either |errorLeft|> tolerance or |errorRight|>tolerance.  Otherwise, the loop should end and the motors stopped.

void square2line(int tol) {
  int errorLeft, errorRight,corrLeft, corrRight;

  do {
    lineSensors.readCalibrated(lineSensorValues);
    errorLeft=lineSensorValues[0]-500;
    errorRight=lineSensorValues[2]-500;
    corrLeft=-errorLeft*KPsq;
    corrRight=-errorRight*KPsq;
    motors.setLeftSpeed(corrLeft);
    motors.setRightSpeed(corrRight);
  } while (abs(errorLeft)>tol||abs(errorRight)>tol);
  motors.setSpeeds(0,0);
} 

The value of the gain, KPsq, is not defined within this function.  It is a constant that is defined at the very beginning of the program.  Constants can be defined with a compiler directive called #define.  This tells the compile to define one term to be something else.  At the beginning of the program, I have:

#define KPsq 0.6

Which tells the compiler to use 0.6 everywhere I have KPsq in the program.  You could simply just use 0.6 directly in the program, but this is a value that should be tweaked, adjusted, optimized, and played with until perfect.  Putting it at the top of the program makes it easy to do this.

Here is the entire program:

#include <Zumo32U4.h>
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;
Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4Buzzer buzzer;

#define FORWARD_SPEED 400
#define EDGE_THRESHOLD 500
#define EDGE_TOLERANCE 50
#define KPsq 0.6

unsigned int lineSensorValues[3];

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

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

  drive2line(FORWARD_SPEED);
  square2line(EDGE_TOLERANCE);
}

void calLightSensors(int calTime) {
  lcd.clear();
  lcd.gotoXY(0,0);
  lcd.print("Press A");
  lcd.gotoXY(0,1);
  lcd.print("to Cal");
  buttonA.waitForButton();
  buzzer.playFrequency(440, 200, 15);
  lcd.clear();
  lcd.print("Cal\'ing");
  for (int i = 20*calTime; i > 0; i--) {
    lcd.gotoXY(0, 1);
    if(i%20==0) {
      lcd.print(i/20);
      lcd.print(" ");
      buzzer.playFrequency(440, 50, 15);
    }
    lineSensors.calibrate();
    delay(20);
  }
  buzzer.playFrequency(600, 200, 15);
  lcd.clear();
  lcd.print("Cal Done");
  delay(1000);
}

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);
}

void square2line(int tol) {
  int errorLeft, errorRight, corrLeft, corrRight;

  do {
    lineSensors.readCalibrated(lineSensorValues);
    errorLeft=lineSensorValues[0]-500;
    errorRight=lineSensorValues[2]-500;
    corrLeft=-errorLeft*KPsq;
    corrRight=-errorRight*KPsq;
    motors.setLeftSpeed(corrLeft);
    motors.setRightSpeed(corrRight);
  } while (abs(errorLeft)>tol||abs(errorRight)>tol);
  motors.setSpeeds(0,0);
}  

I also used a few other values as constants in order to make tweaking the program simpler.  There is a little bit of overshoot in the squaring to the line that is easily fixed with a PID algorithm, which will be the topic for a later post.  If everything is done correctly, the robot should go through a light sensor calibration, pause for the A button to be pressed, drive the an edge of the table, square to the edge, then pause until the button is pressed again.