Building an Arduino Robot, Part IV: A (Not So) Basic Robot Firmware

Welcome to the fourth article in the tutorial series in which I'm building a remote controlled Arduino based vehicle robot.

Here is the list of articles I have published:

• Part I: Hardware Components
• Part II: Programming the Arduino
• Part III: Assembling the Robot
• Part IV: A (Not So) Basic Robot Firmware (this article)
• Part V: Avoiding Obstacles
• Part VI: Remote Control

In the previous article I completed the assembly of my robot vehicle. Today, I'm going to write an Arduino sketch for it that makes the robot go forward.

"Just to move the robot forward?", you may ask. What's so interesting about simply making the robot run forward?

Well, the idea is to not only make the robot move forward, but to write the sketch in such a way that allows me to later add more complex functionality without having to start over from scratch. I'm basically laying out a solid foundation on which I will stack the more complex pieces later.

Once again I want to say that it is not my intention to do a full and detailed coverage of the C++ programming language in these articles. I will share my experience and provide a few pointers, but I recommend you read up on C++ on your own if you have no experience with the language.

Goals

I mentioned what functionality I want my robot to have in past articles, but I did not discuss the goals I have for my project from a software engineering point of view.

Scalable Structure

For simple sketches the default sketch structure based on the setup() and loop() functions is sufficient. The sketches I wrote a couple of articles ago to test all the components of my robot fit in this category, I needed to test and learn to program these components and was able to accomplish both goals quickly and with very low effort.

The problem is that this structure does not scale well. For my robot project I will need to control two motors, the distance sensor and react to remote commands coming over bluetooth all at the same time. In the future I may want to extend it with even more controlled devices. Maybe additional distance sensors on the sides and back, or one looking down to detect when the vehicle reaches the end of a table. I may want to mount a wireless camera, or add sound capabilities. The possibilities are endless, and each new device added will increase the complexity of the sketch.

The loop() function would need to get pretty long and complex to do everything, and then making changes to it would become a nightmare, with any changes I make I could risk inadvertently breaking existing functionality. The sketch would end up being spaghetti code.

For this project I will use object oriented techniques to organize the code of my sketch and make it easier to write, test, maintain and modify.

Device Abstractions

I mentioned before that I will be releasing my sketch as open source. It is safe to assume that other people will build their own robot vehicles using different parts than those I put in mine. For example, someone may use a different motor driver board, or may want to use wi-fi for the remote control instead of bluetooth. If I build my sketch with the assumption that the components of the robot are just those that I'm using on mine then only a small few would be able to benefit right away and most will have to hack on it to make it work for them.

So instead, I will think about the functionality I want to implement in general terms, and then will write device drivers for the modules that I need.

This will give me or others the freedom to replace parts in the robot without having to modify the main sketch. Switching from bluetooth to wi-fi will then just require writing a new remote communications driver. The main sketch will talk to either the bluetooth or wi-fi drivers and will not know the difference!

Configuration

I spoke about configuration before. The idea here is that it should be easy to configure the robot software for the actual hardware. The best example is the use of pins in the Arduino board. I will not hard code any pin numbers. Instead, I will add #define constants at the top of the sketch with all the pins, so that changing the pin assignments becomes a trivial task.

But configuration is not limited to pin numbers. There are also some operational defaults (like the distance to a wall or obstacle when the robot stops and turns) that can also be considered part of the configuration and will be represented by constants at the top of the sketch for users to easily modify.

Smarter Timing

In simple sketches one tends to overuse the delay() function as an easy way to make actions last a certain amount of time. I'm guilty of this myself, as I've used delay() in all my test sketches.

But for a real project that needs to do many things at once this is impractical. For example, what happens if the robot approaches a wall while in the middle of a delay() call? I can tell you what happens, nothing happens! The robot will crash into the wall without having a chance to "see" the wall and act accordingly.

Unfortunately the Arduino board does not support multithreading, which is the ideal way to make a program do many things at once. In a multithreaded environment a thread calling delay() would not prevent other threads from running. But on the Arduino platform there is a single thread of execution, so calling delay() makes the entire program go to sleep, and that is not good for my needs.

So I will not be using delay() for anything except throw away tests. But how can I keep track of time without delay()?

Part of the secret is in making the loop() function do only a small amount of work and return. The loop() function will then get called at a really fast rate (remember that it gets called repeatedly until the board is powered off). Each time loop() is called I can find out what's the up time. Knowing the current time will enable me to implement my own non-blocking delay() equivalent, as you will see soon.

Enough talk... let's write a device driver!

I said earlier that today I'm making the robot move forward, so I'm going to start by writing a device driver that knows how to control motors.

A device driver is simply an abstraction for the intended device. I'm now writing one for a motor, so the basic operation that this driver should know how to do is to set the speed of the motor.

I will store my device drivers in header files, which are files that have a .h extension. For small modules using a header file is a good choice. For larger modules the norm is to store the code in a source file with extension .cpp and then write a small header file that only contains the declaration of the public functions from the source file. Header files work for me at this time, if in the future a module grows to a decent size I can always move the code to a .cpp file.

The intention is that the motor device driver will allow the main sketch to work with different motor controllers using the same function calls, so the first task when defining a driver is to decide what the common functions of the driver will be.

Here is the code for my motor device driver, which I will store in a file called motor_driver.h:

/**
 * @file motor_driver.h
 * @brief Motor device driver definition for the Michelino robot.
 * @author Miguel Grinberg
 */

namespace Michelino
{
    class MotorDriver
    {
    public:
        /**
         * @brief Change the speed of the motor.
         * @param speed The new speed of the motor.
         *  Valid values are between -255 and 255. 
         *  Use positive values to run the motor forward, 
         *  negative values to run it backward and zero to stop the motor.
         */
        virtual void setSpeed(int speed) = 0;

        /**
         * @brief Return the current speed of the motor.
         * @return The current speed of the motor with range -255 to 255.
         */
        virtual int getSpeed() const = 0;            
    };
};

If you are unfamiliar with C++ then there are a number of things here that need an explanation.

Any text that appears between /* and */ is considered a comment, so it will be ignored by the C++ compiler. There is an alternative format for comments that you may have seen, those use a // prefix and go through the end of the line. Both formats are valid, but in this case since I'm writing multi-line comments I'm using the /* ... */ format.

The comments that I'm writing follow a standard comment format that can be used to produce automatic documentation with a tool called doxygen. It is always good practice to write class documentation in this format for any classes that others may need to use. This class defines the format for motor drivers, so those who want to implement other motor drivers will find the documentation useful.

I have inserted a comment block at the top to describe the contents of the file, and then one before each function to describe the function. Each comment line is prefixed with a keyword, for example @file for the name of the file, @brief for a brief description, @param for documenting a function parameter and @return for documenting the return value of a function.

The namespace keyword groups all the definitions inside the { and } under a user defined name. This is helpful to avoid name collisions with other modules, so it is always a good idea to enclose your own code in a namespace. I will put all my code under the Michelino namespace.

The class keyword defines a structure that groups data and functions together into an entity called object. My motor driver class is called MotorDriver and just contains two functions, which in the context of a class are called methods. The setSpeed and getSpeed methods will control a motor, but note that unlike the AF_Motor library that I used before here I'm not defining what kind of motor controller is used for the task. The main sketch will just call these methods without knowing how they are implemented, so to switch from one controller to another one just needs to re-implement these two functions for the selected controller.

The public: keyword that appears before the methods indicates that the declarations that follow are accessible to anyone. You will see later that some classes have contents that are hidden from the outside world.

The virtual keyword that starts each method declaration indicates that these methods may be overwritten by a subclass, and the = 0 at the end of the declarations indicates that the methods do not have an implementation. These two attributes combined are used to make sure that the only way to provide an implementation for these methods is in a subclass. I will show what this means in a moment.

Finally, the const keyword that appears in the getSpeed method indicates that this method is constant, meaning that this method does not change the internal state of the object.

Setting the technical details of writing a C++ class aside, the important point is that device drivers that adopt the above interface should be able to control a DC motor with two operations, setSpeed() and getSpeed(). The speed values are given in the -255 to 255 range, with positive speeds moving forward, negative speeds moving backward and zero making the motor stop.

The Adafruit motor shield device driver

Now I have a very clear idea of what motor drivers for my robot need to do, so I'm ready to write a specific implementation for my Adafruit motor shield.

I will store the Adafruit motor shield driver in a file called adafruit_motor_driver.h. Here is the code:

/**
 * @file adafruit_motor_driver.h
 * @brief Motor device driver for the Adafruit motor shield.
 * @author Miguel Grinberg
 */

#include "motor_driver.h"

namespace Michelino
{
    class Motor : public MotorDriver
    {
    public:
        /*
         * @brief Class constructor.
         * @param number the DC motor number to control, from 1 to 4.
         */
        Motor(int number)
            : MotorDriver(), motor(number), currentSpeed(0)
        {
        }

        void setSpeed(int speed)
        {
            currentSpeed = speed;
            if (speed >= 0) {
                motor.setSpeed(speed);
                motor.run(FORWARD);
            }
            else {
                motor.setSpeed(-speed);
                motor.run(BACKWARD);
            }
        }

        int getSpeed() const
        {
            return currentSpeed;
        }

    private:
        AF_DCMotor motor;
        int currentSpeed;
    };
};

So what's new here? I start by including the motor driver header file. The Adafruit driver derives from the generic driver definition I wrote above so that definition needs to be accessible.

The class definition in this file looks a bit different:

class Motor : public MotorDriver

This says that I'm creating a class called Motor that derives from class MotorDriver. This means that class Motor will follow the protocol established by class MotorDriver.

The class has a public section that starts with a constructor:

Motor(int number)

This is a method with the same name as the class (Motor in this case). Constructor methods are called when an object is created and are intended to set the initial state of the object. This constructor takes the motor number as an argument.

The Motor constructor has an initializer list:

: MotorDriver(), motor(number), currentSpeed(0)

This is a list of items that will be initialized when an object of this class is created. The first item just invokes the constructor of the parent class, the generic driver MotorDriver. I did not include a constructor in this class, but the compiler will generate an empty one for me. It is good practice to always initialize the parent class even when there isn't a constructor there, because in the future a constructor might be added.

The two remaining items in the initializer list are member variables of this class. If you look at the bottom of the class there is a private block that defines two variables named motor and currentSpeed. The motor variable is of type AF_DCMotor, the class defined by the Adafruit motor shield library. To initialize this variable I have to invoke its constructor, which if you recall from a previous article takes the motor number as argument. I received the motor number as an argument to my own constructor, so I just pass it on here. The currentSpeed variable is of type int, so I just initialize it to 0 so that it has a known starting value.

The body of the Motor constructor is empty, since the only thing I need is to initialize the member variables.

What follows is the implementation of the setSpeed and getSpeed methods for this specific motor controller, using the open source AF_Motor library from Adafruit, which must be installed separately (see my previous article for instructions). This library requires two functions to be called to configure a motor, one to set the speed and another one to set the mode. I have defined my motor driver to use a simpler way to configure the motor, with just a speed that can be positive, negative or zero. The implementation of setSpeed translates my format to the one required by the Adafruit motor shield library. I also store the current speed in a member variable, so that I can then return it in getSpeed.

At the bottom there are the two private member variables that this class uses. These variables are declared as private because I do not want them to be accessible from outside the object that owns them. I want any changes the motor or its speed to be accomplished via the public functions of the driver (following the encapsulation principle), so a good way to prevent me from accidentally using the member variables directly is to make them private. A private member variable can only be modified from methods that belong to the object, but it is not accessible from any other place in the code. A good reason to hide these member variables is that other motor drivers will need different ones, so as long as the code outside of this class only calls setSpeed() and getSpeed() swapping out this driver for another one should not be a problem.

If you are using a different motor controller than mine on your robot, then all you need to do is copy the adafruit_motor_driver.h file to <your_controller>_motor_driver.h and implement the class as appropriate for your controller. The only requirement is that the new class is also called Motor and has the same two public functions. The two classes will then be conceptually equivalent, so the main sketch can use either one of them.

Using the device driver

Now I have a completed device driver for my Adafruit motor shield. But how do I use it on a sketch?

This is actually simpler than you may think. Here is an example sketch that starts a motor at full speed using the Adafruit driver that I just built:

#include <AFMotor.h>
#include "adafruit_motor_driver.h"
#define MOTOR_INIT 1

Michelino::Motor motor(MOTOR_INIT);

void setup()
{
    motor.setSpeed(255);
}

void loop()
{
}

The #includes at the top bring in the Adafruit motor library and my driver for it.

Note how the motor object is created:

Michelino::Motor motor(MOTOR_INIT);

The notation Michelino::Motor tells the compiler to locate the Motor class inside the Michelino namespace.

Once the motor object is created it can be controlled using the driver functions setSpeed and getSpeed.

Now let's assume that later you want to make the above sketch work with another motor shield, say one that is called "x", for which you have written a device driver. Then you can just add a few lines of code to the sketch, as follows:

// enable one of the motor shields below
//#define ENABLE_ADAFRUIT_MOTOR_DRIVER
#define ENABLE_X_MOTOR_DRIVER

#ifdef ENABLE_ADAFRUIT_MOTOR_DRIVER
#include <AFMotor.h>
#include "adafruit_motor_driver.h"
#define MOTOR_INIT 1
#endif

#ifdef ENABLE_X_MOTOR_DRIVER
#include <XMotor.h>
#include "x_motor_driver.h"
#define MOTOR_INIT 2
#endif

Michelino::Motor motor(MOTOR_INIT);

void setup()
{
    motor.setSpeed(255);
}

void loop()
{
}

At the top I'm defining two #define constants, but one of the is commented out. This is a pretty easy way to provide configuration, you just uncomment the one constant that corresponds to the part that you have.

An #ifdef ... #endif block will only be seen by the compiler if the constant that follows the #ifdef is defined. Since only one of the motor shield constants will be defined I'm effectively loading only one of the device drivers into the sketch.

In the example above the sketch can be switched between the Adafruit shield using motor #1 and the X motor shield using motor #2 just by changing which of the #define statements at the top is used!

The main sketch

Now that I have a really good solution for controlling my hardware parts I need to think about how the main sketch will be structured.

As I mentioned before, having a large piece of code in the loop() function is not ideal because large and complex functions difficult to maintain. If I created a class to represent a motor, then why not also create a class to represent the whole robot?

Here is a sketch that defines a starting point class for the robot. This file will be named michelino.ino (Arduino sketches use the .ino extension):

/**
 * @file michelino.ino
 * @brief Arduino robot vehicle firmware.
 * @author Miguel Grinberg
 */

#define ENABLE_ADAFRUIT_MOTOR_DRIVER

#ifdef ENABLE_ADAFRUIT_MOTOR_DRIVER
#include <AFMotor.h>
#include "adafruit_motor_driver.h"
#define LEFT_MOTOR_INIT 1
#define RIGHT_MOTOR_INIT 3
#endif

namespace Michelino
{
    class Robot
    {
    public:
        /*
         * @brief Class constructor.
         */
        Robot()
            : leftMotor(LEFT_MOTOR_INIT), rightMotor(RIGHT_MOTOR_INIT)
        {
            initialize();
        }

        /*
         * @brief Initialize the robot state.
         */
        void initialize()
        {
            leftMotor.setSpeed(255);
            rightMotor.setSpeed(255);
        }

        /*
         * @brief Update the state of the robot based on input from sensor and remote control.
         *  Must be called repeatedly while the robot is in operation.
         */
        void run()
        {
        }

    private:
        Motor leftMotor;
        Motor rightMotor;
    };
};

Michelino::Robot robot;

void setup()
{
    robot.initialize();
}

void loop()
{
    robot.run();
}

The sketch should not present any surprises. I begin by loading my motor shield driver, using the #ifdef technique I showed before. When and if I write new motor drivers I will add new constants and #ifdef blocks to load them.

My robot class will be appropriately named Robot and will be inside the Michelino namespace. It will initially have two private data members that represent the two motors named leftMotor and rightMotor. These will be initialized in the Robot class constructor's initializer list. The class has two public methods, initialize() and run() that will be the class counterparts to setup() and loop(). In this early version of the class the initialize() method starts the two motors at full speed forward, and run() does nothing.

After the class is defined an instance is created in the global scope, and the setup() and loop() methods simply call the initialize() and run() methods of this object.

What do you think will happen when you run this sketch? The robot will move forward! I took a few turns and twists, but I finally delivered on my promise.

How to implement delays

Before I end this article I'm going to show you how delays can be implemented without actually using the delay() function, so that the robot can do other things while it waits, like receiving and executing remote commands.

To show you how this is done, I'm going to modify the above sketch to make the robot start by waiting for five seconds, then moving forward for eight seconds, then repeating the cycle.

First, I'm going to add the concept of state to the robot. For this example I will have two possible states, stopped and running. To define these states in the sketch I'm going to use a C++ enumeration:

private:
    enum state_t { stateStopped, stateRunning };
    state_t state;

The enum statement creates a user defined enumerated type named state_t. The second statements defines a new member variable named state of that type. The state member variable wil have one of two possible values, stateStopped or stateRunning. I could have easily used an int variable and call state 0 the stopped state and state 1 the running state, but after you see me using this variable I'm sure you will agree with me that using enumerated types makes the code a lot more readable and easier to understand.

I'm going to add yet another member variable to the Robot class that will help me count time:

private:
    unsigned long startTime;

The type of the startTime variable is unsigned long. This is the type in which time units are returned by the function millis() provided by the Arduino software libraries.

Next, in the initialize() method instead of starting the motors I'm now going to initialize the robot state and record the current time:

    void initialize()
    {
        state = stateStopped;
        startTime = millis();
    }

And finally, in the run() method is where all the magic happens:

    void run()
    {
        unsigned long currentTime = millis();
        unsigned long elapsedTime = currentTime - startTime;
        switch (state) {
        case stateStopped:
            if (elapsedTime >= 5000) {
                leftMotor.setSpeed(255);
                rightMotor.setSpeed(255);
                state = stateRunning;
                startTime = currentTime;
            }
            break;
        case stateRunning:
            if (elapsedTime >= 8000) {
                leftMotor.setSpeed(0);
                rightMotor.setSpeed(0);
                state = stateStopped;
                startTime = currentTime;
            }
            break;
        }
    }

The run() method is repeatedly called through the loop() function, so here I can easily check how much time has passed by substracting the startTime that I initialized in initialize() from the current time provided by the millis() function. I'm storing both the current and elapsed time in two local variables that I named currentTime and elapsedTime.

What's left is to check if I'm done waiting, and if I am, then change the state of the robot. Since the wait period is different depending on which state the robot is in, I've decided to use a switch statement, which looks at the value of an expression and then executes the block of code that begins at the matching case statement and ends with the break statement.

So, when the robot is in the stopped state the following code will execute:

            if (elapsedTime >= 5000) {
                leftMotor.setSpeed(255);
                rightMotor.setSpeed(255);
                state = stateRunning;
                startTime = currentTime;
            }

This code is hopefully self-explanatory. If the elapsed time did not reach five seconds then nothing will happen, the run() function will return without making any changes, and a little bit later will be called again. Eventually, the elapsedTime will become 5000 milliseconds or more, so the code inside the if statement will execute. And those statements start the motors, change the state to stateRunning and reset the startTime to the currentTime, so that elapsed time is reset to zero.

The next time run() executes the state will be stateRunning, so the other block in the switch statement will execute:

            if (elapsedTime >= 8000) {
                leftMotor.setSpeed(0);
                rightMotor.setSpeed(0);
                state = stateStopped;
                startTime = currentTime;
            }

And this works exactly like above but waits eight seconds instead of five.

Doing things in this certainly more complicated than using delay():

        void run()
        {
            leftMotor.setSpeed(0);
            rightMotor.setSpeed(0);
            delay(5000);
            leftMotor.setSpeed(255);
            rightMotor.setSpeed(255);
            delay(8000);
         }

But the delay() solution pictured above is limited in that nothing happens during those five and eight second waits. With the elapsed time solution it will be easy for me to make the robot do other things while waiting, like stopping when it gets too close to a wall or responding to remote commands sends over bluetooth.

Final words

To close this article, here is a video that shows my robot running the above michelino sketch:

In the next article I will incorporate the distance sensor into the firmware to prevent the robot from running into walls or other obstacles.

As always, you are welcome to send me your questions in the comments below. If you wish to download or review my sketch I have uploaded it to github as version 0.1 (download or browse).

I hope to see you in the next article!

Miguel