Drive Forward

In this section, we will program the robot to drive forward for two seconds.  Once again, we will start by loading the example program SimpleRobotBlank:

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

#include <SimpleRobot.h>

class MyRobot : public Robot
{
public:
    void Setup()
    {
    }
    
    void Loop()
    {
    }
    
    void Terminate()
    {
    }
} MyRobot;

Robot    *    g_pRobot    = &MyRobot;

The C++ class to control the motors on the robot is called RobotYellowMotor which is based on the RobotMotor class.  Since we will need access to these motors throughout our program, we need to declare some member variables to hold the two instances we will need.

Add the following lines to your program just before the public: declaration:

    RobotMotor  *  m_pRightMotor;
    RobotMotor  *  m_pLeftMotor;

Note that we have declared these a pointers to the RobotMotor class.  Remember that declaring variables does not initialize them.  We need to do that in our Setup function.  Also note the naming conventions. Each of the variable’s name starts with the prefix m_. This is just to serve as a reminder, when we see it used in our code, that this is a member variable for the class. Secondly, the p the precedes the name server as reminder that these are pointers to the particular class.  Note that C++ does not require these conventions, but it is a good habit to get into.

In order to create new instances of the motor class we will need to consult the documentation to see how to call the constructors.  Since we are using the yellow motors on this robot, we need to look up the documentation for the  RobotYellowMotor:

Here we see that we have two possible constructors to choose from.  Each motor requires two Arduino pins for control.  The second constructor allows us to use any arbitrary two pins.  However the first constructor allows us to select on of two motors that are wired to the standard pins 4, 5, 6, and 7.  Since this is the way our robot is wired, we will use the first constructor.  Note that for our current wiring, the left motor is motor 1, and the right motor is motor 0.

Now add the following lines to the Setup function:

      m_pLeftMotor  = new RobotYellowMotor(1);
      m_pRightMotor = new RobotYellowMotor(0);

These two lines create two new instances of the RobotYellowMotor class, one for the left motor and one for the right motor.

Now that we have instances of the class that we need to control the motors, we need to figure out how to turn the motors on.  Once again, consulting the documentation for the RobotYellowMotor class, we find the following member function:

So if we want to turn both motors on at, say, 75% power, we need to add these two lines to the Setup function, just after the creation of the class instances:

      m_pLeftMotor->SetPower(0.75);
      m_pRightMotor->SetPower(0.75);

Now if we were to upload and run this program as is, we would find that the robot would start driving forward and never stop. This is because, although we turn the motors on, we never turn them off.

Since we want to stop the motors after 2 seconds, we will need a way to measure the elapsed time.  The Arduino has a function called millis() which will return the time, in milliseconds (1/1000 th of a second).  However this value gives us the total number of milliseconds since the Arduino was last reset (which happens each time we upload a new program, or connect to the robot using the Driver Station. What we need, however is a way to measure the elapsed time from when we first start the motors, until we need to turn them off.  Do do this, we will store the current time right after we start the motors, and then we can later compute the elapsed time by getting the current time and subtracting the saved start time.

In order to save the time, we will need a new member variable to hold its value so that we will have access to it later.

Add the following line to the list of member variables, right after the declaration of the left motor:

    unsigned long  m_time;

Note that we are using an unsigned long variable here.  This is because on an Arduino the size if int variables is 16 bits which would represents a maximum of  32,767 milliseconds, which is just over 32 seconds.  Since it is likely we will be running our robot longer than that, we use a long instead.  Also, since the time will never be negative, we can use an unsigned long to give us even a bit more time (i.e. 4,294,967,295 milliseconds, or over 4 million seconds).

Once again, declaring the variable m_time does not initialize it.  This we must do in the Setup function.  Add the following line to the Setup function right after the lines where you turn the motors on:

      m_time  = millis();

This line gets the current time and stores it in the member variable m_time. Note that since m_time is a member variable, it will persist as long as our class exists. This means that we can use this value in other functions within the class.

Now how do we turn the motors off after two seconds have past? The place to do this is in the Loop function. Remember that the Loop function gets called repeatedly once the program starts running.  So what we need to do is check the elapsed time, and when it exceeds two seconds, turn the motors off.

Add the following code to the Loop function:

      unsigned long elapsedTime = millis() - m_time;
      
      if (elapsedTime > 2000)
      {
        m_pLeftMotor->SetPower(0);
        m_pRightMotor->SetPower(0);
      }

Let’s take a look at what these lines do:

The first line computes the time that has elapsed since we started the motors.  This is equal to the current minus the time at which we started the motors which was saved in the member variable m_time.

On line 3, we compare the elapsed time to 2000 milliseconds (i.e. 2 seconds), and if the elapsed time exceeds 2 seconds, we then set the power on both motors to zero (lines 5 & 6).

At this point, your finished program should look like:

#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()
    {
      unsigned long elapsedTime = millis() - m_time;
      
      if (elapsedTime > 2000)
      {
        m_pLeftMotor->SetPower(0);
        m_pRightMotor->SetPower(0);
      }
    }
    
    void Terminate()
    {
    }
} MyRobot;

Robot    *    g_pRobot    = &MyRobot;

Upload your program to the robot and use the Driver Station  to run it.  You robot should drive forward for two seconds and then stop.

One of the things you might note is that your robot does not drive straight.  This is because one of the motors is slightly more powerful than the other.  If the robot is turning right, it means that the left motor is running faster than the right motor.  Similarly, if the robot is turning left, then the right motor is running faster than the left.  So observe which way your robot is turning and adjust the power of the motors to try and make it drive straight.  For example if the robot is turning left, try increasing the power of the left motor from 0.75 to 0.85.  By playing with the motor powers you should be able to get the robot to drive fairly straight.  Since manually adjusting the power of the motors is obviously not an ideal solution, later we will show how to have the robot automatically adjust the power of the motors to keep it driving straight.

In the next section, we will look at another way to time things.

Next: Timer Class