void Bucle_Principal() {
int salida=0;
int angulo = 0;
int who_a_I_tmp = 0;
//------------------------------------------------------------------------------------------//
double accXangle = (atan2(get_ax(), -get_ay()) * RAD_TO_DEG );
double gyroXrate = (double) (get_gz() ) / 131.0;
if (tipoFiltro==1){
angulo = (signed int) Complementary2(accXangle, gyroXrate,Tsample)-90;
}
else {
angulo = (signed int) kalman(accXangle, gyroXrate,Tsample)-90;
}
if (act_motor==1){
salida = pid(setpoint, angulo,5, KP, KI, KD, 5000, -5000, 10000, -10000);
SetPwm1(BIAS1 - salida);
SetPwm2(BIAS2 + salida);
who_a_I_tmp = get_who_I_AM();
if(who_a_I_tmp != 104)
{
Init_I2C();
reset();
}
enviar_valor_NOCR("ax= ",get_ax());
enviar_valor_NOCR(" ay = ",get_ay());
enviar_valor_NOCR(" gz= ",get_gz());
enviar_valor(" who = ",get_who_I_AM());
}
else {
SetPwm1(0);
SetPwm2(0);
ErrorI=0;
}
plot4(accXangle-90,angulo,salida/10,setpoint);
LED_ROJO=!LED_ROJO;
}
void IMU::update(){
read();
now = millis();
int looptime = (now - lastAHRSTime);
lastAHRSTime = now;
if (state == IMU_RUN){
// ------ roll, pitch --------------
float forceMagnitudeApprox = abs(acc.x) + abs(acc.y) + abs(acc.z);
//if (forceMagnitudeApprox < 1.2) {
//Console.println(forceMagnitudeApprox);
accPitch = atan2(-acc.x , sqrt(sq(acc.y) + sq(acc.z)));
accRoll = atan2(acc.y , acc.z);
accPitch = scalePIangles(accPitch, ypr.pitch);
accRoll = scalePIangles(accRoll, ypr.roll);
// complementary filter
ypr.pitch = Kalman(accPitch, gyro.x, looptime, ypr.pitch);
ypr.roll = Kalman(accRoll, gyro.y, looptime, ypr.roll);
/*} else {
//Console.print("too much acceleration ");
//Console.println(forceMagnitudeApprox);
ypr.pitch = ypr.pitch + gyro.y * ((float)(looptime))/1000.0;
ypr.roll = ypr.roll + gyro.x * ((float)(looptime))/1000.0;
}*/
ypr.pitch=scalePI(ypr.pitch);
ypr.roll=scalePI(ypr.roll);
// ------ yaw --------------
// tilt-compensated yaw
comTilt.x = com.x * cos(ypr.pitch) + com.z * sin(ypr.pitch);
comTilt.y = com.x * sin(ypr.roll) * sin(ypr.pitch) + com.y * cos(ypr.roll) - com.z * sin(ypr.roll) * cos(ypr.pitch);
comTilt.z = -com.x * cos(ypr.roll) * sin(ypr.pitch) + com.y * sin(ypr.roll) + com.z * cos(ypr.roll) * cos(ypr.pitch);
comYaw = scalePI( atan2(comTilt.y, comTilt.x) );
comYaw = scalePIangles(comYaw, ypr.yaw);
//comYaw = atan2(com.y, com.x); // assume pitch, roll are 0
// complementary filter
ypr.yaw = Complementary2(comYaw, -gyro.z, looptime, ypr.yaw);
ypr.yaw = scalePI(ypr.yaw);
}
else if (state == IMU_CAL_COM) {
calibComUpdate();
}
}
