Using encoders to drive some distance

From ROBOTC API Guide
Jump to: navigation, search

Encoders vs. Timing

Up until now you have driven some distance by doing something like this:

motor[leftServo] = 20;
motor[rightServo] = 20;
 
wait1Msec(1000);
 
motor[leftServo] = 0;
motor[rightServo] = 0;

While this works perfectly for basic tasks, you may have noticed that this method can be inconsistent. Variables like battery power level, friction on different surfaces, and even manufacturing tolerances in the motors can severely reduce your accuracy in driving a particular distance.

Thankfully, there is a better way to do this. Now that we are utilizing the encoders, we can drive forward a certain distance much more precisely. In fact, the only thing that will stop us from arriving perfectly at our destination every time is if the wheels happen to skid on the surface that is being driven on.

The Theory

We know from math class that the circumference of a circle can be defined as π * d - where π, or pi is a constant around 3.14, and d is the diameter of the circle.

Since our wheels are obviously circular, we can use these rules to make our robot drive a specific distance, without any trial and error - we need only know the diameter of the wheel.

We can see in the image below that when the robot moves, its wheels will rotate once for every time the robot has moved a distance equal to the circumference of the wheel.

The wheels rotate once for every circumference traveled.

Since we know that Circumference = pi * Diameter, we now know that when the wheels rotate once (that is, 360° of rotation), the distance traveled is equal to pi * the diameter of the wheel. If they rotate twice, we get twice that amount. From this, we can determine the formula:

Distance traveled = Wheel rotations * circumference

or...

Distance traveled = (Degrees turned / 360) * circumference

or...

Distance traveled = (Encoder ticks / 360) * circumference

which leads up to our final, useful formula:

Encoder ticks = (360 / circumference) * Distance to travel

The great thing is, (360 / circumference) will always be constant when we have the same robot. We can work this out beforehand and we will never have to worry about it again.

Programming

Say that we wanted our robot to drive ten inches. The first thing to do is find the diameter of the BoeBot wheels so that we can use that in our formula. From the Parallex BoeBot dimensions page, we can see that the wheels on the BoeBot have a diameter of approximately 2.62 inches.


We can multiply that by pi to get the circumference:

2.62 * 3.14 = 8.227 inches.

Now, to find the ratio of ticks to distance, we just divide 360 by that amount (as seen in the above formula), and get:

16 / 8.227 = 1.94 ticks per inch.

Since the current ROBOTC firmware for Arduino cannot handle floating-point (decimal) operations, we will take this time to round our result to 194 ticks per 100 inches.

Now that we know we need 194 encoder ticks to go 100 inches, we can write a simple program:

task main()
{
  //Start the encoder monitoring task, which will run in the background.
  StartTask(encoderTask);
 
  leftEncoderCount = 0; // It is good practice to reset encoder values at the start of a program.
 
  //Calculate total amount of encoder ticks needed by multiplying our factor (194 ticks for 100 inches) by the desired
  //amount of inches, then dividing out the 100. We don't want to calculate this every iteration, so before the loop starts,
  //we write this to the variable 'tickGoal'.
  int tickGoal = (194 * 10) / 100;
 
  while(leftEncoderCount < tickGoal)
  {
    motor[leftServo] = 20;  // The nice thing about encoders is that we can use any power value we want, and 
    motor[rightServo] = 20; // still get the same distance.
  }
  motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
  motor[rightServo] = 0;  
}

Running the program shows that the robot will now consistently travel almost exactly 10 inches. Because we divided by 100 and are limited to non-floating point operations, the distance may not be 100% accurate. However, this is compensated by having a program that provides extremely consistent results, which are far more useful to a programmer.

Using Functions

We can discover the real power of this code when we integrate it into a function - we will be able to write a function that specifies a distance, and a speed, and will make the robot go that distance and speed consistently and accurately.

Have a look at the definition of tickGoal in the code above:

int tickGoal = (194 * 10) / 100;

We are multiplying our constant ratio, 42, which will not change when we use the same wheels, by the desired distance, then dividing it by a constant amount. If we wanted to go a different distance, we would change only one value, the one that here is '10'. With a little alteration of the previous code, then, we can write a useful function for driving a certain distance at power 20:

void driveDistance(int inches)
{
  leftEncoderCount = 0; // It is good practice to reset encoder values at the start of a function.
 
  //Calculate inches by multiplying the ratio we determined earlier with the amount of 
  //inches to go, then divide by ten as the ratio used is for an inch value.
  //Since we don't want to calculate every iteration of the loop, we will find the clicks needed 
  //before we begin the loop.
  int tickGoal = (194 * inches) / 100;
 
  while(leftEncoderCount < tickGoal)
  {
    motor[leftServo] = 20;  // The nice thing about encoders is that we can use any power value we want, and 
    motor[rightServo] = 20; // still get the same distance.
  }
  motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
  motor[rightServo] = 0;  
}

This way, we can call this function within our main program to go any number of inches we wish.

We can make this even more powerful by specifying a second parameter, separated from the first one with a comma, so that we can also specify the motor's power when we call the function:

void driveDistance(int inches, int power)
{
  leftEncoderCount = 0; // It is good practice to reset encoder values at the start of a function.
 
  //Calculate inches by multiplying the ratio we determined earlier with the amount of 
  //inches to go, then divide by ten as the ratio used is for an inch value.
  //Since we don't want to calculate every iteration of the loop, we will find the clicks needed 
  //before we begin the loop.
  int tickGoal = (194 * inches) / 100;
 
  while(leftEncoderCount < tickGoal)
  {
    motor[leftServo] = power;  // The nice thing about encoders is that we can use any power value we want, and 
    motor[rightServo] = power; // still get the same distance.
  }
  motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
  motor[rightServo] = 0;  
}

Now we have a very powerful function. Say you wanted to drive forwards any amount of inches at any speed, then - you can use one line of code in task main to do this. If you want to drive backwards, you can just specify a negative power, and it will work perfectly.

So, let's say we want to drive forward 6 inches, backwards 11 inches, forward 8 inches, and then back 3 inches, returning to the starting point. Power 20 has worked well for us, so we will keep it at that. All we need to do now is add task main(), call the functions, and you should end up with the following program:

#pragma config(CircuitBoardType, typeCktBoardUNO)
#pragma config(PluginCircuitBoard, typeShieldParallaxBoeBot)
#pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0)
#pragma config(Sensor, dgtl2,  rightEncoder,   sensorDigitalHighImpedance)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorDigitalHighImpedance)
#pragma config(Motor,  servo_10,        leftServo,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl10, None)
#pragma config(Motor,  servo_11,        rightServo,    tmotorServoContinuousRotation, openLoop, reversed, IOPins, dgtl11, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
 
//used to store the count of the motor clicks
int leftEncoderCount = 0;
int rightEncoderCount = 0;
 
//a background task used to monitor the encoders and count the clicks
task encoderTask()
{
  //get the state of the encoders so that we know when they have changed
  // for example if the last state was high and the current state is low
  // or vise-versa, then we know that the encoder incremented another click.
  int lastStateLeft = SensorValue[leftEncoder];
 
  while(true)
  {
 
    //get the current state of the left encoder
    int state = SensorValue[leftEncoder];
 
    //has the left encoder changed states
    if (lastStateLeft != state) {
      //if so, then increment the leftEncoderCount by 1
      leftEncoderCount++;
 
      // then store the current state as the last state so that we can keep monitoring every change
      lastStateLeft = state;
    }
 
    //we add a pause between updates to keep from hogging the CPU and
    // so that we debounce the encoders.
    wait1Msec(10);
  }
}
 
void driveDistance(int inches, int power)
{
  leftEncoderCount = 0; // It is good practice to reset encoder values at the start of a function.
 
  //Calculate inches by multiplying the ratio we determined earlier with the amount of 
  //inches to go, then divide by ten as the ratio used is for an inch value.
  //Since we don't want to calculate every iteration of the loop, we will find the clicks needed 
  //before we begin the loop.
  int tickGoal = (194 * inches) / 100;
 
  while(leftEncoderCount < tickGoal)
  {
    motor[leftServo] = power;  // The nice thing about encoders is that we can use any power value we want, and 
    motor[rightServo] = power; // still get the same distance.
  }
  motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
  motor[rightServo] = 0;  
}
 
task main()
{
  //Start the encoder monitoring task, which will run in the background.
  StartTask(encoderTask);
 
  driveDistance(6,20); 
  wait1Msec(500);              //Stop in between each command to prevent momentum causing wheel skid.
 
  driveDistance(11,-20);
  wait1Msec(500);
 
  driveDistance(8,20);
  wait1Msec(500);
 
  driveDistance(3,-20);
}