From Wobbly Mess to Fleeting Stability

My first attempt was a simple "Bang Bang" (on/off) controller. To put it mildly, it didn't work, resulting in a robot that was more floppy than functional.

I then moved on to a PID controller, which was a significant step up. This controller looked at how far the robot was from its vertical setpoint and applied torque to the reaction wheel to correct it. This actually worked! The robot could balance for up to 45 minutes at a time. However, it always ended the same way: with the robot crashing down. The problem was that the motor would gradually spin faster and faster until it hit its maximum speed, at which point it could no longer add the necessary torque to stay balanced.

I tried two control methods - one where the PID loop output was torque and one where the output of the PID loop was "delta velocity".  While the torque output was simpler in implementation, non-linearities in friction at low speeds required nasty fixes in code to account for.  So I ultimately decided that the best course was to plan in delta velocity. 

** Small aside : 
SimpleFOC made it easy to switch between these and while there is technically an underlying PID controller that is responsible for the velocity of the reaction wheel, I was able to abstract this away and forget about it after tuning the motor.  This is because the FOC control loop was running in the microseconds time frame while my IMU and high level controller required approx. 2 ms to run through.

Implementation details aside, I ultimately still had no fix for the gradual accumulation of velocity in the wheel, no matter how well tuned it seemed. 

The Two-Controller Breakthrough
The solution, it turned out, wasn't more power, but a second, smarter control loop. This was a fun step / small breakthrough of the project.

The First Controller: This remained my primary stability controller. It measures the robot's angle from the IMU over I2C, keeps track of the "setpoint", which was whatever quaternion was seen when the "enable" command was flagged, and applies torque via the reaction wheel to stop it from falling over in the short term.

The Second Controller: This one was the new addition. It monitors the speed of the reaction wheel. If the wheel is spinning too fast, this controller subtly and slowly adjusts the robot's balance setpoint. By telling the robot to lean just a tiny bit, I could use gravity to apply a torque that allows the reaction wheel to slow down, all while the first controller keeps it within the controllable range.
This two-controller system finally allowed the robot to balance seemingly forever, as it could now manage both its position and the reaction wheel's speed. 

BUT... there was still a controllability problem.  The tuning for smoothness/ damping seemed on the same axis as dynamic stability/ disturbance rejection.  This meant I could have a robust balancer or a pretty one, but a pretty one would fall with a slight breeze and a robust one wiggled like it had to pee. 

From Stable to Rock-Solid
To smooth things out, I went deeper into the physics. I implemented a control scheme that calculated the robot's total energy (both gravitational potential energy from its tilt and kinetic energy from its tipping velocity) and actively dumped that energy into the reaction wheel.

This energy-based approach, with a small PID loop still running to clean up any minor errors, resulted in an incredibly stable system. The final robot can balance so steadily that it appears almost motionless. It was a fantastic learning experience in control theory and robotics, and I'm thrilled with the final result -- but not enough to be satisfied.  I have many questions still, many theories to explore, and much more math to learn. 

Important tools for this project:
Invaluable to this project were 3 tools that I used/ spent time beforehand with. 
1) The MotorGo Axis - which is an open source robot...