쿼드콥터 제작기 12 - 보드간 I2C통신

Posted by dw0rdptr
2016. 3. 16. 15:08 IoT/QuadCopter

이전글에서 송신기의 PPM 신호를 받아오는데에서 문제가생겨 아두이노보드를 서브와 메인으로 나눠 두개를 사용하기로 했다. 


메인 아두이노(Arduino Uno)

- mpu6050 센서에서 각도값 도출

- 서브아두이노에서 받아온 제어목표값과 센서데이터를 이용, PID제어

- 모터출력 담당



서브 아두이노(Arduino Nano) 

- RC조종기의 PPM신호 해석 

- PPM에서 해석한 PWM신호에서 목표 스로틀과 YAW, PITCH, ROLL 각도를 map함수로 스케일링 후 메인 아두이노로 전달

 * 스로틀범위는 830~2000으로 잡았다. PID제어를생각해 ESC신호의 최대범위보단 조금 낮춰두는게 안전할 것 같아서 최대 스로틀은 2000으로 제한했다.

   PITCH와 ROLL은 -30~30으로 잡아두었다. 단위는 목표각도니까 degree. YAW는 대충 -20~20으로 맞췄다.



RC Receiver -> 서브아두이노(Nano)에서 신호해석, 스케일링 -> 메인아두이노(Uno)로 전달


나노에서 우노로 보드간 통신을 하는방법은 I2C, UART, SPI가있다. 그 중 간단하게 구현가능한 I2C를 사용했다.

MPU6050에서 데이터를 얻어올 때에도 사용하는 I2C에 대해 간단하게 설명하면


- I2C는 필립스에서 개발한 직렬 컴퓨터 버스이며, 마더보드, 임베디드 시스템, 휴대전화 등에 저속의 주변기기를 연결하기 위해 사용

- 1:N연결을 지원하고 Master-Slave 모드로 통신

- 슬레이브 선택을 위해 소프트웨어적인 주소를 사용하므로 연결하는 장치가 늘어나도 사용하는 핀이 증가하지 않는다

- 데이터 송수신을 위한 SDA와 동기화 클록을 위한 SCL 핀 2개만 사용.(2-Wire)

- UART나 SPI와 비교할 때 속도의 한계가 있으므로 사용에는 제한 그러나 아두이노에서는 Wire라이브러리에서 슬레이브 모드를 지원하고, UART와 달리 1:N통신이 지원되므로 비교적 간단하게 다양한 기능 구현이 가능



I2C통신을위한 핀은 위 그림처럼 우노와 나노 각각의 SCL, SDA핀을 연결해준다. 참고로 A5핀이 SCL, A4번핀이 SDA이다. 

리시버핀 연결은 전글을 참고. 전원은 한쪽보드에서만 공급하고 5v-Vin, GND-GND 연결하면 나머지보드도 전원공급이 된다.



Receiver.ino

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
#include <PinChangeInt.h>
#include "Wire.h"
 
#define RC_Ch1 3
#define RC_Ch2 4
#define RC_Ch3 5
#define RC_Ch4 6
 
#define ROLL_MIN -30
#define ROLL_MAX 30
#define PITCH_MIN -30
#define PITCH_MAX 30
#define YAW_MIN -20
#define YAW_MAX 20
 
#define ESC_MIN 800
#define ESC_MAX 2200 
 
 
volatile unsigned long LastCh1 = micros();
volatile unsigned long LastCh2 = micros();
volatile unsigned long LastCh3 = micros();
volatile unsigned long LastCh4 = micros();
 
 
 
 
volatile float Ch1;
volatile float Ch2;
volatile float Ch3;
volatile float Ch4;
 
uint16_t Throttle=0;
int8_t TargetROLL, TargetPITCH, Yaw;
 
void setup() {
  Wire.begin(4);
  Wire.onRequest(Send);
 
  //Serial.begin(9600);
 
  PCintPort::attachInterrupt(RC_Ch1, Ch1_Interrupt, CHANGE);
  PCintPort::attachInterrupt(RC_Ch2, Ch2_Interrupt, CHANGE);
  PCintPort::attachInterrupt(RC_Ch3, Ch3_Interrupt, CHANGE);
  PCintPort::attachInterrupt(RC_Ch4, Ch4_Interrupt, CHANGE);
 
}
 
void loop() {
 
    TargetROLL = map(Ch1, 11001900, ROLL_MIN, ROLL_MAX);
    TargetPITCH = map(Ch2, 11001900, PITCH_MIN, PITCH_MAX);
    Throttle = map(Ch3, 11301800, ESC_MIN, ESC_MAX);
    Yaw = map(Ch4, 11001800, YAW_MIN, YAW_MAX);
 
     
}
 
 
void Ch1_Interrupt() {
    Ch1 = micros() - LastCh1;
  LastCh1 = micros();
}
 
void Ch2_Interrupt() {
    Ch2 = micros() - LastCh2;
  LastCh2 = micros();
}
 
void Ch3_Interrupt() {
    Ch3 = micros() - LastCh3;
  LastCh3 = micros();
}
 
void Ch4_Interrupt() {
    Ch4 = micros() - LastCh4;
  LastCh4 = micros();
}
 
 
void Send() {
 
  byte bufferArr[5];
  
  bufferArr[0= (Throttle >> 8) &0xFF;
  bufferArr[1= Throttle & 0xFF;
  bufferArr[2= TargetROLL & 0xFF;
  bufferArr[3= TargetPITCH & 0xFF;
  bufferArr[4= Yaw & 0xFF;
  
  Wire.write(bufferArr,5);
  
}
 
cs


서브아두이노(Nano)의 전체 코드이다.

우선 인터럽트로 RC신호를 해석해 Throttle, TargetROLL, TargetPITCH, Yaw제어목표값을 map함수로 제어에 쓸수 있게 스케일링하였다.


I2C로 데이터를 넘기는 과정을 간단하게 설명하면

Wire.begin(4); -> 4번 슬레이브장치로 등록

Wire.onRequest(Send); -> 마스터장치(Uno)에서 데이터를 요구하면 Send함수 실행


Send함수에서 bufferArr[5] 바이트배열 버퍼를 선언 후 구한 Throttle, TargetROLL, TargetPITCH, Yaw값을 버퍼에 저장 (Throttle은 16bit, 2byte 차지하므로 한번 밀어준다.)


Wire.write(bufferArr,5); -> 마스터장치로 bufferArr 5byte 전송


Quad_main.ino

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
#include "I2Cdev.h"
#include "Wire.h"
 
 
#define ROLL_MIN -30
#define ROLL_MAX 30
#define PITCH_MIN -30
#define PITCH_MAX 30
#define YAW_MIN -20
#define YAW_MAX 20
 
#define ESC_MIN 800
#define ESC_MAX 2200 
 
#define SamplingTime 0.01
 
 
uint16_t Throttle,  LastThrottle=0;
int8_t TargetROLL;
int8_t TargetPITCH;
int8_t TargetYAW;
int8_t LastTargetROLL, LastTargetPITCH,LastYAW;
 
byte a,b,c,d,e;
 
void setup() {
  Wire.begin();
  TWBR = 24;
 
}
 
void loop() {
    RC_Command();
}
 
 
 
void RC_Command() {
 
  Wire.requestFrom(45);
 
  
  a = Wire.read();
  b = Wire.read();
  c = Wire.read();
  d = Wire.read();
  e = Wire.read();
  
  Throttle = a;
  Throttle = (Throttle << 8| b;
  TargetROLL = c;
  TargetPITCH = d;
  TargetYAW = e;
 
  
    if (Throttle < ESC_MIN || Throttle > ESC_MAX)
      Throttle = LastThrottle;
 
    if (TargetROLL < ROLL_MIN || TargetROLL > ROLL_MAX)
      TargetROLL = LastTargetROLL;
 
    if (TargetPITCH < PITCH_MIN || TargetPITCH > PITCH_MAX)
      TargetPITCH = LastTargetPITCH;
 
    if (TargetYAW < YAW_MIN || TargetYAW > YAW_MAX)
     TargetYAW = LastYAW;
      
    if (TargetROLL >= -3 && TargetROLL <= 3 )
      TargetROLL = 0;
    
    if (TargetPITCH >= -3 && TargetPITCH <= 3 )
      TargetPITCH = 0;
 
    if(TargetYAW >= -3 && TargetYAW <= 3)
      TargetYAW = 0;
 
 
 
    LastThrottle = Throttle;
    LastTargetROLL = TargetROLL;
    LastTargetPITCH = TargetPITCH;
    LastYAW = TargetYAW;
 
    
  }
  
 
cs
d

메인아두이노(Uno)코드중 서브아두이노에서 데이터를 요청하는 부분이다.

RC_Command 함수를 보면 

Wire.requestFrom(4, 5); -> 슬레이브장치 4번에서 5byte의 데이터 요청.


a = Wire.read();

b = Wire.read();

c = Wire.read();

d = Wire.read();

e = Wire.read();   -> byte형변수 5개 선언. I2C통신은 한번에 1byte씩 받으므로 Wire.read();로 읽은 바이트데이터를 a부터 e까지 저장해준다.


Throttle = a;

Throttle = (Throttle << 8) | b;

TargetROLL = c;

TargetPITCH = d;

TargetYAW = e;            -> 각각의 변수에 대입해주면 된다


if문으로 가끔 크게 튀는 노이즈를 제거해주었고 조종기를 건들지않았을때 YAW PITCH ROLL이 0이되도록 트림을 맞춰주면 조종기설정은 끝

PID게인도 거의 맞춰가는 것 같고 다음글은 비행테스트가 될 것 같다. 


쿼드콥터 제작기 11 - PPM과 아두이노 Interrupt

Posted by dw0rdptr
2016. 3. 1. 22:37 IoT/QuadCopter

hobbyking에서 리시버와 조종기를 구매했다. 모델명은 HK-T6A V2 Transmitter&Receiver 2.4Ghz

송신기에서는 PWM신호를 직접 전달하는데 6채널 송신기의 경우 채널별로 에일러론(CH1), 엘리베이터(CH2), 쓰로틀(CH3), 러더(CH4) 외 두가지 PWM 신호, 총 6가지 PWM신호를 포함하게 되는 PPM 신호를 송신한다. 수신기에서는 송신된 무선전파를 수신하여 PPM신호을 검출한다. 검출한 PPM신호를 MCU(컨트롤러 보드)로 전달하고, MCU에서는 PPM신호에서 개별 채널의 PWM신호를 뽑아내는 것이 기본적인 RC PPM신호의 사용이다.

PWM이 Pulse Width Modulation(펄스 폭 변조)라면, PPM은 Pulse Positon Modulation(펄스 위치 변조)인 것이다.


   표준 RC 수신기 block diagram

출처 : http://skymixer.net/electronics/84-rc-receivers/78-rc-ppm-signal

위 그림에서 PPM Frame Decoder가 MCU, 즉 아두이노라고 생각하면 된다. 

그림처럼 아두이노(MCU)와 수신기를 연결해야 하는데 그전에 먼저 송신기와 수신기를 바인딩 시켜야 한다. 이건 제품마다 다르니 제품명을 검색해서 찾으면 된다.


위처럼 5v와 GND핀에 연결, 각 채널 시그널핀은 디지털핀에 연결하면 아두이노와 수신기 연결은 끝.

이제 문제는 아두이노에서 어떻게 PPM신호를 해석해 PWM신호를 뽑아내냐는 것인데..이는 아두이노의 Interrupt로 간단하게 구현 가능하다.


인터럽트에 대해 간단히 설명을 하자면, 지정된 핀의 상태가 원하는 조건과 일치하면 미리 등록한 인터럽트 callback 함수, 즉 ISR (Interrupt Service Routines 인터럽트 서비스 루틴)을 자동으로 호출해주는 기능이다. 이때 실행중이던 loop() 함수 안의 루틴은 인터럽트 콜백 함수가 끝날 때 까지 대기상태가 된다.

쉽게말해 어떤 핀의 상태가 특정한 조건이 되면 loop()를 멈추고 그동안 사용자가 등록한 함수(ISR)를 실행하고 등록한 함수가 끝나면 다시 loop()함수로 되돌아오는게 인터럽트이다.


다음 단계를 통해 PPM신호의 개별 채널을 감지하고 개별 채널의 PWM ON(High)시간을 측정할 수 있다.


1. 상승에지 감지(Rising Edge Sense)를 하면 인터럽트 서비스 루틴이 실행됨

2. Timer1의 카운터값을 기록하고, 센싱모드를 하강에지감지(Falling edge sense)로 바꾼다.

3. 인터럽트 서비스 루틴을 빠져 나간다.

4. 하강에지 감지(Falling Edge Sense)를 하면 인터럽트 서비스 루틴이 실행된다.(위에서 하강에지감지 모드로 바꾸었기 때문)

5. Timer1의 카운터값을 기록하고, 센싱모드를 상승에지감지(Rising Edge sense)로 바꾼다.

6. 펄스의 ON시간을 계산한다.(펄스의 ON 시간 계산하기: 두가지 경우가 있음)

// 경우1: 하강에지 시각이 상승에지보다 후(뒤)일때 : ON시간 = (후 - 전)

// 경우2: 하강에지 시각이 상승에지보다 전(앞)일때 : ON시간 = (카운터최대값 +전 - 후)

7. 인터럽트 서비스 루틴을 빠져 나간다.

8. 상승에지 인터럽트 감지하면, 위의 1번부터 차례대로 진행.(무한 반복)


출처 : AVR과 RC PPM 신호 (3편) - PPM 신호감지를 위한 AVR을 소스코드

좀더 자세하게 알고 싶으면 위 출처의 링크를 보면 된다.


어찌저찌 구현을 했는데..

예상치 못한 문제가 생겨버렸다. RC신호를 받으려고 인터럽트 서비스 루틴을 실행하는 중에는 PID제어값, 특히 D제어값이 이상하게 튀어버린다. 시소테스트를 하는 도중 틱 틱 소리가 났었는데 D제어값이 튄것이 그 이유였다.


500~600이상 D제어 값이 튀어버린다.


그래서 여기저기 도움을 청하고 구글링을 하다 내린 결론은 아두이노가 RC 신호 수신 대기시간동안은 PID제어를 못한다는 것이었다.

그래서 서브 아두이노 보드를 추가해 수신기를 연결하고 RC PPM 신호를 받아 해석한 각 PWM 신호 범위를 필요에 맞게 스케일링 한 뒤 보드간 통신으로 PID제어와 모터 출력을 담당하는 메인 아두이노 보드로 넘겨주는 방법으로 해결했다.


자세한 방법은 다음포스팅에~





쿼드콥터 제작기 10 - 쿼드콥터 프로젝트 reboot

Posted by dw0rdptr
2016. 1. 25. 04:13 IoT/QuadCopter


이전까진 제대로된 정보가 별로 없었지만(구글링을 못해서...) 여러 블로그를 참조해가면서 앞으로 바꿔야할점이나 계획을 정리해보았다.



이중PID제어 구현

쿼드콥터는 기존 PID제어기로 제어하기에는 다소 불안정하다고 한다. 그래서 각도로 계산한 에러에 따로 P게인을 적용한 후 현재각속도와의 차이를 에러값으로

PID알고리즘을 적용하는 이중 PID를 쓴다고 한다. 아래는 소스코드

 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
TargetAngle //목표각도
CurrentAngle //현재각도
CurrentRate //현재각속도
 
AngleError = TargetAngle - CurrentAngle; //각도에러 = 목표각도 - 현재각도
 
RateError = (AngleError*stabilizePgain) - currentRate; // 각속도에러 = (각도에러*Pgain) - 현재각속도
 
 
= RateError*Pgain;
+= (RateError*dt)*Igain;
= ((RateError - LastRateError)/dt)*Dgain;
 
 
PID = P+I+D;
cs

http://red-black.tistory.com/entry/멀티콥터-9-이중-PID-컨트롤


MPU-6050의 DMP 사용

MPU-6050은 센서내부에서 가속도센서와 자이로센서로 각도를 구해주는 필터가 있다고 한다.

아마 하드웨어적으로 처리하는거니까 상보나 칼만필터보다 진동에도 강할거고

무엇보다 아두이노의 처리량을 줄일 수 있다는게 가장 좋은 점 같다. 구한 예제소스가 길지만 대부분 초기설정값이라 분석하는데

오래걸리진 않을 것 같다.


2016.3.20 수정

DMP예제를 쓸경우 센서에서 처리한 값을 기다리는 시간이 생겨 제어주기를 100hz이상 올릴 수 없다고 합니다. 상보필터를 쓰고 우노의 경우 하드웨어적으로 방진처리를 하는 대안으로 진행중입니다.


ESC출력으로 Servo 라이브러리 사용

처음 쿼드콥터를 제어하기 위해선 400HZ는 되어야 가능하기 때문에 라이브러리가 기본 50HZ로 설정이 되어있는 Servo 라이브러리 대신 analogWrite()

로 진행했지만 analogWirte()가 0~255범위로 제어할 수 있는 반면에 servo의 writeMicroseconds() 함수는 600~2400까지 있어 훨씬 세밀하게 제어가

가능한데다가 servo.h 헤더파일을 수정해 주파수를 끌어올릴 수 있는 방법을 찾아 Servo라이브러를 이용해 ESC에 출력을 전달할 생각이다.

http://forum.arduino.cc/index.php?topic=115545.0

REFRESH_INTERVAL과 MAX_SERVOS 값을 바꿔 쿼드콥터를 안정적으로 제어할 수 있는 주파수까지 끌어올릴 수 있다.



보드와 송&수신기 교체

우선 돈이 없어 Rpino보드를 사진 못하고.. 우노 호환보드를 구해 진행하려고 한다.

어차피 라즈베리파이로 연동하는건 전원공급용이고 라즈베리파이로 구현하려는 카메라기능도 독립적인 기능이기 때문에 굳이 RPino보드가 아니어도 되긴 하는지라..


그리고 기존 무선모듈이 고장나서 눈물을 머금고 조종기&리시버 세트 구매...

hobbyking HK-T6A V2 6ch 조종기&리시버, 10x4.5 프롭, 아두이노 우노 호환보드


60만원 돌파!   고쓰리라 여기 매달려 살수는 없지만 틈틈히 하면 여름전엔 띄울수 있을거라는건 내 기대뿐이고 얼마나 더 걸릴지는 모른다. 언젠간 날리겠지뭐



쿼드콥터 제작 근황

Posted by dw0rdptr
2015. 12. 17. 02:41 IoT/QuadCopter

기말고사기간이라 포스팅이 요근래 없었는데, 앞으로 PID조정과 자잘한 보완빼곤 많이 남진 않았지만 이제 고3이라 입시준비도 있고 이런저런일들이 많아 이전처럼 개발속도가 나오지 않을것같다. 올해 안으로 끝내겠다는 자신감은 어디가고...ㅋㅋㅋ


일단 무선모듈이 나가버렸다.. 안드로이드 usb시리얼모듈사용도 사실상 어려움이많아 개발방향을 바꾸기로 결정.

송수신기(조종기)를 구매해 컨트롤하고 조종기위에 핸드폰을 부착할수 있게 만들어 라즈베리-블루투스통신으로 영상송신을 하는방향으로 진행하려 한다.

후에 여건이 되면 안드로이드 프로그래밍을 제대로 배워 다시 시도해야겠다


아무튼 얼마전 첫 비행테스트를 했는데, 한쪽으로 기울어져 자세제어를 전혀 하지못해 프롭을 날려기도 했다

원인은 아마 너무낮은 모터출력에서 PID튜닝을 해서 그런것같다. 무게중심도 균형있게 설계가 필요할듯!


쿼드콥터 제작기 09 - LPF(Low Pass Filter)구현

Posted by dw0rdptr
2015. 10. 15. 00:52 IoT/QuadCopter

저번글[동]에서 시소테스트를 하는데 모터출력이 튀는 원인인 센서진동을 잡기위해 LPF를 구현하려고 한다


LPF(Low Pass Filter)

저역통과필터라고도 하며, 특정값 이하의 저주파는 통과시키고 그이상 고주파를 걸러내는 필터이다.

다.


MPU6050의 DLPF_BW LPF다.

http://hs36.tistory.com/search/lpf


D제어에만 저역통과필터링을 하는 이유는...

센서값 진동은 D뿐만 아니라 P나 I제어에도 영향을 주지만, 순간적인 값변화에 영향을 제일 많이 받는 D제어에만 저역통과필터를 구현해도

안정화 될 것이고 쿼드제어의 생명이 응답속도인데 저역통과필터를 거치면 딜레이가 생기기도 해서.. ㅠㅠ

아무튼, PID튜닝도 어느정도 됐고 이문제만 해결하면 드디어 첫비행 테스트를 할 수 있다!




 출처 - http://pinkwink.kr/437

 코드로 변환하면


val = (tau*pre_val + ts*x)/(tau + ts);

예상외로 간단하다. 여기서 val은 저역통과필터의 결과이고 pre_val은 이전결과값, x가 입력값, ts는 샘플링주파수이다.  

tau는 시정수라고하는데 이값이 크면 클수록 데이터가 깔끔하게 나오지만 그만큼 지연시간이 길어진다.


-

프 - 수(tau) 0.001

한눈에 봐도 노이즈가 많이 줄었긴 하지만 시정수를 좀 더 크게 줘야 할 것 같다.


노란색 그래프 - 시정수(tau) 0.001
녹색 그래프 - 시정수(tau) 0.01

 이정도면 괜찮겠지..? 약간의 지연이 있지만 진동하는것보다야 훨씬 나을 것 같다.



쿼드콥터 제작기 08 - PID 튜닝, 시소테스트

Posted by dw0rdptr
2015. 10. 13. 01:59 IoT/QuadCopter

기체마다 추력, 크기나 무게 등 여러 변수가 많기 때문에 기체에 맞게 PID제어를 각각 어느정도 해줘야 하는지에 대한 상수값을 정해줘야 하는데 이를 PID 튜닝이라고 한다
PID튜닝에는 스텝 응답법과 한계 감도법등 방법이 여러가지가 있는데, 그냥 노가다하는것도 나쁘지않다 ㅎ

상수를 결정하고 D와 I를 차례로 조정한 뒤 전체적으로 조정해나가는 방식으로 튜닝을 하고 있다.

(1) 목표값을 변화한 후, 응답이 너무 느리다.

→ P-게인(Kp)을 올린다. 응답은 빠르나 불안정하다.

→ P-게인(Kp)을 내린다.


(2) 목표값과 피드백값이 같아지지 않는다.

→ 적분 시간 Ti를 감소시킨다. 불안정하게 진동하며 일치한다.

→ 적분 시간 Ti를 증가시킨다.


(3) Kp를 올린 후, 응답이 여전히 느리다.

→ D-게인(Kd)을 올린다. 여전히 불안정하다.

→ D-게인(Kd)을 내린다.

*출처 : http://www.hardcopyworld.com/ngine/aduino/index.php/archives/650


시소테스트 영상

계속 모터출력이 튀는데 아마

모터진동 -> 센서값진동-> D제어에 영향

이 원인이 되는 것 같다




쿼드콥터 제작기 07 - PID제어

Posted by dw0rdptr
2015. 9. 9. 01:30 IoT/QuadCopter

이제 PID로 자세를 제어하는일만 남았다..

PID제어란 비례-미분-적분제어의 약자로

P - 비례(Proportional)
I  - 적분(Integral)
D - 미분(Derivative)


쿼드콥터에서의 PID는 센서에서 나오는 각도값을 받아서 원하는 목표각도를 유지하기 위해 쓰는 알고리즘이다.
예를 들어 현재 기체가 30도로 기울어져있으면 PID 제어기를 거쳐 기울어진 쪽의 모터출력을 높여주면 수평을 되찾을 수 있다.

처음엔 P 비례제어만써서 기울어진만큼만 모터출력을 높이면 되겠지 라고 생각했는데 그렇게 안하는데엔 다 이유가 있더라ㅠ


P 비례제어

P-비례제어는 말그대로 기울어진 각도에 비례해 출력을 높이는 제어다. 비례제어는 PID제어중 가장 기본이지만 문제점이 있다. 바로 목표값에 정확히 일치하지 않고 어느정도 차이가 생긴다는것..

목표값에 근접함에 따라 조작량이 점점 줄어들고 결국엔 목표값에 정확히 일치하지 못하고 조작량이 0이되면 편차가 생기는데 이를 정상편차라고 한다. 이를 해결하기 위해 아래 I-적분제어를 쓴다


PI 비례-적분제어

I-적분제어는 단독으로 쓰이지 않고 P제어를 보정하기위해 쓰인다. P제어에서 약 5도정도 정상편차가 생겼다고 가정해보자. 그럼 기울어진쪽의 적분값이 계속 쌓이면서 조작량으로 발생하고 발생한 5도의 편차를 없앨 수 있다.


PI로만 해도 제어할수 있을 것 같지만, 목표값에 도달하는 속도가 너무 느려 쿼드콥터는 어느새 저멀리.. 날아간다 그래서 쓰는게 D미분제어이다.


PID 비례-적분-미분제어

D제어 역시 단독으로 쓰지 않고 P비례제어를 보조해주는 역할을 한다. I-적분제어가 정상편차를 없애주는거라면 D-비례제어는 PI제어의 단점인 느린 목표값 도달을 빠르게 도달할 수 있게 해준다.

미분은 순간변화량이다. 그래서 D-미분제어는 기체의 순간기울기변화가 클수록 크게작용해 신속하게 목표값에 도달할 수 있게 해주는 것이다.


PID제어는 게인값을 구하는게 중요한데, 각각의 게인값은 구한 P,I,D 제어값을 어느정도로 적용할지를 뜻한다. 기체마다 환경이 다르니 직접 게인을 맞춰봐야 구할수 있는데 이 게인값을 구하는과정을 PID 튜닝이라고 한다.


조작량 = p게인*error + i게인*error적분값 + d게인*전회 error와의 차이

조작량이 바로 모터의 출력이 된다.









사진 출처

http://bearrhee.tistory.com/entry/90008274486

http://www.aistudy.co.kr/control/process_kim.htm





쿼드콥터 제작기 06 - mpu9250(6050) 가속도와 자이로센서

Posted by dw0rdptr
2015. 8. 25. 22:02 IoT/QuadCopter

쿼드콥터는 모터만 돌린다고 알아서 날지 못한다. 이유는 여러가지가 있는데 바람이 변수가 될 수도 있고 캘리브레이션을 해도 모터각각의 출력이 조금씩 차이가 날수도 있다. 또 쿼드콥터의 무게중심이 정확하게 맞는 것도 아니기 때문에 기체가 한쪽으로 기울어진 정도에 따라 기울어진쪽 모터를 더 돌려야 기체가 안정되게 뜰 수 있고 이러한 신호를 주는 주기는 앞글에서 말했듯이 2~3ms 내외여야 빠르게 외란에 대응해 쿼드콥터가 자세를 잡을 수 있게 된다.

기울어진정도에 따라 모터의 출력을 제어하는 알고리즘을 PID 자세제어라고 한다. PID에 대한 자세한 설명은 다음글에서 하고, 먼저 기체의 기울기를 알려면 가속도와 자이로센서가 필요하다. 가속도+자이로 6축센서인 MPU6050이 저렴하면서도 다른센서에 비해 안정적이라 쿼드콥터에서 많이 쓰이고 있다. 난 지자계센서가 추가된 mpu9250을 샀지만 지자계는 나중에 쓰니까 지자계를 제외하면 mpu6050과 똑같다.


센서를 이용해 각도를 구하는 방법은 처음에 생각했던 것보다 조금(아니 많이...) 복잡했다.. 각도를 구하려면 가속도와 자이로센서를 써야한다. 둘은 서로 장단점이 있는데 가속도센서는 값은 오차없이 잘나온다. 하지만 진동에 취약해 값이 요동치게 나온다.

반면에 자이로센서는 요동치는일 없이 깔끔하게 잘나오지만 자이로센서의 출력값에는 바이어스(bais=치우침)값이 포함되어있고 자이로센서 값을 구할때 적분을 하면서 이값이 계속 누적되면 점점 오차가 커지게 된다.


손으로 들고 x축으로 왔다갔다 한 결과다. 빨간색 그래프가 자이로센서값이고 파란색 그래프가 가속도센서값이다. 자이로는 깔끔하고 정확하게 출력되지만 점점 오차가 쌓이는 것을 볼 수 있는데 반면에 가속도센서 그래프는 손노이즈에 값이 심하게 요동쳐서 부정확하지만 시간이 지나도 오차값이 누적되지 않는다.

이러한 두 센서의 장점만을 골라 우리가 원하는 각도를 구할 수 있다. 이 때 사용되는 필터가 칼만필터와 상보(또는 보상)필터가 있는데, 칼만필터는 아두이노에서 처리하기엔 무겁다는 말이 많아 상보필터를 쓸것이다.


참고로 필터를 써도 시간이 지남에 따라 자이로센서에서 생기는 오차값은 생기기 마련이다.  루프를 돌기 전 여러번의 센서값을 구해 생기는 오차의 평균값을 주기마다 자이로값에 빼주면 오차가 줄어드는 것을 볼 수 있지만 없어지지는 않는다. 이를 보완해주는게 HDR(Heuristic Drift Reduction) 알고리즘이다


아두이노 소스

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
#include "Wire.h"
#include "MPU6050.h"
#include "I2Cdev.h"
 
#define pi 3.141592
#define RADIANS_TO_DEGREES 180/3.14159
#define fs 131.0;
MPU6050 mpu;
 
int16_t ax,ay,az;
int16_t gx,gy,gz;
int16_t mx,my,mz;
 
//자이로(각속도)
float gyro_x, gyro_y;
 
//최종 가속도,자이로 각도
float accel_x, accel_y;
float gyro_angle_x=0, gyro_angle_y=0;
 
//상보필터 거친 각도
float angle_x=0,angle_y=0,angle_z=0;
 
//자이로센서 바이어스값
float base_gx=0, base_gy=0, base_gz=0;
 
unsigned long pre_msec=0;
 
void calibrate(){  
   
  int loop =10;
  for (int i=0;i<loop;i++)
  {
    mpu.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
    base_gx += gx;
    base_gy += gy;
    base_gz += gz;
    delay(80);
  }
  
  base_gx /=loop;
  base_gy /=loop;
  base_gz /=loop;
 
}
 
 
void setup() {
  Wire.begin();
 
  Serial.begin(19200);
  mpu.initialize();
  calibrate(); 
}
 
void loop() {
  //단위시간 변화량
  float dt = (millis()-pre_msec)/1000.0;
  pre_msec = millis();
  
  mpu.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
 
  //가속도값 아크탄젠트->각도변환
  accel_x = atan(ay/sqrt(pow(ax,2+ pow(az,2)))*RADIANS_TO_DEGREES;
  accel_y = atan(-1*ax/sqrt(pow(ay,2+ pow(az,2)))*RADIANS_TO_DEGREES;
  
 
  
  //자이로 -32766~+32766을 실제 250degree/s로 변환
  //앞에서 계산한 오차의 평균값을 빼줌 
  gyro_x = (gx-base_gx)/fs;  
  gyro_y = (gy-base_gy)/fs;
 
  //변화량을 적분 
  gyro_angle_x = angle_x + dt*gyro_x;
  gyro_angle_y = angle_y + dt*gyro_y;
 
  //상보필터
  angle_x = 0.95*gyro_angle_x + 0.05*accel_x;
  angle_y = 0.95*gyro_angle_y + 0.05*accel_y;
 
  Serial.print(gyro_angle_x);
  Serial.print("   ");
  Serial.print(accel_x);
  Serial.print("   ");
  Serial.println(angle_x);
  
}
cs

이소스는 HDR없이 상보필터만 적용했는데 오차범위가 40분이 넘어가도록 0.1을 넘지않는다.. 게인값을 잘준건가 아니면 구한 바이어스값이 딱맞아떨어지는건가 ㅋㅋㅋㅋㅋ



--------------2015.10.31수정---------------

I2Cdev와 MPU6050 헤더 물어보시는 분들이 많아서 추가합니다

https://github.com/jrowberg/i2cdevlib



-참고

http://mechasolutionwiki.com/index.php?title=%EC%83%81%EB%B3%B4%ED%95%84%ED%84%B0 -상보필터

http://hs36.tistory.com/32   - HDR 알고리즘

http://hs36.tistory.com/18   - 자이로, 가속도센서 설명




쿼드콥터 제작기 05 -PWM과 제어주기,ESC 캘리브레이션

Posted by dw0rdptr
2015. 8. 16. 02:12 IoT/QuadCopter

조종기 구현도 완벽하게 끝난건 아니지만 일단 호버링이 되어야 방향전환을 할 수 있으니 천천히 하면되고 ESC셋팅부터 하자

ESC는 모터제어 신호를 PWM으로 받아 아두이노에서 PWM을 전달해줘야 한다. 


PWM(pulse Width Modulation)?

아두이노는 모터의 속도를 단계별로 출력하는 아날로그 신호를 출력하지 못하는데

대신 일정한 주기로 디지털신호를 껐다 켰다 하면서 아날로그신호처럼 쓰는게 PWM이다. (따로 있는 아날로그 핀은 입력만 받는 용도)

PWM에는 보드에서 지원하는 하드웨어PWM과 직접 구현하는 소프트웨어PWM이 있는데 반응속도가 빨라야 하는 쿼드콥터에선 하드웨어PWM을 쓴다고 한다.

하드웨어PWM은 analogWrite 함수를 써서 출력할 때 0~255단계 값을 입력가능하다.


짚고갈게 하나 더 있는데,

첫글에서 말했듯이 쿼드콥터의 제어주기는 약 2ms주기로는 돌아야 안정적으로 기체를 제어 할 수 있다. 1초에 2ms주기를 500번 도니까 주파수는 500hz가 된다. 

아두이노에서는 디지털 3,5,6,9,10,11번 핀이 하드웨어 PWM을 지원하는데 이중에서 3,9,10,11번의 주파수가 490hz로 고정되어 있어 이 네개의 핀을 쓴다.

보통 ESC에서 최소PWM 신호는 1ms. 최대 신호는 2ms를 기본으로 한다. 


정리하면 

쿼드콥터를 안정적으로 제어하려면 2ms 주기(약 500hz 전후)로 ESC 최저 1ms, 최고 2ms의 신호를 ESC에 전달해주어야 한다.


이걸몰라서 엄청 헤맸다가 겨우 개념이 잡혔다.. 참고로 제어주기는 아두이노기준(490hz)으로 설명한거라 그렇지 2.5ms 까지도 안정적이라고 한다. 

가끔 Servo라이브러리를 쓰는사람들도 있는데 50hz라 외란에 대응 못한다고 하더라 아래 비교한 동영상을 보자

* 2016.1.23 추가 : Servo 라이브러리를 400Hz로 변경할 수 있는 방법이 있다고 합니다.  Servo의 경우 좀더 세밀한 단계로 조정할 수 있기 때문에 정말할 제어가 가능해져 Servo라이브러리를 써볼 예정. 추후 정리해서 포스팅하겠습니다.


딱봐도 모터의 응답성이 달라진다 이게 나중에 PID적용할 때에도 크게 작용하는거라 어떻게보면 제일 중요할듯

  


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
#include <SoftwareSerial.h>
 
SoftwareSerial APC(78); // RX and TX
int value=126;
int Motor[] = { 391011 };
 
void setup()
{
  APC.begin(57600); // start serial to APC
  Serial.begin(9600);
  for(int i=0;i<4;i++)
    pinMode(Motor[i],OUTPUT);
}
 
void loop()
{
  if (APC.available())
  {
    value = (int)APC.read(); 
    Serial.println(value);
   
    for(int i=0;i<4;i++)
      analogWrite(Motor[i],value);
  }
  else
  {
    Serial.println(value);
    for(int i=0;i<4;i++)
      analogWrite(Motor[i],value);
  }
 
}
 
cs

7,8번핀으로 APC모듈로 받은 값을 APC.read()함수로 읽어서 value에 넣고 analogWrite()함수로 3,9,10,11핀에 연결되어 있는 ESC에 2ms주기로 PWM 신호를 전달해준다.

만약 모듈에서 새로 넘어온 값이 없으면, 즉 쓰로틀 변화가 없으면 그 전에 들어온 값으로 역시 2ms주기로 계속 넘겨준다.  

analogWrite()함수 기준 0~255 중 ESC의 최저속도 신호인 1ms에 대응되는 값은 약 126, 최고속도 신호인 2ms에 대응되는 값은 약 250이라 조종기 쓰로틀바 범위를 126~250로 잡아줘야 한다


이제 ESC캘리브레이션을 해보자

ESC는 초기설정이 되어있는데 ESC마다 약간씩 신호범위가 달라서 같은 PWM신호에 같은출력을 낼 수 있도록 해주는게 캘리브레이션이다

최대신호와 최소신호만 주면 간단하게 끝난다

Turnigy plush 25A 기준으로 조종기 쓰로틀을 최대로 올리고 배터리를 연결하면 비프음이 두 번 울리는데 이때 쓰로틀을 최소로 낮추면

비프음이 배터리 셀 개수만큼 울리고 한번 길게울리면 셋팅 끝! 쓰로틀을 올려보면 모터가 정상적으로 도는걸 볼 수 있다


*영상에서 쓰로틀을 반만내렸는데, 저때 쓰로틀바 범위가 0~255까지라 수정을 해야되는데 안스가 터져서 급하게 아두이노에서 시리얼로 받은 126미만값은 모두 126으로 고정해서 캘리브레이션을 진행함 ㅠㅠ

지금은 수정해서 126~250으로 바꿨다



역시 드림엔터에서 밤새서 빡세게 하니까 진행이 빠르게 되는것같다 

조만간 PID시작해야지

쿼드콥터 제작기 04 - 안드로이드 -> 아두이노 USB 시리얼통신

Posted by dw0rdptr
2015. 8. 13. 04:24 IoT/QuadCopter

APC220 모듈이랑 라즈베리파이, 알피노, 아두이노 나노가 도착했다


아두이노 나노에서 우노로 apc220모듈로 통신하는방법은 간단하다 보드레이트만 맞춰주고 소프트웨어 시리얼로 하면 끝!

참고- apc220모듈 통신 http://bbangpan.tistory.com/27 


이제 안드로이드에서 시리얼브릿지(아두이노 나노)로 usb시리얼통신을 하면 되는데, 안드로이드를 해본적이 없어 여기서 일주일은 삽질하면서 보낸 것 같다. 다 때려치고 조종기 하나 사버릴까 하는 생각도 했고.. 

옆동네 사과랑 달리 안드로이드는 따로 usb시리얼을 지원을 안해서 드라이버 소스코드를 가져다 써야 한다고 한다.

https://github.com/mik3y/usb-serial-for-android  - 안드로이드 usb 시리얼 드라이버

이 드라이버를 프로젝트에 넣고 알아서 하면된다.

----------------------------------------------------------------------

여기까지 안드하는사람 기준 


나같은 경우는 

http://android.serverbox.ch/?p=549 여기에있는 소스를 거의 가져다 썼다. 

아두이노 analogWrite()함수의 pwm신호 범위에 맞게 0~255로 seek바 값의 범위를 맞추고 아두이노 우노까지 전달이 되는지 시리얼모니터를 키고 테스트해봤다.( 동영상은 우노와 나노가 바뀌었음 )

일주일간의_삽질이_끝나는_순간


다음글은 아마 ESC캘리브레이션이 될것같다.

이제 속도가 좀 붙을것 같으니까 다음주가 개학이네 언제쯤 날릴수 있을까..