아두이노 - 쿼드콥터 제작 #4

이번에는 쿼드콥터의 자세를 제어하는 과정입니다.  자이로 센서를 아두이노에 아래의 링크와 같이 연결하고 코딩을 따라해보시기 바랍니다. 그리고, 프로그램 실행 이후 자이로 센서를 기울였을 때 AcX와 AcY의 값이 어떻게 변하는 지 유심히 관찰하시기 바랍니다.

• http://flit.kr/bangled-reading-accelerometer/

 

[그림 1]

[그림 2]는 위의 링크를 토대로 자이로센서의 동작을 확인한 이후 자세 제어를 위한 공식을 고민하는 아이의 모습입니다.  수평인 상태에서의 수치를 알아낸 이후 왼쪽/오른쪽, 윗쪽/아래쪽 으로 기울어 졌을 때, 어느 프로펠러를 얼마나 더 힘차게 돌려야 할 지를 계산하는 과정입니다.  기본적인 원리를 아이와 함께 고민해 보고 최종 공식을 아이가 스스로 찾아 낼 수 있도록 혼자만의 시간을 주었습니다.

[그림 2] 자세제어 공식을 연구중인 아이

수평 상태의 쿼드콥터에 부착 된 자이로 센서의 AcX 값은 -800, AcY는 -150 정도입니다.  이 값을 기준으로 아래와 같은 공식을 만들어 냈습니다.

• 왼쪽으로 기울어 졌을 때: u = x + 800
• 1, 3번 모터 속도를 올려줍니다.
• 오른쪽으로 기울어 졌을 때: u = -x - 800
• 2, 4번 모터 속도를 올려줍니다.
• 윗쪽으로 기울어 졌을 때: u = -y - 150
• 1, 2번 모터 속도를 올려줍니다.
• 아랫쪽으로 기울어 졌을 때: u = y + 150
• 3, 4번 모터 속도를 올려줍니다.
• 기타
• u는 기울어짐 정도를 나타냅니다.
• x = AcX
• y = AcY

즉, 왼쪽으로 기울어러졌을 때, AcX 값이-700이면 100정도의 힘으로 [그림 3]의 1번과 3번 모터를 다른 모터보다 더 돌려서 자세를 바로 잡습니다.  AcX 값이 커질 수록 왼쪽으로 기울짐이 커졌다는 의미가 됩니다.  따라서, u 값도 같이 증가하도록 합니다.  과연 아이와 제 생각대로 움직여 줄지 살짝 걱정이 됩니다 ^^;

[그림 3] 모터의 번호 지정

[소스 1]

#include <wire.h>

const int MPU=0x68;

const int pinM01 = 6;
const int pinM02 = 7;
const int pinM03 = 8;
const int pinM04 = 9;

int baseU = 0;

int baseX = -800;
int baseY = -150;

int m01 = baseU;
int m02 = baseU;
int m03 = baseU;
int m04 = baseU;

int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

void setup()
{
  Serial.begin(9600);

  pinMode(pinM01, OUTPUT);
  pinMode(pinM02, OUTPUT);
  pinMode(pinM03, OUTPUT);
  pinMode(pinM04, OUTPUT);

  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}

void loop()
{
  getData();
  
  m01 = baseU;
  m02 = baseU;
  m03 = baseU;
  m04 = baseU;
    
  if (AcX > -800) doLeft();
  else if (AcX < -800) doRight();
  
  if (AcY < -150) doUp();
  else if (AcY > -150) doDown();
  
  if (m01 > 255) m01 = 255;
  if (m02 > 255) m02 = 255;
  if (m03 > 255) m03 = 255;
  if (m04 > 255) m04 = 255;

  analogWrite(pinM01, m01);
  analogWrite(pinM02, m02);
  analogWrite(pinM03, m03);
  analogWrite(pinM04, m04);

  Serial.print("m01 = "); Serial.println(m01);
  Serial.print("m02 = "); Serial.println(m02);
  Serial.print("m03 = "); Serial.println(m03);
  Serial.print("m04 = "); Serial.println(m04);
}

void doLeft()
{
  Serial.println("doLeft");
  
  int u = (AcX + 800) / 10;
  
  m01 = m01 + u;
  m03 = m03 + u;
}

void doRight()
{
  Serial.println("doRight");
  
  int u = (-AcX - 800) / 10;
  
  m02 = baseU + u;
  m04 = baseU + u;
}

void doUp()
{
  Serial.println("doUp");
  
  int u = (-AcY - 150) / 10;
  
  m01 = m01 + u;
  m02 = m02 + u;
}

void doDown()
{
  Serial.println("doDown");
  
  int u = (AcY + 150) / 10;

  m03 = m03 + u;
  m04 = m04 + u;
}

void getData() 
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  
  Wire.endTransmission(false);

  Wire.requestFrom(MPU, 14, true);  

  AcX = Wire.read() << 8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY = Wire.read() << 8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read() << 8 | Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}

5-8: 각 모터 드라이버(TIP-120)에 연결할 핀 번호입니다.  [그림 3]의 번호 대로 연결하여 줍니다.

10: 수직 강하 수준을 나타내는 변수입니다.  baseU 가 200 으로 된다면 모터에 연결 된 핀들에 200 이라는 값이 출력되어 모든 모터가 그만큼의 속도로 회전하게 됩니다.  이 수치가 낮아지면 속도도 같이 낮아집니다.  0부터 255 사이의 값이면 됩니다.  나중에는 스마트 폰이나 원격 조정기를 이용하여 이 값을 변경하여 쿼드콥터를 올리고 내리는 기능을 담당하게 됩니다.

12: 수평 일 때, 자이로 센서의 X 축 기울기 값입니다.  (추후 스마트 폰으로 재설정 가능하도록 할 계획입니다)

13: 수평 일 때, 자이로 센서의 Y 축 기울기 값입니다.  

15-18: 각 모터마다 baseU 보다 얼마나 더 속도를 올리느냐가 결정됩니다.  만약, "m01"이 "10" 이라는 값을 가지게 된다면, 1번 모터는 다른 모터보다 10만큼 더 속도를 내게 됩니다.  (물론 다른 값들은 0 일 경우)

20: 자이로 센서의 수치를 읽기 위한 변수들입니다.  지금은 AcX, AcY만 사용하고 있습니다.

31-35: 자이로 센서를 준비합니다.

40: 자이로 센서에서 수치를 읽어 옵니다.

42-45: 모든 모터의 속도를 초기화 합니다.

47-48: AcX 가 -800 보다 크면 왼쪽으로 기울어졌다는 의미입니다.  doLeft() 함수를 이용해서 수평을 유지하도록 합니다.  -800 보다 작으면 오른쪽으로 기울어졌다는 의미입니다.  doRight() 함수를 이용해서 수평을 유지하도록 합니다.

50-51: AcY 가 -150 보다 크면 아랫쪽으로 기울어졌다는 의미입니다.  doDown() 함수를 이용해서 수평을 유지하도록 합니다.  -150 보다 크면 윗쪽으로 기울어졌다는 의미입니다.  doRight() 함수를 이용해서 수평을 유지하도록 합니다.

53-56: 계산 결과가 255를 넘지 않도록 합니다.  (아이가 쿼드콥터를 최대 속도로 올리면 어떻게 하느냐고 묻습니다.  아이에게 해결 방법을 부탁해 둔 상태입니다.  실제 비행 때까지 찾아내는 지 봅시다 ^^)

기울어진 쪽 모터의 속도는 u = (-AcY - 150) / 10 와 같이 그 차이를 10으로 나눠서 사용했데, 이는 테스트 시에 확히 드러나도록 한 조치입니다.  실제는 100이나 50정도로 나눠서 사용 할 예정입니다.  (이 부분은 나중에 시행착오를 통해서 값을 찾아나갈 계획입니다 ^^;  웬지 로그를 이용해야 할 거 같은...) 

[동영상 1] 테스트

[동영상 1]은 코딩한 내용을 테스트 녹화한 것입니다.  기울어진 쪽의 프로펠러가 더 힘차게 돌면서 자세를 잡으려고 하고 있습니다.