Micromouse Lab 5

Micromouse
Motors and PID

Last time, we learned how to determine how fast our mouse is driving and how fast it’s turning. This week, we’ll use these values to make our mouse drive straight!

Brushed Motor Control
Motor wiring pinout
Motor wiring pinout

In addition to the encoder lines we’ve seen in previous labs, two of the wires in the motor cable power the motor itself: M1 and M2. Applying voltage in one direction (M1 > M2) makes the motor spin in one direction, and applying voltage in the other direction (M1 < M2) makes the motor spin in the other direction.

Motors need large amounts of current, which our Arduino can’t provide directly. To solve this problem, we use the DRV8833 motor driver chip, which can power two motors.


DRV8833 module
DRV8833 module

Plug in the DRV8833 module and a motor as shown below. One of the motors is connected to the OUT1 and OUT2 pins on the DRV8833, and the other is connected to the OUT3 and OUT4 pins. The Arduino is connected to the INx pins, allowing us to control the corresponding OUTx pins.

The DRV8833 is powered separately from the Arduino, so you will have to plug in the battery too. You should see a green LED on the DRV8833 module light up, which means it’s powered. Now we’re ready to spin some motors!


DRV8833 module
Wiring and Module Location


NOTE: DON'T PLUG IN BOTH THE BATTERY AND USB CABLE AT THE SAME TIME. This is because the 5v from your computer and the 5v from the onboard regulator will be connected together. Realistically speaking this shouldn't cause any problems but we can't account for all computers so better safe than sorry.

Now onto the coding. Paste the following code into a new sketch in the Arduino IDE:

#define PIN_MOTOR_LEFT_1 5
#define PIN_MOTOR_LEFT_2 6
#define PIN_MOTOR_RIGHT_1 9
#define PIN_MOTOR_RIGHT_2 10

void setup() {
  pinMode(PIN_MOTOR_LEFT_1, OUTPUT);
  pinMode(PIN_MOTOR_LEFT_2, OUTPUT);
  pinMode(PIN_MOTOR_RIGHT_1, OUTPUT);
  pinMode(PIN_MOTOR_RIGHT_2, OUTPUT);
}

void loop() {
  // TODO
}

To make the motors spin, we can reference the datasheet.


Datasheet Table Note: “1” means setting a pin to HIGH, “0” means setting a pin to LOW. “Z” means the motor pin is disconnected (typically called “high-impedance”), “H” means the motor pin is 9V, and “L” means the motor pin is 0V.

Try the following code in loop() -- what do we expect to happen?

digitalWrite(PIN_MOTOR_LEFT_1, HIGH);
digitalWrite(PIN_MOTOR_LEFT_2, LOW);

Try spinning it in the other direction too.

How do we make the motor stop? We just need to set both pins to the same value. It seems like there’s two different ways to do it:

digitalWrite(PIN_MOTOR_LEFT_1, LOW);
digitalWrite(PIN_MOTOR_LEFT_2, LOW);

digitalWrite(PIN_MOTOR_LEFT_1, HIGH);
digitalWrite(PIN_MOTOR_LEFT_2, HIGH);

Try both ways. Is there a difference? Turning the motor by hand may help.

So far, we have only been able to make the motor go full speed or not at all. How can we make the motor spin at a different speed? We could try switching the motor on and off really fast, so that the voltage applied to the motor “averages out.” This is the basic idea behind pulse width modulation (PWM).

Arduino has a built-in function for doing this: analogWrite(pin, value). Try the following code, which should make the motor spin slowly:

analogWrite(PIN_MOTOR_LEFT_1, 64); // takes a value from 0-255
analogWrite(PIN_MOTOR_LEFT_2, 0);

Checkoff #1

  1. What’s the difference between setting both motor pins to HIGH and setting both of them to LOW?
  2. How do you change the speed of the motor?
  3. How do you flip the direction of the motor?
  4. What happens when you analogWrite the same value to both motor driver pins? Why?
Setup

Download the skeleton code (Github) which provides easy to use functions for everything we've done so far. The directory structure should look like this:

mouse/
	mouse.ino
	mouse_helpers.ino
	pins.h

Open up mouse.ino. If you cloned it from Github, make sure to rename the folder from micromouse-lab6/ to mouse/. Arduino is weird like that. By default, the code will apply 20% power to each motor:

////////////////////////////////////
// Your changes should start here //
////////////////////////////////////
float left_power = 0.2;
float right_power = 0.2;

applyPowerLeft(left_power);
applyPowerRight(right_power);

applyPowerLeft() and applyPowerRight() should turn their corresponding motors at full power forwards when power is 1.0 and full power backwards when power is -1.0.

Upload and run mouse.ino and open up the Serial Monitor. Do the following:

  • Verify that each wheel is physically spinning in the forward direction (ToF sensors are the front of the mouse). If not, flip the INVERT_MOTOR_LEFT and INVERT_MOTOR_RIGHT flags as needed.
  • Verify that the measured linear velocity is positive. If not, flip the INVERT_ENCODER_LEFT and INVERT_ENCODER_RIGHT flags as needed.
  • Update WHEELBASE_DIAMETER and ENCODER_TICKS_PER_REVOLUTION on lines 109 and 110 in mouse_helpers.ino, using values from the last lab.

Checkoff #2

  1. Show that your motors spin forward and the measured velocity is positive.
Angular Velocity Control (Driving Straight)

The current driving implementation in the skeleton code is an example of open loop control, which uses knowledge of the system (our mouse) to estimate the required control input (motor voltage). We know that applying the same voltage to both motors will make them spin in the same direction at roughly the same speed.


Open Loop
Open Loop

This is great! That is, of course, under the assumptions that:

  1. We are in a vacuum
  2. The mouse is perfectly balanced
  3. The ground is perfectly flat, with no defects
  4. Our motors are exactly the same, to the atom
  5. Friction doesn’t exist

WHEW! That’s a lot of assumptions, which we CANNOT make in this situation.

Unfortunately, encasing the room in a vacuum violates the competition specs (and probably our ability to breathe), so we will have to find some other way to deal with these problems. In reality, applying the same voltage to each motor will not give us identical motor behaviors. The mouse will veer to one side. To fix this, we can use something called closed-loop control or feedback control, which utilizes readings from the encoders to correct the amount of power we apply to the motors.


Closed Loop
Closed Loop


We can use the encoder readings to determine how far we are from driving straight, and the feedback control corrects the error by, for instance, applying extra power to the slower motor and reducing power to the faster one.

One common way to do feedback control is with a proportional (P) controller. P controllers try to minimize an error — the difference between a measured value (input) and its desired value (setpoint).

This is accomplished by increasing a correction power proportionally to the error: the higher our error, the more power we should apply to correct for it.

\(\huge{u(t) = K_p\cdot error(t)}\)

\(\huge{error(t) = setpoint(t) - input(t)}\)

Let’s try driving forward at 20% power again. This time, however, we’ll apply a proportional correction value u as shown:

applyPowerLeft(0.2 + u_angular);
applyPowerRight(0.2 - u_angular);

\(\dots\) where \(K_p\) is a gain that scales our corrective power. There are ways to calculate the right value of \(K_p\), but guessing and checking also works.

Now implement a P controller as discussed above. What's our input? What's our setpoint? After uploading, try placing your mouse on a flat surface and have it drive forward (disconnect the USB cable first!). Does it go straight? Or does it oscillate? If so you may need to try different values of \(K_p\) (start with 1).

Checkoff #3

  1. If we use our mouse’s angular velocity as the input to a control loop that tries to make our mouse drive straight, what should the setpoint be?
    1. Remember, in the last lab, we figured out how to measure angular and linear velocity! What is the angular and linear velocity when the mouse is moving straight? How can we use this to find our setpoint and our error?
  2. If we want our applied correction power \(u(t)\) to be maximized (\(u(t) = 1.0\)) when the error is 2 radians/second, what should \(K_p\) be?
  3. Try a few different \(K_p\) values. Some values to start with: 0.1, 0.5, 5
    1. What happens when \(K_p\) is too low? Why?
    2. What happens when \(K_p\) is too high? Why?
    3. What seems to be a good value for \(K_p\)? Test a few values until you find one that works well.
  4. Plot your angular velocity error. Does it converge to zero?
Linear Velocity Control

Instead of blindly adding 0.2 to our wheel duty cycles to command our mouse to drive forward, we can add another P controller for our linear velocity as well:

applyPowerLeft(u_linear + u_angular);
applyPowerRight(u_linear - u_angular);

This allows our mouse to drive forwards and backwards as well as stop. Being able to control the speed of our mouse is really useful. Just as imbalances in the motors can prevent our mouse from driving straight, they can also cause our mouse to drift when turning in place.

Now implement a P controller but for linear velocity this time. It may help to disable angular velocity control to start. What's our input? What's our setpoint? Notice the similarities in the code?

Checkoff #4

  1. What should the input and setpoint be for our linear velocity controller?
  2. If we want our applied correction power \(u(t)\) to be maximized (\(u(t) = 1.0\)) when the error is 140 mm/second, what should \(K_p\) be?
  3. Try a few different \(K_p\) values. Some values to start with: 0.001, 0.01, 0.1
    1. What happens when \(K_p\) is too low? Why?
    2. What happens when \(K_p\) is too high? Why?
    3. What seems to be a good value for \(K_p\)? Test a few values until you find one that works well.
  4. Plot your linear velocity error. Does it converge to zero?