Drive Straight
As we have previously discovered, our robot really doesn’t want to drive straight unless we manually adjust the power of the motor to compensate, which is not a very satisfying solution. In this chapter we are going to show you how to use the wheel encoders to automatically adjust the power of the motors so that the robot drives straight all by itself.
Let’s start by loading the SimpleRobotDriveStraight example. This is essentially the program you just finished in the last chapter and should look like:
#include <Servo.h>
#include <MsTimer2.h>
#include <SimpleRobot.h>
class MyRobot : public Robot
{
public:
RobotMotor * m_pRightMotor;
RobotMotor * m_pLeftMotor;
RobotEncoder * m_pLeftEncoder;
RobotEncoder * m_pRightEncoder;
void Setup()
{
Serial.println("Setup");
m_pLeftMotor = new RobotYellowMotor(1);
m_pRightMotor = new RobotYellowMotor(0);
m_pRightEncoder = new RobotEncoder(0);
m_pLeftEncoder = new RobotEncoder(1);
m_pRightEncoder->Reset();
m_pLeftEncoder->Reset();
m_pLeftMotor->SetPower(0.75);
m_pRightMotor->SetPower(0.75);
}
void Loop()
{
if (m_pRightEncoder->GetPosition() > 3000)
{
m_pLeftMotor->SetPower(0);
m_pRightMotor->SetPower(0);
}
}
void Terminate()
{
}
} MyRobot;
Robot * g_pRobot = &MyRobot;
So how do we use the encoders to get the robot to drive straight? Since we have an encoder attached to each wheel, we can read both encoder values. If the robot is driving straight, then the wheels will be turning at the same rate and we would expect that the left and right wheel encoder positions will be the same. And if we take the difference between the left and right values we would expect that difference to be zero. However, if the robot is turning right, that would mean that the left wheel is rotating faster than the right and, thus, the difference between the left and right encoder positions would be a positive number. In this case we would want to subtract power from the right motor and add power to the left motor. Similarly, if the robot is turning left, it would mean that the right motor is running faster than the left motor and the difference between the left and right encoder positions will be negative. In this case we would want to add power to the left motor, and subtract power from the right motor. We can accomplish this by modifying the Loop function as follows:
void Loop()
{
long rightPosition = m_pRightEncoder->GetPosition();
long leftPosition = m_pLeftEncoder->GetPosition();
if (rightPosition >= 3000)
{
m_pLeftMotor->SetPower(0);
m_pRightMotor->SetPower(0);
}
else
{
int diff = (rightPosition - leftPosition);
m_pRightMotor->SetPower(0.75 - diff);
m_pLeftMotor->SetPower(0.75 + diff);
}
}
A couple of things have changed here. First we are now reading the position for both the left and right encoders and storing their values in the local variables leftDistance and rightDistance. We are saving the values in variable because we will use them more than once and only want to call the GetPosition function once for each wheel.
Next we see that we are still using the right wheel position to determine when the robot should stop (i.e. stop when the right position exceeds 3000 units).
However we have added an else case which will be executed as long as the right wheel position has not reached 3000. This is the place where we adjust the power of the left and right motors to keep the robot driving straight.
As we discussed above, if the robot is turning right, then the value of diff (i.e. the difference between the left and right encoders) will be positive. Thus we can see that we will be subtracting a positive number from the right motor and adding a positive number to the left motor, which is what we need to do to straighten it out.
Similarly, if the robot is turning left, the value of diff will be negative. In this case, we will be subtracting a negative value from the right motor power (which will increase its value), and adding a negative value to the left motor power (which will decrease its value). Once again this is what we need to do to correct the turning.
There is one problem with this code, however. The value of diff will be a fairly large number, at least compared to the 0.75 power that we are using. For example, if the encoders differ only by 10 units, we will be setting the right power to -9.25, and the left motor power to +10.75. Since the range for the power is -1 to +1, this is clearly not going to work.
What we need to do is to scale the difference value so that the corrections are more subtle. We do this by multiplying diff by some small fraction. The exact value of the fraction that you will need to use will require some trial and error, but the value of 0.005 is a reasonable starting place.
Change the two SetPower calls in the else case to the following two lines:
m_pRightMotor->SetPower(0.75 - diff * 0.005);
m_pLeftMotor->SetPower(0.75 + diff * 0.005);
Now compile and run the program and the robot should drive reasonably straight. This choice of the scale factor is important. If the number is too large, the robot will over correct and it will weave quickly back and forth. If the number is too small, then the robot might turn a significant amount before the correction takes place. You can play with the size of the number and see the effect.
There is one last change that we are going to make before we consider this program complete. When you write code, you should usually not see numbers like ‘0.75’ and ‘0.005’ explicitly. The problem is that hard coding numbers like this is not very flexible, and also leads to problems when you need to change the values. For example if you have the number ‘0.005’ in multiple places, you need to remember to change every instance of them, or you will have problems.
A better solution would be to either use a defined constant (if the numbers never need to change at run-time), or a variable if you want the flexibility of changing them dynamically. In this case we choose to use variables, one for the power and one for the scale factor. First we need to declare variables to hold these values. Add the following lines to the variable declarations at the beginning of your MyRobot class (right after the delcaration of m_pRightEncoder).
float m_scaleFactor = 0.005;
float m_power = 0.75;
Here we declare the variables and initialize them to the numbers we are currently using.
Finally, change the SetPower lines in the Loop function as follows:
#include <Servo.h>
#include <MsTimer2.h>
#include <SimpleRobot.h>
class MyRobot : public Robot
{
public:
RobotMotor * m_pRightMotor = 0;
RobotMotor * m_pLeftMotor = 0;
RobotEncoder * m_pLeftEncoder = 0;
RobotEncoder * m_pRightEncoder = 0;
float m_scaleFactor = 0.005;
float m_power = 0.75;
void Setup()
{
Serial.println("Setup");
m_pRightMotor = new RobotYellowMotor(0);
m_pLeftMotor = new RobotYellowMotor(1);
m_pRightEncoder = new RobotEncoder(0);
m_pLeftEncoder = new RobotEncoder(1);
m_pLeftMotor->SetPower(m_power);
m_pRightMotor->SetPower(m_power);
m_pLeftEncoder->SetInvert(true);
m_pRightEncoder->Reset();
m_pLeftEncoder->Reset();
}
void Loop()
{
long rightDistance = m_pRightEncoder->GetPosition();
long leftDistance = m_pLeftEncoder->GetPosition();
if (m_pRightEncoder->GetPosition() >= 3000)
{
m_pLeftMotor->SetPower(0);
m_pRightMotor->SetPower(0);
}
else
{
int diff = (rightDistance - leftDistance);
m_pRightMotor->SetPower(m_power - diff * m_scaleFactor);
m_pLeftMotor->SetPower(m_power + diff * m_scaleFactor);
}
}
void Terminate()
{
}
} MyRobot;
Robot * g_pRobot = &MyRobot;
Now we can control both the power and scale factor by changing the values we use to initialize those variables. It also gives us the flexibility to change these values at run-time, in response to some external event (e.g. the position of a joystick).
In the next chapter we will learn how to measure our distance using real world units (e.g. inches) instead of the arbitrary tick counts provided by the encoders.