by guenter » Mon Apr 01, 2013 12:00 am
Hello Kunda,
in the Code no single variables where used. So you cant make *3,5. So you make *30 and then /100.
The Code calculate two variables: Tiltangle and Anglerate.
Tiltangle:
Tiltangle is the accelerometer without noise.
The accelerometer has a lot of noise. So a lowpassfilter integrate all this noise. But now all fast changes of the platform are also integrated. To avoid that, a little bit of Gyrosignal is added to get "good" high frequencys back.
Anglerate:
The anglerate is the Gyro with highpassfilter.
First the gyro is integrated very strong to get an Zeropoint of the gyro. Then you calculate the deviation of the aktual Gyro value from this zeropoint.
Balancing:
Put together Tiltangle and Anglerate.
In theory you need only an gyro to balance. But the platform will slowly move away from the zeropoint. So you must add a part of the Tiltangle to compensate this.
when your platform tilt forward, Both Tiltangle and Anglerate are positive. So the motors drive forward. when the platform stay at this angle the Anglerate is Zero, the Tiltangle stay positive.The motors slow down. When the platform move back to zero. The anglerate is negative but the Tiltangle is still positive.The motors are zero.
Günter