Drive Base Code using Encoders

In this lesson we will make a robot drive and turn using the encoders to take us to precise positions and specific angle of the turns. this code will work with any tank drive robot with 2 or more motors on the drive base.  We will extend this to other types of drives in future lessons.

the concepts to learn are 

1. Encoder Measurements

2. The Action of a Wheel  - Circumference

3. Turning Radius or Turning Circle

Code elements

1. While Loop

2. Drive function

3. Control Loop - Speed Control

Math Concepts

1. Concept of Error

2. Absolute Value

Device Set Up

Use the device wizard to add 4 motors that are connected to specific ports on the brain.  Reverse the motors on the right side of the robot.  Our example code looks like this:

controller Controller1 = controller(primary);
motor LF = motor(PORT1, ratio18_1, false);
motor RF = motor(PORT9, ratio18_1, true);
motor LB = motor(PORT3, ratio18_1, false);
motor RB = motor(PORT6, ratio18_1, true);

We have 4 motors LF = Left Front, RB = Right Back etc. A controller is added so that we can upload and start the code remotely

Drive Functions

We first write the general drive functions.  The drive function sets the speed of the motors.  The driveBrake function stops all the drive motors in break mode.

void drive(int lspeed, int rspeed, int wt){
  LF.spin(fwd, lspeed, pct);
  LB.spin(fwd, lspeed, pct);
  RF.spin(fwd, rspeed, pct);
  RB.spin(fwd, rspeed, pct);
  wait(wt, msec);
}
void driveBrake(){
  LF.stop(brake);
  LB.stop(brake);
  RF.stop(brake);
  RB.stop(brake);
}

Using these function reduces the code that we have to write by letting us reuse this code. It also compartmentalizes the code that will make debugging easier. Also if we decide to add motors to our drive like to make a 6 motor drive we will only have to make a change to these functions and not anywhere else in the code.

We call these functions with 2 lines of code, replacing lspeed,rspeed and wt with out desired values for speed and waiting time.

  drive(lspeed,rspeed,wt);
  driveBrake();

InchDrive Variables

This function will make the robot drive a set number of inches based on encoder measurements.first we set up some variables. Since we will be doing some math with these variables it is best to use the type float

void inchDrive(float target){
  Brain.Screen.printAt(1, 20, "inchDrive");
  
  float x = 0.0;
  float error = target-x;
  float speed = 75.0;
  float accuracy = 0.25;

target = the number of inches we want to move

x = our present position in inches

error = the difference between where we want to be and where we are now or mathematically  error = target - x

If our target is a positive distance then error is a positive number that devreases as we move toward the target and goes negative if we pass the target.  If target is a negative value then error starts as a negative value and moves towards zero as the robot moves to the target and becomes postive if the target is passed.

speed = the speed of the robot in percent. we will first code this as a constant number, later we will adjust the direction. and in future lessons we will introduce a more complex speed control.

accuracy is how close to the target do we want to stop. You may think the accuracy should be zero to be most precise, however if we do that then error would need to be 0.0000000 in order to stop if error was 0.000001 the robot would continue past the target.  So we set an accuracy to be something like 0.25 then the robot will stop withing 0.25 inch of the intended target.

Measuring the Distance Moved

Our encoders will measure the rotation of the wheel in revolutions or degrees.  The function of the wheel is to convert these rotations into linear movement. For each revolution of the wheel the robot will move the circumference of the wheel.  To calculate this we need to know the diameter of the wheel so we save this as a Global variable 

float D= 4.0; //diameter of our wheel
float Pi = 3.14;

the circumference is Pi*D

To ger the distance traveled we read the encoder revolutions and multiply by circumference

x = LF.position(rev)*Pi*D;

in this example we only use the encoder on the left front wheel for the calculations. In future we could measure both left and right and average or we could event use left and right to assure that the robot drives straight.

InchDrive Process

The process is first set all the variables and reset our encoder to zero then:

1. Move the robot a small amount

2. Measure the distance moved

3. Calculate the error

4. If error is larger than accuracy repeat the steps 1,2,3  if error is less than accuracy stop the robot

The code to implement this is:

 LF.setRotation(0.0, rev);
  while(fabs(error) > accuracy){
    drive(speed, speed, 10);
    x = LF.position(rev)*Pi*D;
    error = target-x;
  }
  driveBrake();

Absolute Value, Negative target and Driving Backwards

In the while condition we use the absolute value of the error which is the distance to the target.  The absolute value allows our target to be negative or positive.  The code for absolute value of a floating point number is:

fabs(variable)

If our target is negative the speed also needs to be negative.  More precisely if the error is negative the speed should be negative. This will allow the use of a negative target as well as reversing the robot  when it moves past the target.  The math to change the sign of the speed is this:

 speed*fabs(error)/error   //this multiplies the speed by +1 or -1

adding this to our code we get:

void inchDrive(float target){
  Brain.Screen.printAt(1, 20, "inchDrive");
  
  float x = 0.0;
  float error = target-x;
  float speed = 75.0;
  float accuracy = 0.2;
  LF.setRotation(0.0, rev);
  while(fabs(error) > accuracy){
    drive(speed*fabs(error)/error, speed*fabs(error)/error, 10);
    x = LF.position(rev)*pie*D;
    error = target-x;
  }
  driveBrake();
}

Add this code and call it from autonomous:

void autonomous(void) {
  // ..........................................................................
  inchDrive(20);
  wait(25, msec);
  inchDrive(-10);
  // ..........................................................................
}

This will make the robot move 20 inches forward and then reverse 10 inches.