Lab 6: Orientation Control
Prelab
The purpose of this lab was too implement PID control of the yaw of the car to get it to rotate to different angles. To handle sending and receiving debugging data over bluetooth I followed a similar setup to lab 5. I created a new command to start running PID control of the car’s yaw for 10 seconds to ensure a hard stop at some point. I also implemented a command to change the PID parameters for orientation control, and another command to change the desired angle (setpoint) for the car. This allowed me to rotate to multiple angles all while still connected over bluetooth without having to reprogram. To debug, during the control loop I stored the yaw readings, motor input values, and setpoint in separate arrays along with the timestamps. Each motor was driven with the same motor input value in opposite directions to turn.
To compute the yaw I integrated the output of the gyroscope’s Y axis reading over time, similar to the method used in Lab 2. I integrated over Y since I had the IMU taped vertically to the side of my car, with the IMU’s Y axis pointing upwards. While testing, one observation I made was that the gyroscope’s yaw estimate was drifting much more than usual. Over the course of 10 seconds of the car sitting stationary the yaw values started out at 0 and drifted all the way to 16 degrees. To fix this, before running the control loop I sampled 100 values of the gyroscope’s Y measurement and took the average of all of them. Then, in the PID loop, when calculating yaw I subtracted this average from each gyroscope reading. This was very effective in eliminating the drift, because after this adjustment over 10 seconds the yaw only drifted by around 0.5 degrees. Below is an snippet of this fix.
I also had to increase the rotational velocity of the gyroscope. At its default, the car would spin too fast for the gyroscope to keep up. The resulting behavior was the car would end up at an orientation of around 15 degrees larger than what the gyroscope read. To fix this, in the setup section of the Arduino code I used the following code snippet to increase the rotational velocity to 2000 dps.
PID Controller
I ended up implementing a full PID controller, and the PID algorithm I used was pretty much exactly the same as in Lab 5 (before linear interpolation), except now the error is the current yaw minus the setpoint, rather than the ToF reading minus the setpoint. So I will not include the code here because of redundancy.
I implemented the PID controller all together first and then tuned the parameters one by one since I had lots of experience with already getting the algorithm to work from lab 5. The goal at first was to get the car to turn 90 degrees to the left. I started by tuning just PI and leaving the D parameter as 0. With PI I was able to get the car to reach a yaw of 88 degrees, at which point the motors stopped because the error was less than 2 degrees. PI did better than I thought it would as the car snapped quickly to 90 degrees and the integral term helped eliminate steady-state error to bring as P alone couldn’t hit 90. Below are plots of the yaw and motor input vs time.
Here is a video of the behavior.
I then introduced the D term, which was hard to tune while testing. Even just small values for kd in comparison to kp caused the car to decelerate fast while turning and noticeably undershoot the setpoint. I ended up increasing the ki parameter up to 300 because I noticed that when overshooting it took long (~5 seconds) for the ki term to build up enough power to rotate the car to 90 degrees. To counter this, I set kd to be 40, which I found worked great to stop the car in enough time to avoid considerable overshoot. Below is a video of the results, and you can see the car finished straighter compared to PI, where the car seemed to finish slightly slanted. This improved upon the PI controller noticeably. The yaw values sent from the Artemis showed that the car finished at 90.8 degrees, which is also more exact than just using PI.
The motor input plot for PI had a little overshoot over 0 due to the integral term trying to correct a small steady-state error, but for this PID plot you can see there was no correction needed.
Multiple Setpoints
I figured that the main orientations needed for turning would be a 90 degree turn and a 180 degree turn. Therefore, I wanted to calibrate my controller to be able to do both actions consecutively. I first had my car rotate 180 degrees, and then 90 degrees. I found that the current parameters caused major overshoot for 180 degrees while they worked perfectly for 90 degrees. To mitigate this, I increased the kd parameter up to 80 from 40. This got rid of the overshoot for 180 degrees, but introduced some slight error into my 90 degree turn which wasn’t as precise. This tradeoff was ok to me because the accuracy of both turns was now sufficient even if both weren’t 100 percent accurate. Below is a video of the successive movements for kp = 900, ki = 300, and kd = 80.