Step 15: Control system
Again, see my website (link on page 1) for more info on control software principles etc.
The basic idea is this:
gyroangledt=amount of tilt from gyro since last reading was taken
newangle(of tilt from vertical)=(0.99*(oldangle+gyroangledt)) + (0.01 * accelerometerTilt)
This gives the new angle of tilt from vertical. Note how 1% of accelerometer signal is inserted each time with 99% of the gyro-derived short-term angle change calculation. This smooths out variations in accelerometer reading with time (averages out the noise so it self cancels), slowly corrects any zero-point drift in the gyro BUT still allows the gyro to handle the instantaneous short term changes in tilt.
linearvelocity = linearvelocity + (0.00004*angle)
This means "are we STILL tipping?" and if so slowly increase power to the motor. This is how you control the speed manually; if you keep the board continually tilted, it will slowly accelerate in the direction you are tilting it.
level=((k1*angle) + (k2*anglerate) + linearvelocity) * overallgain
"anglerate" is from gyro i.e. is the immediate short term "rate of tipping"
Level controls POWER to be sent to motor (the PWM duty cycle).
k1 is about 4.5
k2 is about 0.5
Overallgain is just that - sort of an "accelerator" knob on control panel - if set too low board feels sloppy, too high and it oscillates wildly!
Finally, the whole thing only works because this program is looping about 100 times per second.