Page 109 - 2

//----------------------------------------------------------------------------
void loop() {
digitalWrite(INTA, HIGH);// 모터드라이버에 신호 출력
digitalWrite(INTB, HIGH);// 모터드라이버에 신호 출력
// x 축, y 축 ,z 축 설정
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000;
timer = micros();

#ifdef RESTRICT_PITCH
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif

double gyroXrate = gyroX / 131.0;
double gyroYrate = gyroY / 131.0;

#ifdef RESTRICT_PITCH
// 각도를 측정하여 roll 값에 넣는다.
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); //kalman 필터 보정 x 축

if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate;
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);

- 109 -