Here is the raw code, I'll explain it... (supaGyro is just a gyro lol):
Code:
// Get the Gyro Angle and Filter it
xAngle = Math.toDegrees(getAxesMeasurements().XAxis);
xAngleFiltered = Constants.hfc * xAngleFiltered + (1 - Constants.hfc) * xAngle;
gyroError = xAngleFiltered - supaGyro.getAngle();
// Get the actual Angle of the bot
angle = Constants.lfc * ((angle + (supaGyro.getAngle() + gyroError)) / 2) + (1 - Constants.lfc) * xAngle;
Basicly the first line of code takes the Accelerometer X-Axis (It would be Y-Axis if the Accelerometer was laying flat) and converts it to degrees...
The second line filters the X-axis and causes it to return a more stable reading (HFC is Heavy filter constant)
the Third line calculates how much the gyro is drifting
The fourth, acually corrects the drift on the gyro, the value wont change unless the accelerometer is moving... It works great for our Gyro
our HFC = 0.9 and LFC = 0.98
We use a gyro for automatic balance, it works well.
Ill give a more complete post with a formula where we derived our code from, but im late for a robotics meeting
