Challenge 3: Path Following With A P Controller

Goal

In this exercise, we'll explore the basics of feedback control to improve the accuracy of our path following code.

⚠️ This exercises is specifically designed to build off of Challenge 2. It is highly recommended that you take the time to work through the odometry challenge before completing this one.

Of course, you may use the provided solution code for Challenge 2 as a starting point for this exercise (but where's the fun in that?).

Setup

As with previous exercises, create a new folder to hold your sketch, add an ardMain.cpp and populate it with the following skeleton code:

#include <mosscap/challenge3.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 updatePosition() {
    /* Your code here */
}

void setup() {}

void loop() {}

Paste in your code for updatePosition() from the last challenge in the marked area, this function will be essential to the work we do in this challenge.

Similarly to Challenge 1, mosscap/challenge3.h provides us with a path of goal positions as a 2 dimensional array of x and y coordinates. This header also provides our expected hardware (including our new encoders) as well as a single button wired to pin 1.

It will also be helpful to know that our robot starts at position $(0.6, 0.75)$ relative to world origin (the bottom left corner of the simulation window).

Theory: Feedforward vs Feedback Control

In challenge 1, the code that we wrote would be described as "feedforward" control. Essentially, we took a guess about how we needed to command the motors to get the robot to do what we wanted. We took educated guesses, of course, using some knowledge about the system (its top speed) with some math to predict the time it would take for our robot to move the way we wanted it to.

Crucially, though, we had no way of checking to see if our guess was actually correct. We couldn't tell if the robot had behaved as we had expected it to. As a result, we observed inconsistent behavior: sometimes the robot would do what we wanted, but other times it would stop short, or go long.

This is where the concept of feedback comes in. With feedback control, we measure the state (in our case the position) of our system and compare it against what we expect. This way, we can detect when the robot doesn't do what we want, and correct for it.

Fortunately for us, we've just build the perfect tool to help us implement feedback control: our odometry! Now that we have the power to keep track of our robot's position, we can compare it against our goal position (say, a point from the pathPoints list) to ensure that we're actually going where we want to.

Aside: Usefulness of Feedback Control

Feedback control is a core pillar of robotics, and it shows up absolutely everywhere: from simple educational robots like ours, to complex production robots like Boston Dynamics' Spot robot dog.

Why is it so central? Simply because things never behave exactly like we expect them to. It doesn't matter how much money you spend on hardware or how precisely you manufacture your parts, there is always some degree of uncertainty when something exists and moves in the real world. Because of this, virtually every robotic system in the world utilizes some form of feedback control.

With this in mind, let's take a look at the simplest (and most commonly used) type of feedback control:

The P-Controller

A P-controller (also called a "proportional controller") can be summarized in this simple equation:

$$ u(t)=K_p * e(t) $$

Let's break it down:

• $u(t)$ represents the output of the controller. In our case this is the pwm value that we send to the motors.
• $K_p$ is a configurable constant that we call a "gain". By changing this number we can change how the system reacts to errors.
• $e(t)$ is the error function. It represents the error between our current state and our desired state.

Let's (hopefully) clarify this with an example:

Let's say our robot only moves forwards and backwards for now and that we choose $K_p=10$. Let's also say that we would like to move the robot forward 2 meters. Assuming that the robot stats at 0 meters, our error ($e(t)$) would be 2. Plugging these values into the equation for our controller, we get a control output of $2 * 10=20$. This is the value we would pass to our motors in their run() function.

As the robot moves forward (because our controller is telling it to), our odometry will update, and our position will be increasing. Let's say that the robot has moved 1.3 meters, what would the controller output be now? Our error would now be $2-1.3=0.7$ and so our controller would output a pwm value of $0.7 * 10=7$.

As our system approaches our desired state, the control input gets weaker and weaker. Essentially, a P-Controller works like a spring, the farther away we are from our target, the stronger the controller pulls the system towards it.

This is also why it's called proportional controller, because the output is proportional to the error (it is changed with a constant multiplier).

Controller Tuning

P-Controllers (and indeed all other feedback controllers) give us some amount of freedom to choose how they respond. In this case, we get to choose the value for $K_p$. The process of choosing this number is called "tuning".

To come back to the spring analogy, imagine that we have a ball attached to a spring hanging from the ceiling. If we pull the ball down a couple of inches and release it it will gradually return to the resting position.

If instead, we pulled the ball down 5 feet (assuming that we don't permanently stretch the spring) and released it, the ball would go flying past the resting position and would end up oscillating back and forth quite a lot before finally settling back down.

The same thing can happen with P-Controllers. If we choose a value for $K_p$ that is too large for our given application, then we can cause the system to oscillate a lot around the desired state. In extreme cases, we can even cause the system to become unstable (imagine the robot jetting off at full speed into a wall), which is definitely bad.

There is an entire field of study focused on using math to determine the optimal values for controllers (if you're interested, take a feedback control systems class) but, in practice, controller tuning comes down to iteratively tweaking values until you get the behavior you want.

The general idea is: Start small, and slowly increase the gain until the system behaves as you want it to.

With that, let's start writing some code!

Implementing a P-Controller

Structurally, this sketch will be very similar to our code from challenge 1. The driveDistance() and turnAngle() functions will be incredibly similar, so for now, let's focus just on driveDistance(). Here's the basic structure:

const double FORWARD_KP = /* Some Number */;

void driveDistance(double dist) {
    while (/* robot is not at target position */) {
        // Update estimated position
        updatePosition();

        // Compute error
        double error = /* math */;

        // Compute control signal (from the equation for a P-Controller)
        double command = error * FORWARD_KP;

        // Send that control signal to the motors
        left.run(command);
        right.run(command);
    }
}

Take a second to give this a try yourself before reading on. This is one of the great parts about a simulator: that there's no expensive hardware to break! So, give it a try, make a mistake, there's no better way to learn.

1. Where Should We End Up?

What condition should we check in the while loop to ensure that we stop when we reach our target destination? Here, we can get some help from our friend Pythagoras:

/* Inside `driveDistance()` function */
const double startX = pose.getX();
const double startY = pose.getY();

while (sqrt(pow(pose.getX() - startX, 2) + pow(pose.getY() - startY, 2)) < dist) {
    /* Inside of loop */
}

This condition will only return true when the distance that the robot has traveled exceeds or is equal to the target distance. Which is the exact behavior we want!

2. Computing The Error

This is actually very similar to our code above. Our error is just the difference between the target distance and our current distance from our starting point:

double error = dist - sqrt(pow(pose.getX() - startX, 2) + pow(pose.getY() - startY, 2));

3. Test it Out

Take some time to try out some different values for $K_p$ to see how our new controller performs. You can add some test code to loop() that just tells the robot to move 1 meter forward:

void loop() {
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    driveDistance(1.0);
}

You might notice a problem... Our robot never actually gets to where we want it to go! And here is another scenario in which the real world makes our lives harder.

4. Friction

Unlike what you may have heard in some physics classes that you may have taken, friction is real, and it does cause problems. For us, this has to do with the pwm value at which our motors actually start moving.

In an ideal world, if we set our motors to run at pwm 1, our robot would creep along the floor at an almost imperceptibly slow pace. Unfortunately, though, this is not what happens. At low pwm settings, our motors may not be producing enough torque to even get the robot moving at all.

Because of this, our robot actually has a minimum pwm value at which it will begin to move. Below this, our robot will stop moving and our driveDistance() function will never exit. We need to do some experimentation to determine what this pwm is for our system.

You can try writing some code yourself, or you can follow the steps outlined below after replacing the code in loop() with the following:

void loop() {
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    static int pwm = 0;
    
    left.run(pwm);
    right.run(pwm);
    
    pwm += 10;
    
    delay(500);
}

This code will gradually increase the pwm value sent to the motors. Hit the button in the simulator to start the process and watch for when the robot just starts moving. For this system, our minimum PWM value should be about 70.

Now, we can implement this into our driveDistance() function:

int command = error * FORWARD_KP;

/* NEW CODE */
if (command < 0) command -= minPWM;
if (command > 0) command += minPWM;
/* END OF NEW CODE */

left.run(command);
right.run(command);

Give this new code a try! You will probably have to do some more tuning, but you should be able to get the robot to reliably move 1 meter, instead of stopping short every time.

Now for Turning

This is left as an exercise to the reader! This function will be almost identical to the driveDistance() function. You will need to add a new gain for the turning controller.

Give it a try!

Running the path

To test your code against the provided path, we can use the same pattern as in Challenge 1:

void loop() {

    // Wait for user to press the button
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    // For each point in the list: turn towards it and drive there
    for (int i = 0; i < pathPoints.size(); i++) {
        // Determine current and target positions
        double currentX = pose.getX();
        double currentY = pose.getY();

        double targetX, targetY;
        if (i == pathPoints.size() - 1) {
            targetX = pathPoints[0][0];
            targetY = pathPoints[0][1];
        } else {
            targetX = pathPoints[i + 1][0];
            targetY = pathPoints[i + 1][1];
        }

        // Calculate number of degrees to turn
        // Convert to degrees
        double targetAngle = atan2(targetY - currentY, targetX - currentX) / PI * 180.0;

        // Always measure target angle positively from x+
        if (targetAngle < 0) targetAngle += 360.0;

        double angleDelta = targetAngle - pose.getTheta();

        // Ensure that we don't turn more than we need to (turn 90 to the left instead of 270 to the right)
        if (abs(angleDelta) > 180.0) {
            while (abs(angleDelta) > 180.0) {
                if (angleDelta > 0) {
                    angleDelta -= 180.0;
                } else {
                    angleDelta += 180.0;
                }
            }

            angleDelta *= -1;
        }

        // Turn the robot
        turnAngle(angleDelta);

        // Calculate distance to drive
        double dist = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2));

        // Move the robot
        driveDistance(dist);
    }
}

Solution

Note that this code represents one possible solution to this challenge. Yours may very well be different, and that's ok! If it works, it works, that's what we like to say.

#include "mosscap/challenge3.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;

    // Clamp to 0->360 range. Not strictly necessary but helpful for later math
    if (thetaNew > 2 * PI) thetaNew -= 2 * PI;
    if (thetaNew < -2 * PI) thetaNew += 2 * PI;

    pose.setX(xNew);
    pose.setY(yNew);

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

const double FORWARD_KP = 510;
const int minPWM = 70;

void driveDistance(double dist) {
    const double startX = pose.getX();
    const double startY = pose.getY();

    while (abs(sqrt(pow(pose.getX() - startX, 2) + pow(pose.getY() - startY, 2)) - dist) >= 0.01) {
        // Update estimated position
        updatePosition();

        // Compute error
        double error = dist - sqrt(pow(pose.getX() - startX, 2) + pow(pose.getY() - startY, 2));

        // Compute control signal (from the equation for a P-Controller)
        int command = error * FORWARD_KP;

        if (command < 0) command -= minPWM;
        if (command > 0) command += minPWM;

        // Ensure that our command is within our PWM range
        if (command > 255) command = 255;
        if (command < -255) command = -255;

        // Send that control signal to the motors
        left.run(command);
        right.run(command);
    }

    left.run(0);
    right.run(0);
}

const double TURN_KP = 2.8;

void turnAngle(double angle) {
    double startAngle = pose.getTheta();

    while (abs(startAngle + angle - pose.getTheta()) >= 2) {
        updatePosition();

        double error = abs(startAngle + angle - pose.getTheta());

        int command = error * TURN_KP;

        if (command < 0) command -= minPWM;
        if (command > 0) command += minPWM;

        // Ensure that our command is within our PWM range
        if (command > 255) command = 255;
        if (command < -255) command = -255;

        if (angle < 0) {
            left.run(command);
            right.run(-command);
        } else {
            left.run(-command);
            right.run(command);
        }
    }

    left.run(0);
    right.run(0);
}

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

    // Set initial position
    pose.setX(0.6);
    pose.setY(0.75);
}

void loop() {

    // Wait for user to press the button
    while (!digitalRead(1)) {}
    while (digitalRead(1)) {}

    // For each point in the list: turn towards it and drive there
    for (int i = 0; i < pathPoints.size(); i++) {
        // Determine current and target positions
        double currentX = pose.getX();
        double currentY = pose.getY();

        double targetX, targetY;
        if (i == pathPoints.size() - 1) {
            targetX = pathPoints[0][0];
            targetY = pathPoints[0][1];
        } else {
            targetX = pathPoints[i + 1][0];
            targetY = pathPoints[i + 1][1];
        }

        // Calculate number of degrees to turn
        // Convert to degrees
        double targetAngle = atan2(targetY - currentY, targetX - currentX) / PI * 180.0;

        // Always measure target angle positively from x+
        if (targetAngle < 0) targetAngle += 360.0;

        double angleDelta = targetAngle - pose.getTheta();

        // Ensure that we don't turn more than we need to (turn 90 to the left instead of 270 to the right)
        if (abs(angleDelta) > 180.0) {
            while (abs(angleDelta) > 180.0) {
                if (angleDelta > 0) {
                    angleDelta -= 180.0;
                } else {
                    angleDelta += 180.0;
                }
            }

            angleDelta *= -1;
        }

        // Turn the robot
        turnAngle(angleDelta);

        // Calculate distance to drive
        double dist = sqrt(pow(targetX - currentX, 2) + pow(targetY - currentY, 2));

        // Move the robot
        driveDistance(dist);
    }
}