balance1: - initial manual control to collect balancing data
Run a balancing servo off a clock.
Get input from IMU
Estimate body angle (in accelerometer Y units!!!!)
Wheel angle is in radians

balance2: - collect data for calibration of IMU signals
Run the balancing servo with the robot in the test stand
Flip sign of wheel angle in radians relative to balance1
Add wheel and wheel velocity gains

balance3:
Body angle is now in radians, and body angular velocity is in radians/second

balance4:
Move to first order filtering on wheel angle to produce wheel velocity
Add a launch behavior
Commented out az, only getting ay now. Assume az is a constant.

balance5:
Add a dead zone on the command.

balance6:
Filter the command, not the wheel velocity

balance7:
Switch to SERVO_INTERVAL = 0.004

Features:

Select how long servo runs with
#define RUN_DURATION 30000 // milliseconds

Select behaviors
#define DO_SINUSOID 0
 drive_frequency is sinusoid frequency in Hz
#define DO_MIN_JERK 0

Launch behavior
 set launch_duration

Dead zone
 set dead_zone

Accelerometer Y zero is now manually set in code.
 set ay0