Page 160 - 만들면서 배우는 아두이노 드론

7.4.3 각도 구하기 소스

이제 직접 각도를 구하는 소스를 작성한 후, 드론의 각도를 확인해보도록 하겠습니다.

아래의 예제 코드를 기체에 업로드하고, 기체를 평평한 지면에 놓고 시리얼 모니터를
실행합니다.

[소스 코드]
#include <Wire.h>

const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
float dt;
float accel_angle_x, accel_angle_y, accel_angle_z;
float gyro_angle_x, gyro_angle_y, gyro_angle_z;
float filtered_angle_x, filtered_angle_y, filtered_angle_z;
float baseAcX, baseAcY, baseAcZ;
float baseGyX, baseGyY, baseGyZ;
float gyro_x, gyro_y, gyro_z;
unsigned long t_now;
unsigned long t_prev;

void initDT() {
t_prev = millis();

}

void calcDT() {
t_now = millis();
dt = (t_now - t_prev) / 1000.0;
t_prev = t_now;

}

void initMPU6050() {
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);

159