이전글에서 송신기의 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, 1100, 1900, ROLL_MIN, ROLL_MAX); TargetPITCH = map(Ch2, 1100, 1900, PITCH_MIN, PITCH_MAX); Throttle = map(Ch3, 1130, 1800, ESC_MIN, ESC_MAX); Yaw = map(Ch4, 1100, 1800, 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(4, 5); 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게인도 거의 맞춰가는 것 같고 다음글은 비행테스트가 될 것 같다.
