Back

Lab 5: Linear PID Control and Linear Interpolation

Prelab

The purpose of this lab was implement PID for position control on my car. The goal was to have the car drive forward and then stop exactly 1 foot away from a wall at as high of a speed as could be accomplished. To set up the code infrastructure to accomplish this, I wrote a bluetooth command to tell the car to drive straight and use PID control for 25 seconds. This command also stored all ToF readings and motor input values in arrays and then sent them over bluetooth after the PID was done to help debug. The 25 second limit was necessary to ensure that the car would stop even if the PID failed and the car started driving elsewhere. I wrote another bluetooth command to change the PID parameters kp, ki, and kd also so that I could debug and test the system without having to upload code after each edit.

Here is a code snippet of my bluetooth command to alter PID parameters.

Here is a code snipet of the start of the PID command which setups the necessary variables for just a P controller, which is what I started with. I used a value of 16 for the analogResolutionBits rather than 8, so for the analogWrite command I could write anywhere between 0 and 65536. I tried lowering it to 8 bits but experienced difficulty with using the motors and sensors together so I changed it back to 16 and implemented my system from there.

Finally, here is snippet of the end of my PID command which turns off the motors and then sends all debugging data over bluetooth. I kept arrays for all ToF readings, motor input values (used in analogWrite), and the timestamps in milliseconds of each.

PID Code

I started with a P controller, then built up to a PI and then PID controller. Here is a snippet of the full PID control code after I got P,I, and D working. I’ll explain the design decisions behind the code then below I will show the results and debugging data from doing P, then PI, then PID to show the incremental progress.

This is the first half of the PID controller, which is calculates error and sets the motor input value. At this stage computations are only done each time a new ToF reading is available, so if there is no data ready yet then the motor just runs at the same value as the previous iteration. At the top the current error is computed using ToF data and then dt is calculated as the amount of time elapsed between ToF readings. The cumulative_error variable is used for the integral term, and you can see that if the error switches sign I zero-out the cumulative_error to prevent integral wind-up. I calculate the derivative term by first low-passing the error to prevent amplification of noise, since I found that my ToF sensor was considerably noisy. Also, I only calculate a new motor input if the sensor reading is not 0.0. This is because while testing I found that often times my sensor would stop working and just return 0.0 over and over, so if a reading is 0.0 I know that it is not accurate ansd to discard it. Also, I clamp the maximum contribution of the integral term to 40000 so that it doesn’t dominate the controller and cause the motors to spin at max speed. Lastly, if the car is within 5 mm of the setpoint I set the motor to 0 since 5 mm of error is acceptable and accurate enough.

Above is the final part of the PID code. All this does is ensure that I am writing a value to the motors within an acceptable range for the analogWrite command. Also, I treat negative motor input values as instructing the car to go backwards. So, I must check the sign of the motor input and if its negative write the magnitude of the value to the second motor inputs rather than first to spin the wheels backwards.

At this point for a fully-working PID I am still only updating motor inputs when a new sensor reading is ready. Through the use of the debug array holding timestamps of sensor readings I found that I was getting a new sensor reading roughly every 50 ms. This means that the timing of my control loop was also ~50 ms, and this is without including any delay statements or blocking loops in the code.

P Results

When I started with just a P controller, the motor input was controlled exclusively by input = kp * error. I found that the car would successfully stop before the wall but would overshoot a foot considerably. Here is a video of the behavior.

Here are plots of the ToF readings vs time and motor input vs time from the debugging data sent over bluetooth. The setpoint for one foot is 304 mm, and the car overshot by around 130 mm as the final ToF reading was 170 mm. Also, the motor input is negative after overshooting indicating the car tried to go backwards, but just a P term does not supply enough power.

PI Results

I then added the integral term so that the controller could accumulate error over time, which could allow the car to move backwards when it overshoots. This did work, but it took very long (~ 5 seconds) for the integral to get large enough error to move the car backwards. Below is a video, and you can see that after overshooting the car does move backwards to the setpoint. I tried to increase the kp value to make the car go backwards quicker, but doing this caused the car to go too fast forwards and crash in the wall. This problem seemed not solvable without adding D.

Here are the plots of ToF readings and motor inputs. Although it takes too long to move backwards, the car ends up with at a distance of 285 mm away (an error of 20), which is a much better job than just P, which finished 170 mm away (an error of 130). Also, the motor input graph shows how with the integral term the motor is able to accumulate a larger negative value which is enough to overcome the motor’s deadband.

PID Results

The derivative term helped the car to decelerate quickly near the setpoint to avoid overshooting. This was very effective, and then the last bit of steady-state error was eliminated by the integral term. As you will see in the video below the car moves quickly and then stops at almost exactly one foot away from the wall. I tuned the PID parameters by first only using kp, and increasing it until the car overshot but didn’t hit the wall. I then added kd and increased this until the car undershot the setpoint slightly. I then added ki and increased this parameter along with decreasing kp until the car stopped at almost exactly a foot.

The ToF readings plot shows how fast the car was able to reach the setpoint compared to P and PI. Also, the ToF reading when the car stopped was around 307 mm, which is extremely good. The setpoint is 304, so the controller reduced the error down to 3 mm.

The speed achieved during this run was 1.16 m/s, which is the fastest so far.

Linear Interpolation

The frequency at which the ToF sensor is returning data is 1 / 50 ms = 20 Hz. In the PID code above all calculations for the motor input are done inside the if statement that checks if the ToF has new data. I removed the code used to set the motor_input variable using the PID expression outside of this if so that even if no new data is ready the motor_input is being recalculated using the P,I, and D terms resulting from the last ToF reading. I found that the PID control is now running at a rate 5-10 ms rather than 50 ms, which is a large speedup.

I then implemented linear interpolation by keeping track of whether or not the ToF produced a reading this iteration. If not I used the most recent two readings to calculate a line through each, and then use the current time (millis()) to predict the next ToF reading along this line. I also needed to keep track of the timestamp for the most recent actual ToF reading. I then proceeded with the same PID calculations as if the sensor had returned a value.

When using interpolation I noticed that now my car drifted to the left and sometimes sharply turned when going faster as if the motors no longer spun at the same speed. I tried to mitigate this by multiplying the input to the stronger motor by 0.7 to weaken it, but the drift was still there. I wasn’t able to get rid of it by lowering this ratio. However, with linear interpolation my car met the setpoint faster than it did with just PID. This is because PID is now running at almost 5 times the rate it was before with sensor values at each iteration (real or predicted). Below is a video. You can see the car still finishes around one foot from the wall, but gets there quicker. I also increased the ki parameter from 3 (regular PID) to 5, because the left-drift caused the car to finish a little short.

Here are the graphs of sensor output/prediction and motor input. You can see that the sensor output has some outliers generated from interpolation, but the car finishes with a final ToF reading of 324, which is around 2cm off of one foot. The motor input curve is also pretty noisy, which makes me think that the drift in the car is coming from sharp spikes in motor input caused by the linear interpolation.

Overall, if I had more time I would polish my interpolation implementation to be smoother and hopefully that would avoid the drift in the car. The speed achieved with linear interpolation was 2 m/s. So, the linear interpolation increased the speed of the car, but introduced a 2cm error in the setpoint. Until I can get a more accurate interpolation implementation regular PID is probably better.