Lab 8: Stunts
The goal of this lab was to perform a stunt with the robot. The stunt I chose was Task B (Orientation Control): driving the car towards a wall and then performing a 180-degree turn within 3 ft of the wall and driving straight back.
To accomplish this, I wrote a new command to execute the drift. The command drives the car straight forward until the car is within 1500 mm of the wall. Once within 1500 mm based on the front ToF sensor, I call a helper function which uses my orientation PID algorithm from Lab 6 to rotate 180 degrees. Once the rotation is complete the helper function has the car drive forward again for one second before stopping. Three ft away is only 914 mm, but I used a more conservative bound of 1500 mm because I found while testing that the car took longer to rotate given how fast it was moving towards the wall. Initiating the rotation 1500 mm away rather than 915 gave the car enough time to fully complete the turn within 3 ft without hitting the wall. I store the ToF readings, motor input values, and the IMU yaw estimates in arrays and send them over to my computer for help debugging also. Below is a code snippet of the command.
I also found that my right motor was more powerful than the left one and causing my car to drift a bit, so I multiply the motor input value by 0.7 when writing the left motor, which allowed the car to drive straight. The orientationPID function is essentially the same code used for the Lab 6 orientation control task, and the motorIndex input is just so the function knows where to continue adding entries to the motor input debug array. The orientationPID function runs the PID with a setpoint of 180 degrees, and when the error is less than 10 degrees it breaks out of the PID loop and drives straight for one second. I gave a more lenient error of 10 degrees because while testing I found that the car turns fast and will often overshoot, so if it is within 10 degrees than it is straight enough to drive back. I also break out of the PID loop after 2 seconds in case the yaw estimate can’t keep up with the rotation and never gets close to 180.
Below is a video of the first somewhat successful trial I had using the code from above. I accidentally was testing using the black mat area for the Task A stunt, but my robot does perform the drift pretty well. It ends up driving back very straight as well. One thing to note is that I changed the kd parameter for PID from 40 to 125. As mentioned above the car turned very fast while drifting, and I often found it would turn 270-300 degrees and drive away sideways. Increasing kd helped eliminate this. Also, I started the car around 3m away from the wall, so I had to change my distance sensor to use long mode instead of short to pick up distances that far.
Below are plots of the ToF readings, motor input values, and yaw estimates vs time during the run.
As seen the last ToF reading is around 1400 mm, at which point the car initiates the turn. Before this point the motor input was always 40000, but on the motor plot once then drift starts the input drops all the way down to -65500. The drift takes around half a second, and the motor values rise closer to 0 as the yaw gets closer to 180 degrees, as expected with PID. At the very end the last motor input is 40000, which is the car driving straight back. The yaw plot also shows that in 0.6 s the car achieved a rotation of almost 175 degrees, at which the point the error was less than 10, so the PID was then stopped.
After testing the code without the mat, I found that the turn was overrotating a lot again. The mat must have been helping slow the turn down a little. Below is a video of the results.
Here is a plot of the yaw values, showing that the IMU estimated the yaw as being 175, even though the car over-rotated way past 180. For the next trial I raised kd_orient from 125 to 150.
To correct the overturning I increased kd from 125 to 330. I also increased the forward speed of the car at the start and end to 45000. This was to try and drift at a higher speed but also to help the car cover more ground going back. I was able to successfully perform the stunt. Below is a video of the first successful stunt.
Here are plots of the ToF readings, motor input values, and yaw estimates using the new parameters for the successful run.
At this speed and PID parameter settings I was able to get pretty reliable results. There was some variance with when the car starts the turn which caused it to hit the wall a couple times which I will discuss below. Here are the second and third videos of the working stunt.
During testing I tried to increase the forward speed of the car up to a motor input value of 50000. This was fast enough to decrease the number of ToF readings I was getting before having to initiate the turn. This often led to the turn being initiated too late and the car hitting or bumping the wall. Sometimes the stunt was performed successfully but other times the wall was hit as in the video below.
Also, sometimes the car bumped the wall but was able to recover using the PID and finish the 180 degree turn, as in the videos below.
Overall, to increase the forward speed of the robot larger than a motor input value of 45000, with more time I would have had to implement the Kalman Filter or some sort of extrapolation to estimate ToF readings if none were ready. Just using the ToF sensor itself, lowering the input to 45000 was slow enough to collect the ToF readings needed to initiate the turn early enough to successfully perform the stunt.