Page 107 - 2

나. Arduino 에서 데이터 처리를 위한 프로그램 소스

#include <Wire.h> //Wire 헤더파일 불러오기
#include "Kalman.h"//kalman 헤더파일 불러오기
#define RESTRICT_PITCH
Kalman kalmanX; //kalman 필터를 통한 x 값
Kalman kalmanY;// kalman 필터를 통한 y 값
double accX, accY, accZ; //mpu x 축, y 축, z 축
double gyroX, gyroY, gyroZ; //mpu 자이로 값 축x , y 축, z 축
int16_t tempRaw;

double gyroXangle, gyroYangle; // 자이로 스코프 각도
double compAngleX, compAngleY; // 상보 필터 각도
double kalAngleX, kalAngleY; // 칼만필터후 각도
double corrected_x, corrected_y; // 오프셋 보정

uint32_t timer;
uint8_t i2cData[14]; //i2c 데이터 버퍼
// 모든 값 초기화및 핀설정
char a;
double m = 0.7;
double m1 = -0.7;
int d = 0;
int c = 0;
char p;
int in1_motor_left = 8;
int in2_motor_left = 7;
int in3_motor_right = 3;
int in4_motor_right = 4;
int INTA = 9;
int INTB = 10;
int pwm_on = 5; // ms ON
int pwm_off = 5; // ms OFF
//----------------------------------------------------------------------------
--
void setup() {
// 출력 설정
pinMode(in1_motor_left, OUTPUT);
pinMode(in2_motor_left, OUTPUT);
pinMode(in3_motor_right, OUTPUT);
pinMode(in4_motor_right, OUTPUT);
pinMode(INTA, OUTPUT);
pinMode(INTB, OUTPUT);
// serial stat
Serial.begin(115200);

- 107 -