Challenge 2: Knowing Where You Are

Goal

This challenge will walk you through basics of Odometry. This is a common method that robots use to keep track of their position as they move throughout a space.

Setup

Create a folder for your new sketch and an ardMain.cpp inside it. Open this folder in VS Code, and populate your .cpp file with the following skeleton code:

#include <mosscap/challenge2.h>

#include <Encoder.h>
#include <Motor.h>

Motor left(11, 10, 12);
Motor right(21, 20, 22);

Encoder leftEncoder(15, 16);
Encoder rightEncoder(25, 26);

TelemetryPosition pose;

void setup() {}

void loop() {}

You may notice that we now have more than just motors in our sketch. Read on to discover what these new lines mean!

New Hardware: Encoders

Our little differential-drive robot now has encoders attached to its wheels! Encoders are small devices that keep track of how much our wheels have rotated. We can use this information, along with some math, to estimate where our robot is after we move.

These encoders report position in "counts". A full rotation of our wheels is equivalent to 1500 encoder counts for our hardware here. To read this data from our encoders we can use their read() or readAndReset() methods.

• read() Will return the total number of encoder counts since the simulation started.
• readAndReset() will return the number of counts since the last time readAndReset() was called. Note that this will not change the number returned by read().

The second method is useful because we often only want the change in the encoder counts since we last checked. These encoders will be central to our Odometry calculations, but we need some more information to be able to use their data effectively.

New Tool: TelemetryPosition

You may have also noticed that this skeleton code has a TelemetryPosition object included as well. This object is how we will keep track of the position of our robot. The general idea is that we will use the readings from our encoders to update our position a few times per second, we will store our updated position in this pose object.

Our pose has 3 pairs of functions that will be useful in this endeavor:

• getX() & setX(): These allow us to access and modify the $x$ component of the robot's estimated position.
• getY() & setY(): Similarly, these allow us to access and modify the $y$ component of the robot's estimated position.
• getTheta() & setTheta(): These allow us to access and modify the angle ($\theta$) of the robot's estimated position.

You may be wondering at this point: "Why use this special class? Why not just have 3 doubles above setup() to keep track of this?" Those are very astute questions! This TelemetryPosition class actually has some secret sauce that allows it to be displayed in the simulator's Telemetry window. This way, you can compare your code's estimated position against the robot's actual position in the simulator easily, without having to write a bunch of Serial.println() statements to check if everything is working correctly.

We certainly could just use 3 regular doubles to do the same thing and, if we were uploading code to a real robot, that is exactly what we would do! Because we are working purely in a simulated environment, however, we can use features like this to make our lives much easier.

As we will come to learn in later challenges, these types of features will be invaluable for understanding and learning about the systems we write. Indeed, features like these are why simulation is such a powerful tool in robotics at all.

Code structure

There are two primary components to our code:

1. Odometry calculation. We'll want to implement a function that we can call repeatedly (possibly in loop()) that will update our estimated position.
2. Robot movement. To see if our calculations are correct, we'll have to implement some code that actually moves the robot around. We've done this a couple of times already, so we'll mainly stick to the odometry stuff here.

Our basic structure will be along the lines of:

/* Includes and hardware objects */

void updatePosition() {
    // Math!
}

void setup() {
    // Buttons to move the robot around (forward, left, and right)
    pinMode(1, INPUT);
    pinMode(2, INPUT);
    pinMode(3, INPUT);
}

void loop() {
    // Call our function to update our position
    updatePosition();

    // Movement code
    if (digitalRead(1)) {
        left.run(255);
        right.run(255);
    } else if (digitalRead(2)) {
        left.run(-255);
        right.run(255);
    } else if (digitalRead(3)) {
        left.run(255);
        right.run(-255);
    } else {
        left.run(0);
        right.run(0);
    }
}

Implementing updatePosition()

And so, we come to the meat of this challenge: How do we use readings from our encoders to figure out how much the robot has moved?

There are two components to this:

1. Convert encoder counts to distance traveled for each wheel
2. Use the distances traveled by each wheel to calculate the overall change in position of the robot

Encoder Counts to Distance

Our encoders tell us how much our wheels have turned, but for us to actually make use of this information we need to convert these counts to distances that the wheels have actually traveled. We can do this using a tool known as dimensional analysis. This technique allows us to convert units intuitively by "canceling them out" when represented as fractions.

Here's a simple example: Say we have a plane traveling at 300 meters per second, how can we calculate the distance it travels in 3 seconds? Let's use dimensional analysis:

$$ \frac{300\text{ meters}}{1\text{ second}} * \frac{3\text{ seconds}}{1} \to \frac{300\text{ meters}}{1\ \cancel{\text{second}}} * \frac{3\ \cancel{\text{seconds}}}{1} \to 300\text{ meters} * 3 = 900\text{ meters} $$

By "canceling out" the $\text{seconds}$ unit, we were able to get our desired result. Take a second to think about how we could extend this logic to apply to our encoder counts. Then, take a look at the equations below.

$$ \begin{gather*} \frac{\text{counts}}{1} * \frac{\text{wheel rotations}}{\text{count}} * \frac{\text{meters}} {\text{wheel rotation}}\\ \downarrow\\ \frac{\cancel{\text{counts}}}{1} * \frac{\cancel{\text{wheel rotations}}}{\cancel{\text{count}}} * \frac{\text{meters}} {\cancel{\text{wheel rotation}}}\\ \downarrow\\ \text{meters} \end{gather*} $$

All that's left to do is to fill in the actual numbers that we need for our conversions:

• Our encoders produce $1500$ counts for every wheel revolution.
• Our wheels are 8 centimeters in diameter, which corresponds to ($\pi * d=$) $0.251$ meters traveled per revolution of a wheel.

It would be smart to precompute a conversion factor for this to make calculations easier. Plugging these numbers into our equation, we get:

$$ \frac{\text{counts}}{1}*\frac{1\text{ wheel rotation}}{1500\text{ counts}} *\frac{0.251\text{ meters}}{1\text{ wheel rotation}}=1.673 * 10^{-4} \text{ meters per count} $$

Now, to convert our encoder counts to distances, our code can just simply be:

// Somewhere in our sketch
const double metersPerCount = 1.673e-4;

// Inside of our `updatePosition()` function
double leftDistance = leftEncoder.readAndReset() * metersPerCount;
double rightDistance = rightEncoder.readAndReset() * metersPerCount;

Distances to Change in Position

There are many great explanations of how these equations are formulated, see this Medium article for a great walk through of it all.

In short, the math boils down to these equations:

$$ \begin{gather*} d=\frac{d_L+d_R}{2}\\ \Delta \theta = \frac{d_R-d_L}{d_{\text{tw}}}\\ \downarrow\\ x_{\text{new}}=x_{\text{old}}+d * \cos\bigg(\theta_{\text{old}} + \frac{\Delta \theta}{2}\bigg)\\ y_{\text{new}}=y_{\text{old}}+d * \sin\bigg(\theta_{\text{old}} + \frac{\Delta \theta}{2}\bigg)\\ \theta_{\text{new}}=\theta_{\text{old}}+\Delta \theta \end{gather*} $$

You might notice that we actually need a little bit more information about our robot to do this math. Specifically, we need to know what $d_\text{tw}$ is. This is the "trackwidth" and it is the distance between our robot's wheels. For our robot, this measurement is 20 centimeters.

Translating this into code is your job. After all, this one has been pretty light on the coding requirements.

Testing

With all of our functionality implemented, it's time to see if our odometry calculations are correct. With the simulator open, you should be able to push the different buttons to make the robot move.

You should compare the numbers listed in the Odometry section of the telemetry window against the Robot Position section, to see if your calculations are yielding similar numbers.

Note that these numbers won't match exactly, the simulation doesn't think in terms of encoder counts, it tracks each wheel's position exactly. So, we lose a little bit of precision with our math, but it's more than good enough for this application.

A couple more notes:

• You may notice that the robot doesn't start at $(0, 0)$ in the simulated world. Instead, it starts at $(0.5, 1.0)$. This may make comparison difficult between your estimated position and the actual position. To rectify this, you can set the initial position of the robot in the setup() function. See the solution code for details.
• You may also notice that your calculated theta is way different than the simulator's theta. That's because our math gives results in radians, whereas the simulator reports degrees. You can rectify this by converting your values for theta before you set them and after you get them. See the solution code for details.

Solution

#include "mosscap/challenge2.h"

#include <Encoder.h>
#include <Motor.h>

Motor left(11, 10, 12);
Motor right(21, 20, 22);

Encoder leftEncoder(15, 16);
Encoder rightEncoder(25, 26);

TelemetryPosition pose;

const double metersPerCount = 1.673e-4;
const double trackWidth = 0.20;

void updatePosition() {
    double leftDistance = leftEncoder.readAndReset() * metersPerCount;
    double rightDistance = rightEncoder.readAndReset() * metersPerCount;

    double d = (leftDistance + rightDistance) / 2.0;
    double dTheta = (rightDistance - leftDistance) / trackWidth;

    // Convert from degrees to radians
    double thetaOld = pose.getTheta() * PI / 180.0;

    double xNew = pose.getX() + d * cos(thetaOld + dTheta / 2.0);
    double yNew = pose.getY() + d * sin(thetaOld + dTheta / 2.0);
    double thetaNew = thetaOld + dTheta;

    pose.setX(xNew);
    pose.setY(yNew);
    
    // Convert back to degrees
    pose.setTheta(thetaNew * 180.00 / PI);
}

void setup() {
    // Buttons to move the robot around (forward, left, and right)
    pinMode(1, INPUT);
    pinMode(2, INPUT);
    pinMode(3, INPUT);

    // Set initial position
    pose.setX(0.5);
    pose.setY(1.0);
}

void loop() {
    // Call our function to update our position
    updatePosition();

    // Movement code
    if (digitalRead(1)) {
        left.run(255);
        right.run(255);
    } else if (digitalRead(2)) {
        left.run(-255);
        right.run(255);
    } else if (digitalRead(3)) {
        left.run(255);
        right.run(-255);
    } else {
        left.run(0);
        right.run(0);
    }
}