// obtaining and filtering Acc angles
accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
Ax=ax/16384.00;
Ay=ay/16384.00;
Az=az/16384.00;
Angel_accX =atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Ax_offset; //offset
Angel_accY =-atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Ay_offset; //offset
estima_X = estima_cal(estima_X, Angel_accX, 0.2);
estima_Y = estima_cal(estima_Y, Angel_accY, 0.2);
// Serial.print("estima_X=");
// Serial.print(estima_X);
// Serial.print("estima_Y=");
// Serial.println(estima_Y);
// obtaining Gyro angles without filter simply because I'm lasy
ggx=gx/131.00;
ggy=gy/131.00;
ggz=gz/131.00;
Gx=Gx+(ggx-Gx_offset)*time_span;
Gy=Gy+(ggy-Gy_offset)*time_span;
autoCorrection();
// Serial.print("Gx=");
// Serial.print(Gx);
// Serial.print(" Gy=");
// Serial.println(Gy);
//fusion angles by combining 40% last observe + 30% Acc + 30% Gyro
fusion_angle_X = update_Estimat(fusion_angle_X, estima_X, Gx );
fusion_angle_Y = update_Estimat(fusion_angle_Y, estima_Y, Gy );
// Serial.print("fusion_X=");
// Serial.print(fusion_angle_X);
// Serial.print("fusion_Y=");
// Serial.println(fusion_angle_Y);
// if (fusion_angle_Y>=80){
// digitalWrite(13, HIGH);
// delay(1);
// digitalWrite(13, LOW);
// delay(1);
// }
//---------------------------------PID's----------------------------------------
uodate_error(orientX, orientY ,fusion_angle_X, fusion_angle_Y); |