Diff: main.cpp

Revision:

0:7663c81cdf71

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 27 09:09:04 2019 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include "Pulse1.h"  // RF통신을 위한 헤더파일
+#include "Map.h"    // 범위 변환을 위한 헤더파일
+#include "Servo.h"  // BLDC 구동을 위한 헤더파일
+
+
+// BLDC 1100일 때 전진 최고 추진
+// BLDC 1900일 때 후진 최고 추진
+// BLDC 1500일 때 정지
+// 후타바 범위 920 - 1500 - 2080
+
+Servo Servo1(PB_8); //BLDC 핀 선언
+Servo Servo2(PB_9); //BLDC 핀 선언
+
+PulseInOut Channel1(PA_0); // 후타바 신호 1번 선언
+PulseInOut Channel2(PA_4); // 후타바 신호 2번 선언
+PulseInOut Channel3(PB_0); // 후타바 신호 3번 선언
+PulseInOut Channel4(PC_1); // 후타바 신호 4번 선언
+
+DigitalOut DC_e(D10); // DC 모터 방향 설정
+PwmOut DC(D11); // DC 모터 PWM 설정
+
+float max_speed = 1200; // 전진 최고 속도
+float remax_speed= 1800;// 후진 최고 속도
+
+int main() {
+    Servo1.Enable(1000,20000); // BLDC 범위 설정
+    Servo2.Enable(1000,20000); // BLDC 범위 설정
+    Servo1.SetPosition(1500); // BLDC 초기 중간값 설정
+    Servo2.SetPosition(1500); // BLDC 초기 중간값 설정
+    wait(3);
+    
+    float raw1,raw2,raw3,raw4; // 후타바에서 채널별로 변수 선언
+    float a,b,c,d,dcp; // BLDC 추진 변수의 세부 변수
+    float bldc_l,bldc_r; // 각각 BLDC 추진 변수
+    
+    DC_e = 0; // 모터 방향 설정
+    
+    while(1) {
+       
+       raw1 = Channel1.read_high_us(30000); // 후타바 1번채널 펄스 읽은 값을 설정
+       raw2 = Channel2.read_high_us(30000); // 후타바 2번채널 펄스 읽은 값을 설정
+       raw3 = Channel3.read_high_us(30000); // 후타바 3번채널 펄스 읽은 값을 설정
+       raw4 = Channel4.read_high_us(30000); // 후타바 4번채널 펄스 읽은 값을 설정
+       
+//       printf("%.0f\t", raw1);
+//       printf("%.0f\t", raw2);
+//       printf("%.0f\t", raw3);
+//       printf("%.0f\t", raw4);
+//       printf(" || ");
+       
+       a = map(raw1,1500,2080,0,400);  // 후타바에서 받아온 변수를 범위 변환 (전진)
+       b = map(raw1,1500,920,0,400); // 후타바에서 받아온 변수를 범위 변환 (후진)
+       c = map(raw2,1500,920,0,200); // 후타바에서 받아온 변수를 범위 변환 (좌회전)
+       d = map(raw2,1500,2080,0,200); // 후타바에서 받아온 변수를 범위 변환 (우회전)
+       dcp = map(raw3,960,2080,0,100); // 후타바에서 받아온 변수를 범위 변환  (DC)
+       
+       dcp = dcp / 100; // DC 값을 PWM 값으로 변환
+       DC.write(dcp); // 모터 구동
+       
+       
+//       printf("%.0f\t", a);
+//       printf("%.0f\t", b);
+//       printf("%.0f\t", c);
+//       printf("%.0f\t", d);
+//       printf("%.0f\t", dcp);
+       
+       if(c >0) { // 좌회전 인식할 때 우회전 값은 0선언
+           d =0;
+        }
+       if(d >0) { // 우회전 인식할 때 좌회전 값은 0선언
+           c =0;
+        }    
+       if( a>2) { // 전진 인식할 때 후진 값은 0선언
+           b=0;
+        }
+        if( b>2) { // 후진 인식할 때 전진 값은 0선언
+           a=-31;
+        } 
+        if( a < -30) { // 후진하며 방향 설정시 역방향으로 선언
+            c = c*-1;
+            d = d*-1;
+            }
+        bldc_l = 1500 - a + b + c - d; // BLDC 각각의 변수 선언 중간값 + 전진 + 좌,우회전
+        bldc_r = 1500 - a + b - c + d; // BLDC 각각의 변수 선언 중간값 + 전진 + 좌,우회전
+        
+        if( a > -30) { // 전진 시 BDLC 변수 범위 선언
+           if(bldc_l >=1500) {
+               bldc_l = 1500;
+           }
+           if(bldc_l <=1100) {
+               bldc_l = 1100;
+           }
+           if(bldc_r >=1500) {
+               bldc_r = 1500;
+           }
+           if(bldc_r <=1100) {
+               bldc_r = 1100;
+           }
+           bldc_l = map(bldc_l,1500,1100,1500,max_speed); // BLDC 출력 범위 설정
+           bldc_r = map(bldc_r,1500,1100,1500,max_speed); // BLDC 출력 범위 설정
+        }
+        if(a <= -31) { // 후진 시 BDLC 변수 범위 선언
+           if(bldc_l >=1900) {
+               bldc_l = 1900;
+           }
+           if(bldc_l <=1500) {
+               bldc_l = 1500;
+           }
+           
+           if(bldc_r >=1900) {
+               bldc_r = 1900;
+           }
+           if(bldc_r <=1500) {
+               bldc_r = 1500;
+           }
+           bldc_l = map(bldc_l,1500,1900,1500,remax_speed); // BLDC 출력 범위 설정
+           bldc_r = map(bldc_r,1500,1900,1500,remax_speed); // BLDC 출력 범위 설정
+        }        
+//        printf("%.0f\t", bldc_l);
+//        printf("%.0f\n", bldc_r);   
+
+       Servo1.SetPosition(bldc_l); // BLDC 작동
+       Servo2.SetPosition(bldc_r); // BLDC 작동
+       wait(0.005);
+       
+    }
+}
\ No newline at end of file