2. Creating your First Mode

RobotUtils allows to quickly switch between different robot operation modes. For example, we might want to start the robot in an idle mode before the game start to make sure it doesn't move before the game begins. When we press a button, we would then switch to a different mode which allows to control the robot. Another use of such modes would be to allow for different ways to control the robot on the field. For instance, the robot might be faster in a tank mode, but more precise in holonomic mode. Modes are particularly useful in this case since they can be changed on the fly. These modes have their own input manager and access to different sets of actuators.

Creating a Mode

Modes work with an object-oriented concept called "polymorphism". When a class is inherited from another parent class (which is what we will do with mode), it keeps that top class's member variables and methods. However, an external part of the code could still refer to that new class as the parent class. For example, take two classes, Triangle and Rectangle, which both inherit from a common Shape class. An external class coudl refer to a Triangle or Rectangle object as a Shape and ask to calculate it's area, since all shapes have an area. However, the way that area is measured depends on the actual shape we are using.

In the case of RobotUtils, we have a parent class called Mode which has three methods that all child classes must implement: update(), load() and unload(). These will define the behaviour of our robot when that mode is activated (or deactivated). We can think as update() and load() as being the equivalent of arduino's loop() and setup() functions, respectively. We have to add a third unload() functions since modes might be replaced throughout the robot's execution.

Now, let's get to coding. As said, previously, we have to create a class that will inherit from RobotUtils' Mode class:

#include <RobotUtils.h>
using namespace Crc;

class IdleMode : public rou::Mode
{
public:
    // ...
};

In C++, we inherit (publicly) from a class by adding : public <parent name> next to the class name. All of the code in RobotUtils is contained in its own namespace (rou), so all the code coming from the library will be prefixed with rou::. You write using namespace rou; just after including the header if you don't want to type it every time.

Then, we want to add functionality to our mode. To do that, we'll define the functions that were mentionned previously, right after the public keyword from the previous screenshot:

    void load() override
    {
        Serial.println("Idle mode loaded.");
    }

    void unload() override
    {
        Serial.println("Idle mode unloaded.");
    }

    void update(float dt) override
    {
        Serial.println("Idle mode updated.");
    }

This will simply print out different messages to the Serial console depending on the state of the IdleMode. The override keywords are not mandatory but are VERY strongly recommended, as they will yield a compilation error if you try to use them on functions that are not present in parent classes. This can save a lot of time if you made a typo in the name of any of the functions.

If you try compiling the code now, you will notice that nothing will happen. This is because we haven't setup the ModeManager yet.

Setting up the ModeManager

To handle all the modes and update the appropriate one, we need to create a ModeManager object. As the name suggests, it handles all the various modes the robot has. After declaring all our Mode classes (we can add as many as we want, but we'll add an other one later in this tutorial), we can add the following lines:

IdleMode idleMode;
rou::ModeManager modeManager;
rou::ModeManager& rou::Mode::ModeManager = modeManager;

Careful! These are all written before the start() function, which we will get to very shortly. The first two lines are pretty straightforward, we create our custom mode object as well as the modeManager object. We will create all the objects for the modes that we will use there.

The third line gives all the modes a reference to the mode manager. This will allow them to request changes in modes when, for example, a button is pressed. Since it is common to all modes, the mode manager is declared as static, which makes this line look quite complicated. We're one step closer to making it work. Now, let's look at what we have to add to the Arduino functions.

Making it all work!

We can add the following code to the setup() and loop() functions:

void setup()
{
    Serial.begin(9600);

    CrcLib::Initialize();

    modeManager.changeMode(&idleMode);
}

void loop()
{
    CrcLib::Update();

    modeManager.update(1.f);
}

Since we are using Serial communication in IdleMode, we will initialize it here. Then, we have the standard CrcLib functions in both functions to ensure the CrcDuino works properly. Finaly, the changeMode() function tells the modeManager which mode to start with. The update() function will update the mode that is currently active and switch to the next mode (if there is one) at the very end of the update. For now, we pass in 1.f to the dt parameter of the update function as a placeholder. This value should be the amount of time that has passed since the last time the update functions was called. We will look into how to pass in a correct value in the Setting up a clock section of this tutorial.

If you compile, you should see the message in the load() method of our custom mode briefly before seeing the update() message repeatedly. Since we do not switch to another mode, we will not see the unload() message. Now, let's see how we can change modes.

Switching between modes

Let's start by creating a new mode for moving a robot with a Holonomic drivetrain:

class HolonomicMode : public rou::Mode
{
public:
    void load() override
    {
        Serial.println("Let's go!");
    }

    void unload() override
    {
    }

    void update(float dt) override
    {
        Serial.println("Moving robot.");
    }
};

This is very similar to what we've done for the IdleMode. We'll also update our IdleMode so it can switch to this new mode after its first update:

class IdleMode : public rou::Mode
{
public:
    static rou::Mode* StartingMode;

    // ...

    void update(float dt) override
    {
        Serial.println("Idle mode updated.");

        rou::Mode::ModeManager.changeMode(StartingMode);
    }
};

The load() and unload() functions were omitted because they do not need to be changed. The new startingMode member variable contains a pointer to the mode that will be loaded when the robot exits the idle state. It is static to make future tutorials a little simpler, but it doesn't have to be. The new line added to the update() method calls the changeMode() method of the modeManager, which can be accessed from here since it is a static member of the Mode class. Note that it takes a pointer to a Mode, this is the power of polymorphism!

Now, we have to create an object for this new mode and set the idleMode's new startinMode member variable. We do this before the setup() function:

IdleMode idleMode;
HolonomicMode holonomicMode;
rou::Mode* IdleMode::StartingMode = &holonomicMode;

rou::ModeManager modeManager;
rou::ModeManager& rou::Mode::ModeManager = modeManager;

We just need to add two lines here: one to create the mode and the other to change the starting mode! Again, setting the starting mode looks complicated because it is a static member variable. If it weren't static, we would have to write idleMode.startingMode = &holonomicMode; in the setup() function. Now there's one last thing missing: to give an correct value to the dt parameter of the update() method of the ModeManager.

Setting up a clock

To create a clock that will keep track of the time between two updates, we first need a variable that will contain the time of the last update. We can declare it right before the setup() function:

unsigned long lastUpdateTime = 0;

We will store this as an integer containing the amount of milliseconds elapsed between the preivous update and the start of our program. We will first update that value at the very end of the setup() function. This will ensure we do not have a huge dt for the first update due to the initialization of our program.

void setup()
{
    //...

    lastUpdateTime = millis();
}

Finaly we will calculate the actual change in time in the loop() function, right below the call to CrcLib::Update():

    unsigned long currentTime = millis();
    float dt = static_cast<float>(currentTime - lastUpdateTime) / 1000.f;
    lastUpdateTime = currentTime;

    modeManager.update(dt);

We first get the time of the current update, then calculate the difference in time. We cast the change into a float before converting it to seconds. Before passing the dt value to the modeManager's update() method, we store the current time in lastUpdateTime for the next update.

That's it! You're now able to switch between different modes. In the next tutorial, we'll get the robot to move. You might also notice that the parent mode class has a Controller member variable. This is used to manage inputs, which is a topic we will cover in the Setting up the Inputs tutorial