State Machines and Movement
I want my robot to move autonomously, and that means multiple movements. In my head, it makes sense to say, “Move forward 5 feet. Turn 90 degrees. Move forward 2 feet.” But, unfortunately, my robot can’t understand that yet.
To see how we should implement movement for the robot, we first need to know about how Arduino programs work. Each program has an setup() function and a loop() function and the programming language is C++. The setup() function runs once when the microcontroller turns on, and the loop() function runs again and again until the battery dies or you put it out of its mercy.
The code below is what we ended up with last time. Some custom encoder handling code (not shown here), and a way to update the Raspberry Pi with the encoder count. In embedded programming, the master is the device that initiates and manages the communication, while the slave is the one receiving and doing what the master says. In our case, the Raspberry Pi is the master and the Arduino is the slave. Here we update the buffer of what the slave can send to the Raspberry Pi should it ask for it.
void setup() {
// Initialize encoders
left_encoder.init();
right_encoder.init();
}
void loop() {
slave.buffer.leftEncoder = left_encoder.getEncoder();
slave.buffer.rightEncoder = right_encoder.getEncoder();
}So let’s try and apply our ‘Move forward, turn, move forward’ logic from before:
void loop() {
slave.buffer.leftEncoder = left_encoder.getEncoder();
slave.buffer.rightEncoder = right_encoder.getEncoder();
moveForward();
turn();
moveForward();
}This seems to work, but what if we need to get values from the encoder during the movements to get the robot’s position?
void loop() {
slave.buffer.leftEncoder = left_encoder.getEncoder();
slave.buffer.rightEncoder = right_encoder.getEncoder();
moveForward();
slave.buffer.leftEncoder = left_encoder.getEncoder();
slave.buffer.rightEncoder = right_encoder.getEncoder();
turn();
slave.buffer.leftEncoder = left_encoder.getEncoder();
slave.buffer.rightEncoder = right_encoder.getEncoder();
moveForward();
}That’s a little better, but it’s still only updating every movement, and we may want to be able to react to changes inside of the movement. The robot could potentially hit a rock and move off its path ever so slightly and we would only be able to correct at the start of another movement. With a style like this, the errors from each movement compound to move the robot off the intended path – they can’t really be corrected as they happen.
So, how do you fix this? A dramatic answer to that question is you don’t use loops, only the loop() function. You also use switch statements to determine what movement you are on. The purpose of not using any loops is so that after every command to the motor, the loop() function finishes, and the encoder values are updated. This ensures that sensor values that determine how the robot should execute the movement are continuously updated and ‘fresh’.
int current_action = 0;
void loop() {
slave.buffer.leftEncoder = left_encoder.getEncoder();
slave.buffer.rightEncoder = right_encoder.getEncoder();
switch (current_action) {
// Moving forward
case 0:
break;
// Turning
case 1:
break;
// Moving forward again
case 2:
break;
}
}The example should read as
- Update the encoder values
- If the current action is the first one, do this.
- Else, if the current action is the second one, do this.
- Else, if the current action is the third one, do this.
Then after those functions, the loop function has finished executing, and will be immediately called again to start the sense, update, run process again.
Okay, so now let’s flesh out the case statements a little more, with code that will actually do things.
One way to use the encoders with the robot’s movement is by checking if their current value is above a certain target encoder count. So some pseudo-code would be
if (encoders < target) {
move();
} else {
stop();
current_action = next_one;
}Now let’s translate the pseudo-code into each of our different cases. For moving forward, we have to account for the encoders of the left and right motors. For turning, we have to make one of the encoder targets negative, and make that motor move in the opposite direction.
To calculate the targets, we can use the calculated proportion of about 160 ticks/inch to convert our distance into inches. We want to move 12 inches, so that’s 1920 ticks or about 2000.
case 0:
if (slave.buffer.leftEncoder < 2000 && slave.buffer.rightEncoder < 2000>) {
motor.setSpeeds(200, 200);
} else {
motor.setSpeeds(0, 0);
current_action = 1;
}
break;
case 1:
if (slave.buffer.leftEncoder < 4000 && slave.buffer.rightEncoder > 0) {
motor.setSpeeds(200, -200);
} else {
motor.setSpeeds(0, 0);
current_action = 2;
}
break;
case 2:
if (slave.buffer.leftEncoder < 6000 && slave.buffer.rightEncoder < 2000) {
motor.setSpeeds(200, 200);
} else {
motor.setSpeeds(0, 0);
}
break;As you may be able to tell, this code is a little repetitive. Thankfully, we can make functions that drive the robot by taking in a target tick distance input, and returning whether we have completed the movement or not to move onto the next state.
bool driveForward(int target) {
if (slave.buffer.leftEncoder < target && slave.buffer.rightEncoder < target) {
motor.setSpeeds(200, 200);
return false;
} else {
motor.setSpeeds(0, 0);
return true;
}
}What this code is saying is:
- If we aren’t at the target yet, drive forward, and say we aren’t there yet.
- If we are at the target, stop moving, and say we are there.
However, this assumes that the encoder targets are the same value for both motors, but for the 3rd case they aren’t. What we can do is have a case in between the commands to reset the encoders to 0. This way every command can assume the target is the same without relying on the previous command’s target. Let’s write a quick reset encoders command, then put it together.
void resetEncoders() {
left_encoder.value = 0;
right_encoder.value = 0;
}We can now remove the drive forward commands, and add the one we made with a function. :)
case 0:
if (drive(2000)) {
current_action++; // move to the next command
}
break;
case 1:
reset_encoders();
current_action++;
break;
case 2:
if (slave.buffer.leftEncoder < 2000 && slave.buffer.rightEncoder > -2000) {
motor.setSpeeds(200, -200);
} else {
motor.setSpeeds(0, 0);
current_action = 2;
}
break;
case 3:
reset_encoders();
current_action++;
break;
case 4:
if (drive(2000)) {
current_action++;
}
break;
case 4:
// Do nothing. We are finished! :)
break;You could write a function for the turn command, so that it’s easier to turn the robot again later. Another way to improve this could involve making an object oriented system. With that, you could put the list of commands you want your robot to execute into an array. Then each loop the robot would work on executing the current command in the array.
This certainly isn’t the only way to write a state machine. It’s just a convenient one for getting started programming in robotics. If you want more information, be sure to check out Finite-state machines on Wikipedia.
Next post will be on quickly deploying code to the Raspberry Pi. See you then!
Check out other posts in this series
Join me in learning about how computers work
Just enter your email into the box below. You'll also receive my free 12 page guide, Getting Started with Operating System Development, straight to your inbox just for subscribing.
