Zumo 32U4 Synchronize Motor

I received a recent question on how to make the Zumo drive straight.  There are a few ways to do this, but the easiest way is to synchronize the motors.  In this post, I’ll briefly discuss a simple method to slave the right motor off of the left motor.  In this case, the right motor will dynamically adjust the power so that the encoder counts on the right motor match the left motor.  As a test, you can hold the left track to slow it down, and this should slow down the right track automatically.  Note that this doesn’t work the other way around.  That would take a slightly more complicated program, and I’ll try to cover that in a later post.

The main difficulty with the Zumo not going straight is due to the fact that we are using unregulated motor commands.  No two motors are ever assembled perfectly equal.  In addition, there are minor differences in friction in the gear boxes, the axles, etc.  As a result, we tell the motors to use the same power settings, and the H-bridges deliver the correct power settings, but the motors respond slightly differently.

To overcome this, we need to measure the values of the encoders as the robot is driving, and adjust the power to the right motor to force the encoders to be equal.  The basic flow would be:

• Reset the encoders.
• Set the power levels of the motors to an initial guess.
• Do the following:
  • Measure the left and right encoders values.
  • Calculate the error between the two encoders (error = left encoder – right encoder).
  • Calculate a correction factor (Kp*error where Kp is a proportional gain value).
  • Modify the right motor power value (right power = initial power + correction factor).
  • Apply the modified motor power values to the setSpeed of the motors.
  • Repeat this until an end condition is met.

However, even this may not have the Zumo go perfectly straight due to minor differences in wheel diameters and track thickness.  Therefore, I added a fudge factor for calculating the error, called STRAIGHTFACTOR.  This should be adjusted for very minor values.  Default should be 1, but you may need to tweak it somewhere between 0.9 and 1.1 depending on the robot.

#include <Zumo32U4.h>
Zumo32U4Encoders encoders;
Zumo32U4Motors motors;
Zumo32U4LCD lcd;
Zumo32U4ButtonA buttonA;

#define SPEED 200
#define DISTANCE 2000
#define Kp 1
#define STRAIGHTFACTOR 1  // Adjust this to correct for minor curve.  Should be in the 0.9 to 1.1 range

void setup() {
}

void loop() {
  int currentSpeedLeft=SPEED;
  int currentSpeedRight=SPEED;
  int error;
  int correction;

  // Start Sequence - Wait for Button
  lcd.print("Press A");
  motors.setSpeeds(0, 0);
  buttonA.waitForButton();
  lcd.clear();
  delay(200);
  
  int countsLeft = encoders.getCountsAndResetLeft();
  int countsRight = encoders.getCountsAndResetRight();

  motors.setSpeeds(currentSpeedLeft,currentSpeedRight);
  do {
    countsLeft = encoders.getCountsLeft();
    countsRight = encoders.getCountsRight();
    error = countsLeft-STRAIGHTFACTOR*countsRight;
    correction = Kp*error;
    currentSpeedRight = SPEED + correction;
    motors.setSpeeds(currentSpeedLeft,currentSpeedRight);
  }  while(countsLeft<DISTANCE&&countsRight<DISTANCE);
  motors.setSpeeds(0,0);
} 

The above example sets the left motor to a speed of 200, then modifies the speed of the right motor so that the error between the two encoders is reduced to zero.  I had the loop exit condition be 2000 encoder counts, but this can be modified to go any distance, or even use light sensors to exit the loop.  I was rather explicit with the math.  Much can be simplified to some thing like:

currentSpeed Right = SPEED + Kp*(encoders.getCountsLeft() - encoders.getCountsRight()); 

All this does is save a couple variables and a few lines of code and is not necessary.