Drive Distance

As we mentioned in the previous section, we rarely need to drive for a period of time.  Usually what we really want to do is drive a specific distance.  To accomplish this feat we will use a device that is attache to each wheel called a shaft encoder.  What this device does is measures the amount by which the wheel turns.  Since the number of turns of the wheel is directly related to the distance the robot move, this gives us the ability to measure distance.

For this exercise we will start with the example program SimpleRobotDrive:

#include <Servo.h>
#include  <MsTimer2.h>

#include <SimpleRobot.h>

class MyRobot : public Robot
{
public:
    RobotMotor  *  m_pRightMotor;
    RobotMotor  *  m_pLeftMotor;
    unsigned long  m_time;
    
    void Setup()
    {
      Serial.println("Setup");
      
      m_pLeftMotor  = new RobotYellowMotor(1);
      m_pRightMotor = new RobotYellowMotor(0);
      
      m_pLeftMotor->SetPower(0.75);
      m_pRightMotor->SetPower(0.75);
      
      m_time  = millis();
    }
    
    void Loop()
    {
      if ((millis() - m_time) > 3000)
      {
        m_pLeftMotor->SetPower(0);
        m_pRightMotor->SetPower(0);
      }
    }
    
    void Terminate()
    {
    }
} MyRobot;

Robot    *    g_pRobot    = &MyRobot;

If you recall, this is the program we wrote that drives the robot forward for 3 seconds.  What we want to do now, is change it so that the robot drives a fixed distance.

To do this we will need to access the encoders that are attached to each wheel. Fortunately, there is a C++ class for this called RobotEncoder.

The first thing we must do is create a variable to hold a pointer to an instance of the RobotEncoder class.  For this exercise, we will be only using the encoder attached to the right wheel, but since we will be needing both encoders later, we will add declarations for both. Add the following lines just after the declaration of m_pLeftMotor:

    RobotEncoder  *  m_pLeftEncoder;
    RobotEncoder  *  m_pRightEncoder;

We must now initialize these two new variable in the Setup function.  We consult the documentation to find the appropriate constructor to use:

To create new instances of the RobotEncoder class for the left and right wheels, we need to add the following two lines right after the initialization of the right motor variable m_pRightMotor.

      m_pRightEncoder = new RobotEncoder(0);
      m_pLeftEncoder  = new RobotEncoder(1);

We want to measure the amount the wheels turns relative to the starting position. To do this we must also reset their positions to zero.  Once again, consulting the documentation we find a member function to do just that:

Add the following lines just after the lines above:

      m_pRightEncoder->Reset();
      m_pLeftEncoder->Reset();

Finally in the Loop function we want to check for the distance instead of time.  Consulting the RobotEncoder documentation, we find that the function that we need to use is GetPosition:

Note that the value that is returned by the GetPosition function is in arbitrary units which we will need to calibrate if we wish to translate them to real world units such as inches.  For now, however, we will just use the units as they are returned.  For the encoders on these robots, one complete revolution of the wheel will be about 1000 units.  So if we want the robot to move forward by about 3 revolutions we need to change the if statement in the Loop function to read as follows:

      if (m_pRightEncoder->GetPosition() > 3000)
      {
        m_pLeftMotor->SetPower(0);
        m_pRightMotor->SetPower(0);
      }

The complete program should now 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;

Now upload your program to the robot and use the Driver Station to run it.  You should find that the robot moves forward by a short distance and then stops.  One of the advantages to using distance is that you can change the power of the motors, and the robot will drive forward the same distance.  Try this yourself by changing the power from 0.75 to 1.0 in the SetPower calls in the Setup function (remember to change the power for both motors if you want the robot to drive straight).

As we mentioned previously, your robot probably does not drive in a straight line.  In the next section we will learn how to use the wheel encoders to make the robot automatically drive straight.

Next: Drive Straight