안녕하세요 자이로센서(MPU6050)를 두개 사용하여 각 센서의 각도를 구하고싶습니다.

​

자이로센서(mpu6050) 두개를 사용하여 연결할 때 i2c 통신으로 i2c 주소가 겹치지 않도록 연결까지 마친 상태입니다.(0x68와 0x69) 그 후 코딩을 하는데 두 센서의 주소를 어떤식으로 구분해서 코딩을 해야하는지 모르겠어서 질문드립니다. 자이로센서를 두 개 사용하여 각각의 각도를 구하는 코딩을 하고싶은데 두 센서의 주소를 어떤식으로 구분해서 코딩을 해야하는지 모르겠습니다... 일단 제가 한번 짜보긴 했는데 실행을 했을 때 값이 제대로 나오지 않는데 이유가 뭔지 알 수 있을까요?

​

#include<Wire.h>

​

const int MPU1_ADDR = 0x68; // I2C통신을 위한 MPU6050의 주소

const int MPU2_ADDR = 0x69;

int16_t AcX1, AcY1, AcZ1, Tmp1, GyX1, GyY1, GyZ1, AcX2, AcY2, AcZ2, Tmp2, GyX2, GyY2, GyZ2; // 가속도(Acceleration)와 자이로(Gyro)

double angleAcX1, angleAcY1, angleAcZ1;

double angleGyX1, angleGyY1, angleGyZ1;

double angleFiX1, angleFiY1, angleFiZ1;

double angleAcX2, angleAcY2, angleAcZ2;

double angleGyX2, angleGyY2, angleGyZ2;

double angleFiX2, angleFiY2, angleFiZ2;

​

const double RADIAN_TO_DEGREE = 180 / 3.14159;

const double DEG_PER_SEC = 32767 / 250; // 1초에 회전하는 각도

const double ALPHA = 1 / (1 + 0.04);

// GyX, GyY, GyZ 값의 범위 : -32768 ~ +32767 (16비트 정수범위)

​

​

unsigned long now = 0; // 현재 시간 저장용 변수

unsigned long past = 0; // 이전 시간 저장용 변수

double dt = 0; // 한 사이클 동안 걸린 시간 변수

​

double averAcX1, averAcY1, averAcZ1;

double averGyX1, averGyY1, averGyZ1;

double averAcX2, averAcY2, averAcZ2;

double averGyX2, averGyY2, averGyZ2;

​

void setup() {

initSensor();

Serial.begin(115200);

caliSensor_1(); // 초기 센서 캘리브레이션 함수 호출

caliSensor_2();

past = millis(); // past에 현재 시간 저장

}

​

void loop() {

getData_1();

getData_2();

getDT();

​

angleAcX1 = atan(AcY1 / sqrt(pow(AcX1, 2) + pow(AcZ1, 2)));

angleAcX1 *= RADIAN_TO_DEGREE;

angleAcY1 = atan(-AcX1 / sqrt(pow(AcY1, 2) + pow(AcZ1, 2)));

angleAcY1 *= RADIAN_TO_DEGREE;

angleAcX2 = atan(AcY2 / sqrt(pow(AcX2, 2) + pow(AcZ2, 2)));

angleAcX2 *= RADIAN_TO_DEGREE;

angleAcY2 = atan(-AcX2 / sqrt(pow(AcY2, 2) + pow(AcZ2, 2)));

angleAcY2 *= RADIAN_TO_DEGREE;

// 가속도 센서로는 Z축 회전각 계산 불가함.

​

// 가속도 현재 값에서 초기평균값을 빼서 센서값에 대한 보정

angleGyX1 += ((GyX1 - averGyX1) / DEG_PER_SEC) * dt; //각속도로 변환

angleGyY1 += ((GyY1 - averGyY1) / DEG_PER_SEC) * dt;

angleGyZ1 += ((GyZ1 - averGyZ1) / DEG_PER_SEC) * dt;

angleGyX2 += ((GyX2 - averGyX2) / DEG_PER_SEC) * dt; //각속도로 변환

angleGyY2 += ((GyY2 - averGyY2) / DEG_PER_SEC) * dt;

angleGyZ2 += ((GyZ2 - averGyZ2) / DEG_PER_SEC) * dt;

​

// 상보필터 처리를 위한 임시각도 저장

double angleTmpX1 = angleFiX1 + angleGyX1 * dt;

double angleTmpY1 = angleFiY1 + angleGyY1 * dt;

double angleTmpZ1 = angleFiZ1 + angleGyZ1 * dt;

double angleTmpX2 = angleFiX2 + angleGyX2 * dt;

double angleTmpY2 = angleFiY2 + angleGyY2 * dt;

double angleTmpZ2 = angleFiZ2+ angleGyZ2 * dt;

​

// (상보필터 값 처리) 임시 각도에 0.96가속도 센서로 얻어진 각도 0.04의 비중을 두어 현재 각도를 구함.

angleFiX1 = ALPHA * angleTmpX1 + (1.0 - ALPHA) * angleAcX1;

angleFiY1 = ALPHA * angleTmpY1 + (1.0 - ALPHA) * angleAcY1;

angleFiZ1 = angleGyZ1; // Z축은 자이로 센서만을 이용하열 구함.

angleFiX2 = ALPHA * angleTmpX2 + (1.0 - ALPHA) * angleAcX2;

angleFiY2 = ALPHA * angleTmpY2 + (1.0 - ALPHA) * angleAcY2;

angleFiZ2 = angleGyZ2; // Z축은 자이로 센서만을 이용하열 구함.

Serial.print("AngleAcX1:");

Serial.print(angleAcX1);

Serial.print("\t FilteredX1:");

Serial.print(angleFiX1);

Serial.print("\t AngleAcY1 :");

Serial.print(angleAcY1);

Serial.print("\t FilteredY1:");

Serial.println(angleFiY1);

Serial.print("\t AngleGyZ1:");

Serial.print(angleGyZ1);

Serial.print("\t FilteredZ1:");

Serial.println(angleFiZ1);

Serial.print("AngleAcX2:");

Serial.print(angleAcX2);

Serial.print("\t FilteredX2:");

Serial.print(angleFiX2);

Serial.print("\t AngleAcY2 :");

Serial.print(angleAcY2);

Serial.print("\t FilteredY2:");

Serial.println(angleFiY2);

Serial.print("\t AngleGyZ2:");

Serial.print(angleGyZ2);

Serial.print("\t FilteredZ2:");

Serial.println(angleFiZ2);

Serial.print("\n");

}

​

void initSensor() {

Wire.begin();

Wire.beginTransmission(MPU1_ADDR); // I2C 통신용 어드레스(주소)

Wire.beginTransmission(MPU2_ADDR);

Wire.write(0x6B); // MPU6050과 통신을 시작하기 위해서는 0x6B번지에

Wire.write(0);

Wire.endTransmission(true);

}

​

void getData_1() {

​

Wire.beginTransmission(MPU1_ADDR);

Wire.write(0x3B); // AcX 레지스터 위치(주소)를 지칭합니다

Wire.endTransmission(false);

Wire.requestFrom(MPU1_ADDR, 14, true); // AcX 주소 이후의 14byte의 데이터를 요청

AcX1 = Wire.read() << 8 | Wire.read(); //두 개의 나뉘어진 바이트를 하나로 이어 붙여서 각 변수에 저장

AcY1 = Wire.read() << 8 | Wire.read();

AcZ1 = Wire.read() << 8 | Wire.read();

Tmp1 = Wire.read() << 8 | Wire.read();

GyX1 = Wire.read() << 8 | Wire.read();

GyY1 = Wire.read() << 8 | Wire.read();

GyZ1 = Wire.read() << 8 | Wire.read();

}

​

void getData_2() {

​

Wire.beginTransmission(MPU2_ADDR);

Wire.write(0x3B); // AcX 레지스터 위치(주소)를 지칭합니다

Wire.endTransmission(false);

Wire.requestFrom(MPU2_ADDR, 14, true); // AcX 주소 이후의 14byte의 데이터를 요청

AcX2 = Wire.read() << 8 | Wire.read(); //두 개의 나뉘어진 바이트를 하나로 이어 붙여서 각 변수에 저장

AcY2 = Wire.read() << 8 | Wire.read();

AcZ2 = Wire.read() << 8 | Wire.read();

Tmp2 = Wire.read() << 8 | Wire.read();

GyX2 = Wire.read() << 8 | Wire.read();

GyY2 = Wire.read() << 8 | Wire.read();

GyZ2 = Wire.read() << 8 | Wire.read();

}

​

​

// loop 한 사이클동안 걸리는 시간을 알기위한 함수

void getDT() {

now = millis();

dt = (now - past) / 1000.0;

past = now;

}

​

// 센서의 초기값을 10회 정도 평균값으로 구하여 저장하는 함수

void caliSensor_1() {

double sumAcX1 = 0 , sumAcY1 = 0, sumAcZ1 = 0;

double sumGyX1 = 0 , sumGyY1 = 0, sumGyZ1 = 0;

getData_1();

for (int i=0;i<10;i++) {

getData_1();

sumAcX1+=AcX1; sumAcY1+=AcY1; sumAcZ1+=AcZ1;

sumGyX1+=GyX1; sumGyY1+=GyY1; sumGyZ1+=GyZ1;

delay(50);

}

averAcX1=sumAcX1/10; averAcY1=sumAcY1/10; averAcZ1=sumAcY1/10;

averGyX1=sumGyX1/10; averGyY1=sumGyY1/10; averGyZ1=sumGyZ1/10;

}

void caliSensor_2() {

double sumAcX2 = 0 , sumAcY2 = 0, sumAcZ2 = 0;

double sumGyX2 = 0 , sumGyY2 = 0, sumGyZ2 = 0;

getData_2();

for (int i=0;i<10;i++) {

getData_2();

sumAcX2+=AcX2; sumAcY2+=AcY2; sumAcZ2+=AcZ2;

sumGyX2+=GyX2; sumGyY2+=GyY2; sumGyZ2+=GyZ2;

delay(50);

}

averAcX2=sumAcX2/10; averAcY2=sumAcY2/10; averAcZ2=sumAcY2/10;

averGyX2=sumGyX2/10; averGyY2=sumGyY2/10; averGyZ2=sumGyZ2/10;

}

​