Académique Documents
Professionnel Documents
Culture Documents
x
Acc[1] = atan(-1*(ax/A_R)/sqrt(pow((ay/A_R),2) + pow((az/A_R),2)))*RAD_TO_DEG
;//y
Acc[2] = (atan((-ay/A_R) /(-ax/A_R))+M_PI)*RAD_TO_DEG;//z
//Calulamos el angulo del giroscopio
Gy[0] = gx/G_R;
Gy[1] = gy/G_R;
Gy[2] = gz/G_R;
//aplicamos el filtro de error
Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];
Angle[2] = 0.98 *(Angle[2]+Gy[2]*0.010) + 0.02*Acc[2];
// display tab-separated gyro x/y/z values
Serial.print("Angle X: "); Serial.print(Angle[0]); Serial.print("\n");
Serial.print("Angle Y: "); Serial.print(Angle[1]); Serial.print("\n");
Serial.print("Angle Z: "); Serial.print(Angle[2]); Serial.print("\n-----------\n");
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(RED, blinkState);
delay(10);
}