Gimbal with Servo -2

젯슨나노에는 i2c 버스가 2개가 있다.

따라서 하나의 i2c는 모터를 제어하고, 하나의 i2c는 mpu를 사용하여 mpu의 값 변화로 2개의 모터를 제어해보겠다.

 

먼저 mpu6050을 사용할건데 해당 센서를 사용하기 위헤 데이터시트를 먼저 확인해준다.

 

https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf

 

mpu6050의 주소는 0x68인데 여기서 확인이 가능하다.

 

117번 즉, 0x75이 WHO_AM_I 라고 되어있고, 이것의 초기 값은 0x68임을 확인할 수 있다.

 

https://chigun.tistory.com/20

[MPU6050] 1. 데이터 시트를 통해 센서 값 확인하기

여기서 데이터 시트를 해석하면서 아두이노 코드를 짰으니 여기를 보면서 필요한 부분은 데이터 시트를 참고해 값을 얻자!

 

일단 나는 초기화 부분과 각가속도와 중력가속도가 필요하여 데이터 시트를 보면서 위 블로그를 참고하여 코드를 작성했다.

 

https://github.com/PCY00/Ubicomp_Lab/blob/main/AIoT/U-Neck/mpu6050/basic/basic.cpp

Ubicomp_Lab/AIoT/U-Neck/mpu6050/basic/basic.cpp at main · PCY00/Ubicomp_Lab

 

다음으론 위 코드와 서보모터 동작 코드를 합쳐 짐벌느낌을 낼 수 있도록 만들어 볼것이다.

 

https://github.com/PCY00/Ubicomp_Lab/blob/main/AIoT/U-Neck/MotorControl/Servo/V2.0/2_servo_with_mpu6050.cpp

Ubicomp_Lab/AIoT/U-Neck/MotorControl/Servo/V2.0/2_servo_with_mpu6050.cpp at main · PCY00/Ubicomp_Lab

 

위 코드에 대해 설명을 하겠다.

 

i2c 연결하는 부분은 이미 이전 포스팅에서 많이 다루었으니 그 부분을 제외하고 어떻게 mpu6050의 값으로 서보모터를 움직이는지에 대해 포스팅하겠다.

 

구분	설명
roll	X축을 기준으로 회전 좌우로 기울어진 정도 짐벌이 좌우로 기울어짐
pitch	Y축을 기준으로 회전 앞뒤로 기울어진 정도 짐벌이 앞뒤로 기울어짐

 

// 각도 계산 (Roll/Pitch)
void calculateAngles(int16_t* acc, float* roll, float* pitch) {
    *roll = atan2(acc[1], acc[2]) * 180.0 / M_PI;
    *pitch = atan2(-acc[0], sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * 180.0 / M_PI;
}

// 각도 -> PWM 변환
int angleToPWM(float angle) {
    return (int)((angle / 180.0) * (SERVO_MAX - SERVO_MIN) + SERVO_MIN);
}

 

이 부분이 핵심이다.

Roll: 기기의 Y, Z 축을 이용해 좌우 기울기를 계산.
Pitch: 기기의 X, Y, Z 축을 이용해 앞뒤 회전을 계산.
atan2: 삼각함수를 통해 각도를 라디안으로 계산.
180.0 / M_PI: 라디안을 도(degree) 단위로 변환.

 

https://blog.naver.com/codingbird/221766900497

MPU6050센서를 통한 각도계산

 

각도를 계산하는 방법은 위 블로그에서 자세히 설명되어 있으니 참고 바란다.

이렇게 얻은 각도값은 많은 노이즈가 발생한다.

 

제일 좋은 방법은 칼만필터를 사용하는 건데, 아직 내가 공부를 하지 않은 부분이라 먼저 gpt의 도움을 받고 구현한 다음 차차 이해해보겠다.

* 칼만필터는 나중에도 매우 유용하게 쓰일 필터라 공부를 시작해볼 예정이다.

 

https://github.com/PCY00/Ubicomp_Lab/blob/main/AIoT/U-Neck/MotorControl/Servo/V2.1/2_servo_with_mpu6050.cpp

Ubicomp_Lab/AIoT/U-Neck/MotorControl/Servo/V2.1/2_servo_with_mpu6050.cpp at main · PCY00/Ubicomp_Lab

 

https://youtu.be/xx1YAP6B_W0

 

오늘 포스팅은 이정도로 마무리 지을려한다.