To implement the inverted pendulum stunt, I planned on reusing my orientation PID controller, now restructured to control the motors based on the robot's pitch. The intended behavior was for the robot to balance by driving forward as a response to forward pitch and driving backwards as a response to backward pitch. I eventually switched to a DMP-based controller instead of a PID controller, which resulted in a significant increase in the balancing time of the robot.

My first implementation reused my PID code and had pitch calculated via raw IMU readings that were filtered using the complementary filter. In my implementation, forward pitch is registered as negative and backward pitch is registred as positive, so the motor response signs are flipped as well.

Visually, I saw that the robot was somewhat able to balance if driving backwards but was continuously falling forwards. In the graphs of the pitch vs target, you could also see that the robot was not starting at a perfect zero degree vertical pitch, so I also tried shifting the target pitch.

I then experimented with decreasing Kp and increasing the Kd value which resulted in a lot more oscillation in the robot's response.

For the following test, I added a warm up period of 500 ms which allowed the pitch measurement to stabilize before the controller started responding. I added it because I noticed that the robot had a strong response to the initial pitch measurement and was very sensitive to the exact position that I held the robot in when I started the test. I also increased the motor deadband and swapped motor braking for motor stopping in my PID loop.

This test resulted in a more stable response with more balancing. But the robot was still falling over.

It was difficult to determine the cause of the falling since I was catching the robot, so I tried to get a balancing result without my support.

The test without support showed that the robot could balance but just for extremely short periods of time. Once the error got too high, the motors could not respond fast enough to correct the error and the robot would fall over.

After not getting significantly longer balancing times with the old PID controller, I switched my code to use a DMP-based controller instead. I thought that the drift in the pitch measurements was causing the PID controller to have a hard time stabilizing the robot even with tuning of the PID parameters.

Switching from my previous PID implementation to a DMP-based controller resulted in a significant increase in the balancing time of the robot due to the DMP's ability to account for the drift in the pitch measurements. I also found that I needed to have a very high minimum motor response of about 70 - 80 PWM to prevent the robot from falling over. Lastly, I had a condition to brake the motors when the error was too small the motors would brake instead of stopping, but I found that switching to a full abrupt stop resulted in better balancing.