Page 108 - 2

//BT.begin(9600);
delay(50);
Wire.begin();

TWBR = ((F_CPU / 400000L) - 16) / 2; // ic2 주파수 400kHz 로 설정

i2cData[0] = 7; // 주파수 설정1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // FSYNC 를 비활성화하고 260Hz 를 설정 Acc 필터링, 256Hz 자이로 필터링 8
KHz sampling
i2cData[2] = 0x00; // 자이로 풀 스케일 범위를 250deg / s 로 설정합니다.
i2cData[3] = 0x00; //Accelerometer Full Scale Range 를 2g 로 설정

while (i2cWrite(0x19, i2cData, 4, false));
while (i2cWrite(0x6B, 0x01, true));

while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));// 오류판단
while (1);
}
delay(100); // 센서값을 읽기위한 딜레이

//kalman 필터 계산 설정------------------------------------------------------------
----------
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];

#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

kalmanX.setAngle(roll);
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}

- 108 -