Drive Inches

So far, we have been using the raw encoder values to measure how far the robot has moved.  While this works, it might be more convenient to be able to specify the distance in real world units (e.g. inches).  In this chapter we will calibrate the encoders so that we can do just that.

Let’s start by loading the ‘SimpleRobotDriveStraight’ example:

#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;

This should be the same program that we created in the last chapter.

What we want to do is to create a function which will return the distance in real world units, in this case inches.  To do that we must first determine the scale factor the converts the number of encoder ticks into inches.

Download this program to your robot and place the robot on the floor.  Now use a piece of tape to mark the starting position of the robot and then run the program.  After the robot has moved, use a tape measure to measure the distance the robot has moved.  You will probably find that it has moved about 24 inches.  This means that 3000 ticks is the same as 24 inches.  Since we will be reading the encoder value which returns ticks, what number do we need to multiply it by to get inches.  The answer is as follows:

#ticks * 24 inches / 3000 ticks

That is, we need to multiply the number of ticks returned by the encoder by 24/3000 (i.e. 0.008) to get the number of inches.

Now we could do this multiplication every place that we needed to get the distance, but it is better if we create a function to do this for us.  Add the following function right before the Loop function in your program:

    float GetDistanceInches(RobotEncoder * pEncoder)
    {
        return(pEncoder->GetPosition() * 0.008);
    }

Notice that we are passing an pointer to one of the encoders into this function.  This is because this function will need to read the encoder to get the number of ticks before it can convert them to inches.  Also notice that our function is returning a float rather than an int.  This is because we would like to measure distances that are fractions of an inch (e.g. 4.7 inches) as opposed to only integer distances (e.g. 4 inches).

We now need to change our Loop code to use this new functions.  Let’s say that we want the robot to drive forward by 20 inches.  To do this we change the following line:

        if (m_pRightEncoder->GetPosition() >= 3000)

to

        if (GetDistanceInches(m_pRightEncoder) >= 20.0)

Instead of calling the GetPosition function of the encoder directly, we call our newly created function.  Note that we are passing in a pointer to the right encoder so that our GetDistanceInches function has access to it.  Choosing to use the right encoder is somewhat arbitrary. Since the robot is driving straight, we could have easily used the left encoder instead.

Now if we download and run the program, and measure the distance, we should see the the robot moves 20 inches.

Before we move on, we should replace the explicit use of 0.008 with a constant that we define at the beginning of the class.  Honestly, it does not make a huge difference here since the 0.008 is only used once, but it is more convenient to have all of our constants in one place so that it will be easy to adjust them later if we need to.  It is also useful to get into the habit on not having arbitrary numbers (like 0.008) scattered throughout your code.

First we must create a new variable m_scale which will hold our value of 0.008. Add the following line to the variable declaration section at the beginning of your MyRobot class, just after the declaration of m_power.

    float            m_scale         = 0.008;

We make this variable of type float (because it is a fraction and cannot be represented as an integer), and we initialize it to the desired scale factor of 0.008.  The only thing left is to change the GetDistanceInches function to use this new value as follows:

    float GetDistanceInches(RobotEncoder * pEncoder)
    {
        return(pEncoder->GetPosition() * m_scale);
    }

Your final program should then be:

#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_adjustFactor  = 0.0025;
    float            m_power         = 0.75;
    float            m_scale         = 0.008;
    
    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();
    }
    
    float GetDistanceInches(RobotEncoder * pEncoder)
    {
        return(pEncoder->GetPosition() * m_scale);
    }
    
    void Loop()
    {    
        long rightDistance = m_pRightEncoder->GetPosition();
        long leftDistance = m_pLeftEncoder->GetPosition();
        
        if (GetDistanceInches(m_pRightEncoder) >= 20.0)
        {
            m_pLeftMotor->SetPower(0);
            m_pRightMotor->SetPower(0);
        }
        else
        {
            int diff = (rightDistance - leftDistance);
            
            m_pRightMotor->SetPower(m_power - diff * m_adjustFactor);
            m_pLeftMotor->SetPower(m_power + diff * m_adjustFactor);
        }
    }
    
    void Terminate()
    {
    }
} MyRobot;

Robot    *    g_pRobot    = &MyRobot;

Now compile and download this program, and it should, of course, behave as it did before.

Next: