[아두이노]mpu6050 을 이용하여 2축 서보모터 제어하기

아두이노와 같은 마이크로 칩을 이용하여 무엇인가를 해보다 보면 정말 놀라운 경험들을 많이 하게 됩니다. 그 중 하나가 바로 mpu6050 같은 센서가 될 수 있겠습니다.

요 조그만 칩을 이용하여 기울어진 정도나 각도, 나침반과 같은 방향 등을 알아낼 수 있다니 놀랍지 않으신가요?

요 조그만 칩이라고 했는데 이미지는 엄청 큰 mpu6050

오늘 소개해 드릴 내용은 RC Headtracking FPV 만들기의 두번째 스탭인 6축 자이로 센서를 이용한 움직임 신호 받기와 해당 신호를 이용 하여 2축 서보모터를 제어하는 내용 입니다.

헤드트래킹 무선 FPV 를 만들기 위하여 지난 포스트에서 2축 서보모터 마운트를 아주~ 아~~~주 간단하게 만드는 방법을 소개해 드렸었는데요, 해당 프레임에 부착한 서보모터를 이번 시간에 자이로 센서를 이용하여 제어를 해보도록 하겠습니다.

 

2022.01.31 - [DIY/Arduino] - 2축 서보모터 초간단 제어하기, 간단한 프레임 제작까지

2축 서보모터 초간단 제어하기, 간단한 프레임 제작까지

 

먼저 라이브러리를 받아야 할텐데요,  아래 첨부된 라이브러리를 다운 받으셔서 바로 내문서- Arduino-libraries 폴더에 압축을 해제하여 넣으시면 됩니다. 

 

MPU6050.zip

0.10MB

I2Cdev.zip

0.01MB

 

요렇게 넣으면 됩니다.

 

혹시 Wire.h 라이브러리가 없으시면 아래 파일도 다운 받으시면 됩니다.

Wire.zip

0.01MB

 

반응형

 

mpu6050 라이브러리를 받으시면 예제 소스코드가 들어있을 텐데요, 저는 여기서 시작 이후 실제 회전하는 값, 즉 상대 회전 값만을 이용할 계획이고 아래의 코드를 이용하여 회전 값을 추출할 계획입니다. 본 포스트에서 소개해 드리는 예제 외에도 라이브러리가 제공하는 예제 코드를 실행해보시면 다른 창작품 제작에도 도움이 될 수 있을 것 같습니다. ^^

 

기존 조이스틱 대신 이번에는 mpu6050 보드를 연결해야 하는데요. I2C 방식으로 연결할 것이므로 선은 4가닥만 있으면 되며 vcc 는 3.3v 에 연결해 주시고, GND 는 아두이노의 GND 에 연결, SDA 는 아두이노의 A4 번에 SCL 은 아두이노의 A5에 연결하시면 됩니다. (다른 핀은 안됩니다. 정확히 A4, A5에 연결해 주셔야 합니다)

 

코딩을 해보자

mpu6050 에 대한 자세한 내용은 이미 여러분들께서 다루어 주시고 있으므로 저는 실제 이번 프로젝트에 사용한 코드만 보여드리도록 하겠습니다.

 

#include <Servo.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

Servo myservo_LR;   // streering servo
Servo myservo_UD;  // 2speed gear box servo

int pin_servo_LR = 9;
int pin_servo_UD = 10;

int pin_x = A3;
int pin_y = A4;
int angle_x = 512 ;
int angle_y = 512 ;

void setup() {

  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
    Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif
  
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

 
  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // wait for ready
  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  //while (Serial.available() && Serial.read()); // empty buffer
  //while (!Serial.available());                 // wait for data
  //while (Serial.available() && Serial.read()); // empty buffer again

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(51);
  mpu.setYGyroOffset(8);
  mpu.setZGyroOffset(21);
  mpu.setXAccelOffset(1150);
  mpu.setYAccelOffset(-50);
  mpu.setZAccelOffset(1060);
  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    Serial.println();
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
    Serial.println(F(")..."));
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  
  myservo_LR.attach(pin_servo_LR);
  myservo_LR.write (90);
  myservo_UD.attach(pin_servo_UD);
  myservo_UD.write (90);
}

void loop() {

  // if programming failed, don't try to do anything
  if (!dmpReady) return;
  // read a packet from FIFO
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 

  #ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    
    float angle_x = (ypr[0] * 180 / M_PI) * -1 + 90 ; //* -1 + 90;
    float angle_y = (ypr[1] * 180 / M_PI) + 90 ; //* -1 + 30;    
   
    if (angle_y < 60) angle_y = 60;
    if (angle_y > 120) angle_y = 120;
    
    if (angle_x < 30) angle_x = 30;
    if (angle_x > 150) angle_x = 150;

    myservo_LR.write (angle_x);  
    myservo_UD.write (angle_y);
  
    Serial.print("ypr\tx:");
    Serial.print(int(angle_x));
    Serial.print("\ty:");
    Serial.print(int(angle_y));
    Serial.print("\t");
    
    

  #endif
  }
  Serial.println(".");
  delay(10);
  
}

 

코드가 조금 길어졌기는 하지만 기존 첫번째 과정에 mpu6050 관련 코드가 추가되었고 크게 달라진것은 없습니다. mpu6050 관련된 상세한 코드는 저도 잘 모르고요. 그냥 예제에서 긁어온 코드입니다. ^^;; (불필요한 내용이 들어 있을 수도 있고요... 어쨌든 위의 코드면 잘 실행됩니다.)

위 코드에서 아래 부분이 실제 회전 값을 받아서 내가 원하는 각도로 세팅하는 부분인데요. 입력 값이 최초 실행된 위치로부터의 상대 값이므로 가만히 있으면 0 이 들어오게 됩니다. 그러므로 기준이 되는 앵글을 더해주면 기준이 되는 앵글에  + - 로 각도가 변경되게 되며 저의 x 위치에 -1 이 들어있는 것은 서보 모터의 설치 방향 때문에 방향을 뒤집어 주기 위함입니다. Y 값 역시 서보모터의 방향이 저와 다른 방향으로 설치되어 반대로 움직인다면 센서의 값에 -1 을 곱해 주시면 됩니다.

	float angle_x = (ypr[0] * 180 / M_PI) * -1 + 90 ; //* -1 + 90;
    float angle_y = (ypr[1] * 180 / M_PI) + 90 ; //* -1 + 30;    
   
    if (angle_y < 60) angle_y = 60;
    if (angle_y > 120) angle_y = 120;
    
    if (angle_x < 30) angle_x = 30;
    if (angle_x > 150) angle_x = 150;

 

어쨌든 서보모터 제어를 위한 부분은 이전에 소개해 드린 코드 그대로 가져왔고요, 회전 정보를 받아 서보모터로 전달하는 과정에서 축의 기준위치, 방향 등을 변경해주기 위하여 위와 같이 약간의 코드가 추가되었습니다. if 구문을 이용하여 입력된 값 + 기준값이 모터의 최대 회전 범위 보다 크다면 최대 회전 범위의 값으로 설정을 해주면 됩니다.

 

저는 기준위치를 서보의 상하, 좌우 90도를 기준으로 설정하였고 상하로는 ±30도씩, 좌우로는 ±60도씩 움직일 수 있도록 하였습니다.

참 쉽죠?

 

이것은 동영상은 아닙니다. 

 

과연 잘 동작할런지??

자 구동 되는 모습은 아래와 같습니다.

 

 

 

아주 잘 되죠?

기존 조이스틱이 움직이는 범위에 비하여 출력되는 범위가 리니어 하지 않은지 휙휙 움직이던것에 비하면 아주 아주 잘 동작하는 것을 확인할 수 있습니다. 

 

이렇게 mpu6050 6축 지자기센서를 이용하여 2축 서보모터를 제어하는 것까지 마쳤습니다. 

생각보다 어렵지 않죠?

다음은 이제 대망의 무선 입니다. 

다음 포스트를 기대해 주세요.

2022.01.31 - [DIY/Arduino] - [아두이노]NRF24L01을 이용하여 무선으로 mpu6050 신호 전달하기

[아두이노]NRF24L01을 이용하여 무선으로 mpu6050 신호 전달하기

 

2022.01.25 - [DIY/RC] - 20년된 엔진 RC 카 전동화 해보자 [kyosho inferno]

20년된 엔진 RC 카 전동화 해보자 [kyosho inferno]

 

2022.01.31 - [DIY/Arduino] - 2축 서보모터 초간단 제어하기, 간단한 프레임 제작까지

2축 서보모터 초간단 제어하기, 간단한 프레임 제작까지

 

2020.11.29 - [DIY/RC] - 1/24 완벽한 RC 카 RGT ADVENTURE

1/24 완벽한 RC 카 RGT ADVENTURE