Trailer Back Up
Here you can see our truck backing up and making adjustments to the trailer angle as it moves. The code adjusts the trailer even when given a bit of a kick. However we also have code such that if the angle diverges too much (greater than 60 degrees), the truck will stop and back up to straighten the trailer, and then continue forward.
Similar to above, except now the angle we want is around while keeping the difference between the truck gyro and the trailer gyro at 20 degrees to drive in a large circle (although we didn't show the entire circle in the video).
Here we just adjusted the angle difference to be 40 degrees to make a small circle.
Trailer Backup Code
This code is identical for both big and small circles, except we change the value of the angle (i.e. it is 20 in this code for the big circle, but 40 for the small circle.