Page 110 - 2

compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // kalman 필터로 y 축 각도 계산

if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate;
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); //kalman 필터로 x 축 각도 계산
#endif

gyroXangle += gyroXrate * dt; // 자이로 값계산
gyroYangle += gyroYrate * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // 각도 계산
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;

// 자이로 값과 각도값 리셋
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
delay(2);
Serial.println();
// 오프셋 보정
corrected_x=kalAngleX-171,746;
corrected_y=kalAngleY-81,80;
corrected_y = corrected_y+84;
// 시리얼 모니터로 출력
Serial.print(corrected_y);
pwm_adjust(corrected_y);
//mpu correctrd 값이 20 이상이면 멈춤
if(corrected_y>=m && corrected_y<20){
if(c>6){
m-=0.2;
m1-=-0.2;
c=0;
}
backward();
}
//mpu correctrd 값이 -20 이하면 멈춤
else if(corrected_y>=-20 && corrected_y<=m1){
Serial.print(" ");
if(d>6){
m+=0.2;
m1+=0.2;

- 110 -