-
Notifications
You must be signed in to change notification settings - Fork 0
Challenge 3: Path Following With A P Controller
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?).
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
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.
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:
A P-controller (also called a "proportional controller") can be summarized in this simple equation:
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 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
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).
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
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
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!
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.
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!
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));Take some time to try out some different values for 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.
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.
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!
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);
}
}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() {
// Button setup
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);
}
}