Using Encoders to Correct Motor Variation
|
Video
Why is the robot not driving straight?
You have probably noticed by now that your robot usually has trouble driving straight. You can see it both on your own robot and in our previous videos: It often drifts to one side or the other over a long (or even short) period of time. This is because the robot's wheels do not go the same speed at the same power. If each wheel does not go at exactly the same speed, the robot is not driving straight but it is in fact executing a very wide swing turn. "But I am telling them to go the same speed!" you may say. "The power value I am sending is the same for each wheel!"
Unfortunately, even if you send the same power value to each wheel, this will absolutely not guarantee that each wheel will move at the same speed. Why? Because of tiny variables such as friction, resistance, and manufacturing differences, one motor at power 100 will not go at the same speed as another motor at power 100.
How can I fix this?
Now that your robot can make use of its encoders, we can find out a motor's speed within a program very easily. If we take the amount the motors turn within a specific timeframe, say 1 second, and find they go a certain amount of encoder ticks, say 300, we can easily determine the speed of the first motor, then compare that speed to the speed of the other one. That difference becomes a guide to how much power to give or take from that wheel to match the other motor.
The Theory
This method is called 'proportional control' and is very useful in many contexts where a balance between elements is important. Proportional control relies on the principle that one motor is the “master,” who sets the pace, and the other motor is the “slave,” which adjusts its power to match the “master.” It doesn't matter which side you choose, but we will use the left motor as the master motor and the right motor as the slave.
Think of it this way: The master is going along a path at his own pace. His pace might change slightly when he gets tired, has to climb over an obstacle, or is going downhill. The slave's job is to keep alongside the master by speeding up when he falls behind, and slowing down when he goes too far ahead.
The difference between the master's speed and the slave's speed is called the 'error'. If they are going along at exactly the same pace, the error value will be zero. If the slave is too slow, the error is positive. If the slave is going too fast, the error is negative. Error is defined as:
Error = Speed of master - Speed of slave
After the error is found, it should be added onto the power value of the slave motor, so that it will go faster by an appropriate amount. However, there is something important to to first - the error should be multiplied by a number called the Constant of Proportionality, usually expressed as 'kp'. Kp converts the difference in encoder speeds (error) into something that can be used to adjust the motor power.
For example: say we find that the master encoder is ticking at 300 ticks per second, and the slave encoder is ticking at 250. We apply the error formula and find that the error = 300 - 250 = 50. However, we don't want to add 50 straight onto the motor power! It will overcompensate far too much and zoom ahead, and the next time it calculates the error it will speed backwards, overcompensating again. To keep the slave motor from oscillating wildly, we need to multiply the error by a value, which here is kp. If kp is, say, 0.2, we multiply the error value (50) by kp (0.2) and get the result: 50 * 0.2 = 10. If we were to add 10 to the slave motor power instead of 50, this would give us a much more reasonable increase in speed.
There is no magic way to determine the best value of kp. You simply need to use trial and error to find a value that will result in neither overcompensation nor under compensation. This is refereed to as 'tuning' kp.
In our circumstances, using the Arduino UNO with RobotC, we cannot use floating point numbers, which means that a value of, say, 0.2 for kp is not possible. However, if you consider that multiplying by 0.2 is the same as dividing by 5, then we can use this property and set kp so that the error is divided by a whole number instead of multiplied by a decimal.
So, we have added a suitable value to the power of the slave motor to compensate for the error. Now, we need to do it again, looping around a certain amount of times per second. For this application, ten times per second is ideal.
The Code
Let's write a program encompassing the concepts we have just talked about. This will make the robot drive straight indefinitely.
task main() { /*The powers we give to both motors. masterPower will remain constant while slavePower will change so that the right wheel keeps the same speed as the left wheel.*/ int masterPower = 30; int slavePower = 30; /*Essentially the difference between the master encoder and the slave encoder. Negative if slave has to slow down, positive if it has to speed up. If the motors moved at exactly the same speed, this value would be 0./* int error = 0; /*'Constant of proportionality' which the error is divided by. Usually this is a number between 1 and 0 the error is multiplied by, but we cannot use floating point numbers. Basically, it lets us choose how much the difference in encoder values effects the final power change to the motor.*/ int kp = 5; //Reset the encoders. SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; //Repeat ten times a second. while(true) { //Set the motor powers to their respective variables. motor[leftServo] = masterPower; motor[rightServo] = slavePower; /*The error value is set as a scaled value representing the amount the slave motor power needs to change. For example, if the left motor is moving faster than the right, then this will come out as a positive number, meaning the right motor has to speed up.*/ error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; /*This adds the error to slavePower, divided by kp. The '+=' operator means that this expression really says "slavePower = slavepower + error / kp", effectively adding on the value after the operator. Dividing by kp means that the error is scaled accordingly so that the motor value does not change too much or too little. You should 'tune' kp to get the best value. For us, this turned out to be around 5. */ slavePower += error / kp; //Reset the encoders every loop so we have a fresh value to use to calculate the error. SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; /*Makes the loop repeat ten times a second. If it repeats too much we lose accuracy due to the fact that we don't have access to floating point math, however if it repeats to little the proportional algorithm will not be as effective. Keep in mind that if this value is changed, kp must change accordingly.*/ wait1Msec(100); } } |
This may look like a long program, but without the comments it is only 18 lines long.
Run the program and you will see your robot drive much straighter than it has before.
Driving straight for a distance
We are now going to use this knowledge and apply it to the previous lesson by writing a function that will drive straight for a certain distance.
Recall the function driveDistance from the previous lesson:
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; } |
Remember to put this at the top of the program, below the configuration code:
#define abs(X) ((X < 0) ? -1 * X : X) |
See how we simply set both motors' power within the while loop in driveDistance,? If we merge this with our drive straight code, we will get a new function:
void driveStraightDistance(int tenthsOfIn, int masterPower) { int tickGoal = (102 * tenthsOfIn) / 10; /*Initialise slavePower as masterPower - 2 so we don't get huge error for the first few iterations. The -2 value is based off a rough guess of how much the motors are different, which prevents the robot from veering off course at the start of the function.*/ int slavePower = masterPower - 2; int error = 0; int kp = 5; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; //We still only have to monitor only one encoder as we have made it so that they will have the same values anyway. while(abs(SensorValue[leftEncoder]) < tickGoal) { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); } motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks. motor[rightServo] = 0; } |
Will this work? No. The while loop will never end. We reset both encoder values every iteration, so the encoder tick count never gets anywhere near the threshold.
To fix this, we will need to add another variable called 'totalTicks', which will add the new encoder values to the total every time the loop iterates. Therefore, at any time, it will have a value equal to the total encoder ticks since the function was called. It is very simple to implement:
void driveStraightDistance(int tenthsOfIn, int masterPower) { int tickGoal = (102 * tenthsOfIn) / 10; //This will count up the total encoder ticks despite the fact that the encoders are constantly reset. int totalTicks = 0; /* Initialise slavePower as masterPower - 2 so we don't get huge error for the first few iterations. The -2 value is based off a rough guess of how much the motors are different, which prevents the robot from veering off course at the start of the function. */ int slavePower = masterPower - 2; int error = 0; int kp = 5; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset. while(abs(totalTicks) < tickGoal) { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); //Add this iteration's encoder values to totalTicks. totalTicks+= SensorValue[leftEncoder]; } motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks. motor[rightServo] = 0; } |
Now unfortunately, we still have a problem. Whenever we make "tenthsOfIn" greater than 320, the robot will either not move on that command, or go for a distance much shorter than was programmed. When we go into debug mode, we see that the value of "tickGoal" is coming out to be either negative, or way smaller than it should be. This is because the variable "tickGoal" is an integer type (which we defined by the word "int" before the variable). Integers have a positive value limitation of 32,767, after which they wrap around to -32,768 (its most negative limit) and keep counting up. Because there are so many ticks to an inch, and because we're using tenths of an inch in our calculations, any value much larger than 320 causes the integer to wrap around and mess up the calculations.(At 320 tenths of an inch, "tickGoal" has to process 320*102 (32,640), which is dangerously close to the integer limit) For example, if we coded "tenthsOfIn" to be 400, whenever the computer multiplies 400 by 102, it gets 40,800. But since "tickGoal" as an integer can't handle this number in its allotted byte space, it wraps around at 32,767 to -32,768 and keeps counting up to -24,734, at which point it continues to solve the equation and divides by 10. "tickGoal" ends up being -2,473, causing the program to skip the while() loop and go straight to the stop command at the end of the function.
Normally, we would deal with this issue by changing
int tickGoal = (42 * tenthsOfIn) / 10; |
to
long tickGoal = (42 * tenthsOfIn) / 10; |
Thereby making "tickGoal" a 'long' data type, which has a positive limitation of 2,147,483,647, a value that is more than enough for our purposes (and would in fact allow you to program your robot to go forward for several hundred miles in one function!)
Unfortunately, the Arduino UNO does not have enough memory to allow it to process 'long' type data, and if you try to include one in your program, the compiler will pick it up and refuse to compile your program. (Note that anything that results in a number over 32,767 or below -32,768 will cause this to occur as well. Try replacing "tickGoal" in the while() statement with the number 40,000 and see what happens.)
So now we're going to have to get creative. What we can do to circumvent the numerical limit on the integer type is create a counting while() loop.
void driveStraightDistance(int tenthsOfIn, int masterPower) { int inchGoal = tenthsOfIn / 10; /*Initialise slavePower as masterPower - 2 so we don't get a huge error for the first few iterations. The -2 value is based off a rough guess of how much the motors are different, which prevents the robot from veering off course at the start of the function. */ int slavePower = masterPower - 2; int error = 0; int kp = 5; //These three values should NOT be reset each iteration in order for the power corrections to work. int i = 0; //initialize the counting variable (by tradition, 'i') so it can be used in the while loop while (i < inchGoal) { //This will count up the total encoder ticks despite the fact that the encoders are constantly reset. int totalTicks = 0; //unlike the other values, totalTicks needs to be reset each iteration SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset. while(abs(totalTicks) < 102) /*note that we have changed "tickGoal" to the amount of ticks in an inch. This is because every iteration in the count is telling the robot to go 1 inch further*/ { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); //Add this iteration's encoder values to totalTicks. totalTicks+= SensorValue[leftEncoder]; } i++; } motor[leftServo] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks. motor[rightServo] = 0; } |
Recall that the ++ modifier increases the value of the variable by one. So this program causes the robot to drive one inch each iteration, increasing the count every time an inch passes until it's reached the inputted number of inches, all while adjusting the motors to drive in a straight line. Thus, no value ever has to get anywhere near the 32,767 limit.
Using a for() Loop
We can make this program even simpler by using our old friend the for() loop. If you remember, the for() loop works in a similar fashion to the while() loop, but allows you to put two extra conditions on it that change the execution of the loop. The first is a variable definition, in this case int i=0;, which is only executed once. This variable is usually used in the main condition. The second condition is the usual while() loop esque condition i < 30 The last is a command to be executed at the end of each iteration (i++). This is ideal for a counting loop because you can put the counting variable and the count command into the condition. As we know, the ++ modifier increases whatever it modifies by 1. Thus i++ causes the counter, i, to go up one every iteration of the program.
The basic form of the for() loop is as follows:
for (/*variable definition*/; /*condition*/;/*action at the end of each iteration*/) { //loop body commands } |
So for example,
int i=0 /*variable definition*/ while (i < 30 /*condition*/) { //loop body commands i++; /*action at the end of each iteration*/ } |
becomes
for (i = 0/*variable definition*/; i < 30 /*condition*/; i++ /*action at the end of each iteration*/) { //loop body commands } |
Or without the notes:
for (i=0; i < 30; i++) { //loop body commands } |
See how easy that is? the for() loop is another good way to make your code easier to understand. So, if we take our old function and replace the while() command with a for(), we get something like this:
void driveStraightDistance(int tenthsOfIn, int masterPower) { int inchGoal = tenthsOfIn / 10; /*Initialise slavePower as masterPower - 2 so we don't get a huge error for the first few iterations. The -2 value is based off a rough guess of how much the motors are different, which prevents the robot from veering off course at the start of the function. */ int slavePower = masterPower - 2; int error = 0; int kp = 5; //These three values should NOT be reset each iteration in order for the power corrections to work. for (i=0, i < inchGoal,i++) /*this will keep looping the 1 inch driving iterations until the robot has driven the inputted amount*/ { //This will count up the total encoder ticks despite the fact that the encoders are constantly reset. int totalTicks = 0; //unlike the other values, totalTicks needs to be reset each iteration SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset. while(abs(totalTicks) < 102) /*note that we have changed "tickGoal" to the amount of ticks in an inch. This is because evey iteration in the count is telling the robot to go 1 inch further*/ { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); //Add this iteration's encoder values to totalTicks. totalTicks+= SensorValue[leftEncoder]; } } motor[leftServo] = 0; // Stop the motors once the count has reached the correct number of inches. motor[rightServo] = 0; } |
Unfortunately, we have lost some of our fine control in the process, as our count is in inches, not in tenths of an inch as before. Not to fear, however, there is an easy way to remedy this by way of the % operator.
The % operator is used in place of a division slash (/), and returns the remainder of that division. For example, 9 % 4 would return 1, and 18 % 5 would return 3. So, what we can do to put the tenths of an inch back into the function is take tenthsOfIn % 10, then use that value (which we will define as inchGoalRemainder) to do one last while loop using the remainder to determine the number of ticks.
void driveStraightDistance(int tenthsOfIn, int masterPower) { int inchGoal = tenthsOfIn / 10; //determine the number of inches to go int inchGoalRemainder = tenthsOfIn % 10 //determine the number of tenths of inches to go /*Initialise slavePower as masterPower - 2 so we don't get a huge error for the first few iterations. The -2 value is based off a rough guess of how much the motors are different, which prevents the robot from veering off course at the start of the function. */ int slavePower = masterPower - 2; int error = 0; int kp = 5; //These three values should NOT be reset each iteration in order for the power corrections to work. for (int i=0; i < inchGoal; i++) /*this will keep looping the 1 inch drving iterations until the robot has driven the inputted amount*/ { //This will count up the total encoder ticks despite the fact that the encoders are constantly reset. int totalTicks = 0; //unlike the other values, totalTicks needs to be reset each iteration SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset. while(abs(totalTicks) < 102) /*note that we have changed "tickGoal" to the amount of ticks in an inch. This is because evey iteration in the count is telling the robot to go 1 inch further*/ { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); //Add this iteration's encoder values to totalTicks. totalTicks+= SensorValue[leftEncoder]; } } int totalTicks = 0 //reset the total ticks again for the last loop int tenthsOfInGoal = inchGoalRemainder * 102 / 10 //caluclating the number of ticks to the tenths of an inch while (abs(totalTicks) < tenthsOfInGoal) // a copy of the previous proportional control, with the tenths of inch being the goal { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); //Add this iteration's encoder values to totalTicks. totalTicks+= SensorValue[leftEncoder]; } motor[leftServo] = 0; // Stop the motors once the count has reached the correct number of inches. motor[rightServo] = 0; } |
And that is our final function. We now have the ability to easily program the robot to drive any number of inches while adjusting to move in a straight line throughout the distance. Let's use this in a program where the robot should eventually end up back at its starting point.
#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 driveStraightDistance(int tenthsOfIn, int masterPower) { int inchGoal = tenthsOfIn / 10; //determine the number of inches to go int inchGoalRemainder = tenthsOfIn % 10 //determine the number of tenths of inches to go /*Initialise slavePower as masterPower - 2 so we don't get a huge error for the first few iterations. The -2 value is based off a rough guess of how much the motors are different, which prevents the robot from veering off course at the start of the function. */ int slavePower = masterPower - 2; int error = 0; int kp = 5; //These three values should NOT be reset each iteration in order for the power corrections to work. for (int i=0; i < inchGoal; i++) /*this will keep looping the 1 inch drving iterations until the robot has driven the inputted amount*/ { //This will count up the total encoder ticks despite the fact that the encoders are constantly reset. int totalTicks = 0; //unlike the other values, totalTicks needs to be reset each iteration SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; //Monitor 'totalTicks', instead of the values of the encoders which are constantly reset. while(abs(totalTicks) < 102) /*note that we have changed "tickGoal" to the amount of ticks in an inch. This is because evey iteration in the count is telling the robot to go 1 inch further*/ { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); //Add this iteration's encoder values to totalTicks. totalTicks+= SensorValue[leftEncoder]; } } int totalTicks = 0 //reset the total ticks again for the last loop int tenthsOfInGoal = inchGoalRemainder * 102 / 10 //caluclating the number of ticks to the tenths of an inch while (abs(totalTicks) < tenthsOfInGoal) // a copy of the previous proportional control, with the tenths of inch being the goal { //Proportional algorithm to keep the robot going straight. motor[leftServo] = masterPower; motor[rightServo] = slavePower; error = SensorValue[leftEncoder] - SensorValue[rightEncoder]; slavePower += error / kp; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0; wait1Msec(100); //Add this iteration's encoder values to totalTicks. totalTicks+= SensorValue[leftEncoder]; } motor[leftServo] = 0; // Stop the motors once the count has reached the correct number of inches. motor[rightServo] = 0; } task main() { //Distances specified in tenths of an inch. driveStraightDistance(400,100); wait1Msec(500); //Stop in between to prevent momentum causing wheel skid. driveStraightDistance(245,-100); wait1Msec(500); driveStraightDistance(155,100); wait1Msec(500); driveStraightDistance(310,-100); } |
This combination of encoder-based distance measurement and proportional control should become your primary straight line driving function, as it is by far the most accurate and reliable method available to you.
