Zumo 32U4 Unregulated, Unsynchronized Motor Steer Block

This post brings us back to motion and navigation.  One of the goals was to make the Zumo behave a bit more like a LEGO EV3.  The EV3 has these very useful MoveSteer block.  While this seems like a very simplistic block when you use it, there is a lot of information under the hood of the block that the user doesn’t see.  A few highlights of the MoveSteer block are:

• The block automatically calculates the power distribution to each motor for values of steering between -100 and +100, increments of 1.
• The block counts the encoder pulses, and automatically determines to use the left encoder when turning right and the right encoder when turning left in order to select the encoder the with most amount of travel (maximizes accuracy).
• It uses a PID control to regulate the rotational velocity of the under any load within valid range.  This is a very complicated process with a lot of calculations.  Basically, the value that you enter for power is actually proportional to rotational velocity.  The brick dynamically measures the rotational velocity and adjusts the power to each motor to maintain a constant velocity.
• Wheel synchronization.  The brick calculates the differences in rotation between the two motors and adjusts the power to attempt to keep the motors in synchronization.

The purpose of this post is to look at the first two items in this list.  We will hopefully get to the other two, but that will be the topics of later posts.  First of all, let’s talk about the power distribution to the motors.  After logging the power values to the motors under several conditions, I mapped the relative power to each motor for adjusting the turn value from -100 to 100.  This is shown in the plot below.

As expected, if the turn value is 0, both motors are at the same power.  If the turn value is -100, the right motor is at the power value entered, and the left motor is at the negative value of the power level entered, and the opposite for the value of 100.  Of course, there are all of the values in between as well.

Another way of saying this is:

• If the turn value is less than 0, then the right motor is the target speed, and the left motor is the target speed times (turn value + 50)/50, or for those that like it in slope/intercept, it is 1/50*(turn value)+1.
• If the turn value is 0 or greater, then the left motor is the target speed, and the right motor is the target speed times (turn value-50)/(-50), or -1/50*(turn value)+1.

In Arduino, it is simply:

if(turnDirection>0) {
  targetSpeedLeft=targetSpeed;
  targetSpeedRight=targetSpeed*(turnDirection-50)/(-50.0);
}
else {
  targetSpeedRight=targetSpeed;
  targetSpeedLeft=targetSpeed*(turnDirection+50)/(50.0);
} 

Note that we do not have to actually evaluate if turnDirection<0 as it will be the default case if turnDirection is not greater than 0.   Once the weighting of the speed it done, you can set the motors:

motors.setSpeeds(targetSpeedLeft,targetSpeedRight); 

You can drive the motors based on the encoder counts just like we previously did in the post regarding how to read the encoders. However, they will be different if the robot is turning. Like the EV3, we would like to base the movement on the encoder that moves the most, or the one on the outside of the turn. This is shown below:

if (turnDirection>=0) 
  {
    do {
      countsLeft = encoders.getCountsLeft();
    } while (abs(countsLeft)<abs(enCounts));
  } else {
    do {
      countsRight = encoders.getCountsRight();      
    } while (abs(countsRight)<abs(enCounts));
  } 

The complete function for driving a specific number of encoder pulses for a specific turn direction is given below:

void unRegMoveSteer(int targetSpeed, int turnDirection, int enCounts) {
  int countsLeft; 
  int countsRight;
  int targetSpeedLeft;
  int targetSpeedRight;
 
  if(turnDirection>0) {
    targetSpeedLeft=targetSpeed;
    targetSpeedRight=targetSpeed*(turnDirection-50)/(-50.0);
  }
  else {
    targetSpeedRight=targetSpeed;
    targetSpeedLeft=targetSpeed*(turnDirection+50)/(50.0);
  }
  motors.setSpeeds(targetSpeedLeft,targetSpeedRight);
  if (turnDirection>=0) 
  {
    do {
      countsLeft = encoders.getCountsLeft();
    } while (abs(countsLeft)<abs(enCounts));
  } else {
    do {
      countsRight = encoders.getCountsRight();      
    } while (abs(countsRight)<abs(enCounts));
  }    
} 

You can use the function in the program as shown below:

#include <Zumo32U4.h> 

Zumo32U4Encoders encoders;
Zumo32U4Motors motors;
Zumo32U4ButtonA buttonA;
Zumo32U4Buzzer buzzer;

void setup() {
}

void loop() {
  
  motors.setSpeeds(0,0);
  buttonA.waitForButton();   
  buzzer.playFrequency(440, 200, 15);
  delay(200);

  unRegMoveSteer(150, 0, 1000);
  motors.setSpeeds(0,0); 
} 

This will make the robot move forward at a speed of 150, turn direction of 0 (straight forward) for 1000 encoder pulses.  You can try other values and make sure that the function works under all conditions.  Note that negative encoder counts won’t actually work as we use the absolute values as the end condition for the movement.