Using encoders to drive a specific distance

Arduino → Arduino Tutorials and Guided Projects → Lego + Arduino, Mobile Robotics Platform → Using encoders to drive a specific distance

Video

The precise distances for the robot to travel in the end program are drawn here. As you can see, the robot hits the marks pretty well.

Configuring the Encoders

Now that the encoders are wired and functional, we need to configure RobotC to register them. Once again, we must turn to the Motors and Sensors menu, select DFRobot Motor Shield, setup the motors, and then click over to the digital tab. Set dgtl8 and dgtl10 to "Quadrature Encoder" on the drop down menus, then name them "rightEncoder" and "leftEncoder" respectively.

The setup screen for the two encoders

Notice that RobotC automatically configures the next pin down to be "Quad Encoder 2nd Port". You do not have to do anything with these ports; they are merely there to show that those ports have been taken by the encoder.

Once you are done with that, RobotC should produce a block of code that looks like this:

#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Sensor, dgtl8, rightEncoder, sensorQuadEncoder) #pragma config(Sensor, dgtl10, leftEncoder, sensorQuadEncoder) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard

Measuring Distances with Encoders vs. Time

Encoders are not so much a new sensor as a more accurate way to set distances for your robot. Previously, you've coded a command to move forward and stop like this:

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

The main problem with this method is that it relies on time to judge distance. Time is an inherently flawed way of coding a driving distance, simply because the robot's speed (distance per unit of time) is not always the same. Variables like battery power, friction on different surfaces, and even manufacturing tolerances in the motors can change the speed of different robots using the same program or or even the speed of the same robot running the program a second time. What this means is the distance traveled during each wait1MSec () command is not necessarily the same.

Encoders, by measuring rotation distances, fix this problem handily. Because the circumference of the wheel is always the same, the distance you travel by rotating the wheel a full 360 degrees is also a constant, because they are the same distance.

The wheels rotate once for every circumference traveled.

Using this knowledge, we can figure out the distance traveled in one rotation by finding the circumference. Circumference is simply pi * Diameter, which in the case of the lego wheels equals ~7.07".

And if we want to find distances for rotations that are more or less than a full rotation:

Distance traveled = Wheel rotations * circumference

Which if you turn rotations into their individual degrees, becomes:

Distance traveled = (Degrees turned / 360) * circumference

And if you convert degrees into encoder values, each "tick" of the encoder being 1/2 a degree, you get:

Distance traveled = (Encoder Ticks / 720) * circumference

Add in the circumference of the Lego wheels, and the final distance formula becomes:

Distance traveled = (Encoder Ticks / 720) * 7.07"

Keep in mind that there will be a small amount of error, as the circumference is an estimate.

Programming with Encoders

Now that we have the formula for distance per rotation, we can use it to give us a far more useful piece of information: The number of encoder ticks per inch.

If we simply plug in 1" for distance traveled and solve for Encoder Ticks, we get:

1" * 720 / 7.07" = Encoder Ticks which is equivalent to 101.838 = Encoder Ticks

Which means that there are approximately 102 ticks to an inch

Now that we know we need 102 encoder ticks to go an inch, we can write a simple program:

task main() { sensorValue[leftEncoder] = 0; // It is good practice to reset encoder values before any command that uses them   /*Here we are calculating inches by multiplying the ratio we determined earlier with the amount of inches we desire. Since we don't want to calculate every iteration of the loop, we will find the ticks needed before we begin the loop. This also makes the program easier to understand and modify.*/   int tickGoal = 102 * 10;   while(SensorValue[leftEncoder] < tickGoal) { motor[leftServo] = 100; // Encoders will always allow the robot to go the same distance no matter what power the motors are set to motor[rightServo] = 100; } motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks. motor[rightServo] = 0; }

NOTE: It is absolutely vital that you set your encoders to zero every time that you will use them. Otherwise, the encoder ticks that have been accumulating will throw your program off and cause your robot to skip commands.

Run it, and you will see that the robot will go almost exactly 10 inches, very consistently. Because we rounded off a decimal at a point in our calculations, the distance may not be 100% accurate, but you will notice extremely consistent results - and this is far more useful to a programmer.

Also, note how we used the value of the left encoder. When the robot is going in a straight line, you can read from either encoder as they should be going at the same rate - it will not matter which one you choose.

Smaller units

What if we wanted to go, say, 10.5 inches? We know that the firmware cannot support floating-point operations, meaning that it cannot process decimals, which might make this task seem impossible at first. However, if we just change our units to be smaller (say, tenths of an inch), we can now input '105 tenths of an inch' and still use a whole number. We will keep the same ratio of 102, and divide by ten at the end of the calculation to minimize information lost in rounding.

task main() { sensorValue[leftEncoder] = 0; // It is good practice to reset encoder values before a command that uses them.   /*Here we are calculating how many ticks we will use by multiplying the number of inches by 10, then dividing the entire result by 10, thus making the resulting number still correct for a measurement in inches The program will automatically round this number to the nearest integer. Since we don't want to calculate every iteration of the loop, we will find the ticks needed before we begin the loop.*/   int tickGoal = (102 * 105) / 10;   while(SensorValue[leftEncoder] < tickGoal) { motor[leftServo] = 100; motor[rightServo] = 100; } motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks. motor[rightServo] = 0; }

Now we have driven 10.5 inches by specifying distance to the tenth of an inch.

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.

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

int tickGoal = (102 * 105) / 10;

We are multiplying our constant ratio, 102, 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 only have to change one value, in this case 105. With a slight alteration of the previous code, we can write a useful function for driving any distance that we want at power 100:

void driveDistance(int tenthsOfIn) { sensorValue[leftEncoder] = 0; // It is good practice to reset encoder values before a command that uses them.   /*Here we are calculating how many ticks we will use by multiplying the number of inches by 10, then dividing the entire result by 10, thus making the resulting number still correct for a measurement in inches The program will automatically round this number to the nearest integer. Since we don't want to calculate every iteration of the loop, we will find the ticks needed before we begin the loop.*/   int tickGoal = (102 * tenthsOfIn) / 10;   while(SensorValue[leftEncoder] < tickGoal) { motor[leftServo] = 100; // The nice thing about encoders is that we can use any power value we want, and motor[rightServo] = 100; // 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 tenthsOfIn, int power) { sensorValue[leftEncoder] = 0; // It is good practice to reset encoder values before any command that uses them.   /*Here we are calculating how many ticks we will use by multiplying the number of inches by 10, then dividing the entire result by 10, thus making the resulting number still correct for a measurement in inches The program will automatically round this number to the nearest integer. Since we don't want to calculate every iteration of the loop, we will find the ticks needed before we begin the loop.*/   int tickGoal = (102 * tenthsOfIn) / 10;   while(SensorValue[leftEncoder] < tickGoal) { motor[leftServo] = power; // We can now set the power from the function's second parameter. motor[rightServo] = power; } 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 useful function. You can easily command the robot to move forward to any distance at any power. If you want to drive backwards, however, it is a little different. It might seem easy, as you can just set the 'power' parameter to a negative value. However, since the encoder reads a negative value when the motor spins backwards, and the loop tests for the value of the encoder being are less than the tickGoal, You would end up driving backwards indefinitely, since the encoder would always read less than the tickGoal.

There are two ways to deal with this. The simplest way is to just place a negative sign in front of every distance going backwards.

However, since this might be difficult to remember and lead to unnecessary error, what you can do is encase the encoder value in the while() loop's condition in the abs() (absolute value) function, which turns negative numbers positive, and leaves positive numbers as positive. That way, the less than comparison will always change whether the encoders tick forwards or backwards.

There is one small problem - because the Arduino UNO has such little memory, The abs() function is not automatically coded into it. Fortunately, there is a way to add this function back into robotC for this program. Simply put this code right below the configuration code:

#define abs(X) ((X < 0) ? -1 * X : X)

This simply tells the program that when the abs() function is called, the program should read X (the value inside the parentheses), determine if it is below zero, and multiply it by -1 if it is. It uses Ternary operators (? and :) to simplify the code needed to accomplish that set of tasks

Click here for more details about Ternary Operators

When we add these changes to the "driveDistance" function, your program should look like this:

#define abs(X) ((X < 0) ? -1 * X : X)   void driveDistance(int tenthsOfIn, int power) { sensorValue[leftEncoder] = 0; // It is good practice to reset encoder values at the start of a function.     /*Here we are calculating how many ticks we will use by multiplying the number of inches by 10, then dividing the entire result by 10, thus making the resulting number still correct for a measurement in inches The program will automatically round this number to the nearest integer. Since we don't want to calculate every iteration of the loop, we will find the ticks needed before we begin the loop.*/   int tickGoal = (102 * tenthsOfIn) / 10;   while(abs(SensorValue[leftEncoder]) < tickGoal) { motor[leftServo] = power; // We can now set the power from the function's second parameter. motor[rightServo] = power; } motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks. motor[rightServo] = 0; }

So, let's say we want to drive forward 6.2 inches, backwards 5.4 inches, forward 8.7 inches, and then back 9.5 inches, returning to the starting point. Power 100 has worked well for us, so we will keep it at that (If you wish to have more accuracy, lower the power a bit). 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, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Sensor, dgtl8, rightEncoder, sensorQuadEncoder) #pragma config(Sensor, dgtl10, leftEncoder, sensorQuadEncoder) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard   #define abs(X) ((X < 0) ? -1 * X : X)   void driveDistance(int tenthsOfIn, int power) { SensorValue[leftEncoder] = 0; // It is good practice to reset encoder values at the start of a function.   /*Here we are calculating how many ticks we will use by multiplying the number of inches by 10, then dividing the entire result by 10, thus making the resulting number still correct for a measurement in inches The program will automatically round this number to the nearest integer. Since we don't want to calculate every iteration of the loop, we will find the ticks needed before we begin the loop.*/   int tickGoal = (102 * tenthsOfIn) / 10;   //Use abs() to allow the < operator to apply when there are negative encoder values. while(abs(SensorValue[leftEncoder]) < tickGoal) { motor[leftServo] = power; // We can now set the power from the function's second parameter. motor[rightServo] = power; } motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks. motor[rightServo] = 0; }   task main() { //Distances specified in tenths of an inch.   driveDistance(62,50); wait1Msec(500); //Stop in between each command to prevent momentum causing wheel skid. driveDistance(54,-50); wait1Msec(500); driveDistance(87,50); wait1Msec(500); driveDistance(95,-50); }

NOTE: Because of limitations on data storage in the Arduino (see next lesson for details), the maximum distance for the driveDistance function is 32 inches (320 tenths of an inch). If you want the robot to go farther than that, you will have to code multiple driveDistance functions one after another.