main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "Map.h"
00004 #include "Pulse1.h"
00005 
00006 // 후진 불가능한 BLDC 사용 
00007 
00008 
00009 PwmOut led_left(LED1); //확인용 LED1
00010 PwmOut led_right(LED2); //확인용 LED2
00011 
00012 Servo bldc_left(PB_8); // 왼쪽 BLDC D12
00013 Servo bldc_right(PB_9); // 오른쪽 BLDC PC_7
00014 
00015 PulseInOut Channel1(A0); //채널1, futaba기준 좌우방향(오른쪽 조이스틱)
00016 PulseInOut Channel2(A1); //채널2, futaba기준 상하방향(오른쪽 조이스틱)
00017 PulseInOut Channel3(A2); //채널3, futaba기준 상하방향(왼쪽 조이스틱)
00018 
00019 int main() {
00020     bldc_left.write(0); // BLDC 초기 설정
00021     bldc_right.write(0);
00022     
00023     led_left = 1; // 확인을 위한 LED
00024     led_right=1; // 확인을 위한 LED
00025     wait(0.5);
00026     
00027     bldc_left.write(1);
00028     bldc_right.write(1);
00029     
00030     led_left =0; // 확인을 위한 LED
00031     led_right=0; // 확인을 위한 LED
00032     wait(2);
00033     
00034     bldc_left.write(0); // BLDC 초기 설정
00035     bldc_right.write(0); // BLDC 초기 설정
00036     wait(2);
00037     
00038     float raw1,raw2,raw3;
00039     float mod1,mod2,mod3,mod4,mod5;
00040     printf("\nBLDC Setup & end\n");
00041     
00042     while (1) {
00043         
00044         
00045         raw1 = Channel1.read_high_us(30000);
00046         raw2 = Channel2.read_high_us(30000);
00047         raw3 = Channel3.read_high_us(30000);
00048         
00049         
00050         //printf("raw1=%.0f mV\t", raw1);
00051         mod1 =  map(raw2, 1450, 1000, 0, 60); // 후진 //PWM 150을 변환
00052         mod2 =  map(raw2, 1550, 2000, 0, 90); // 전진 //PWM 150을 변환
00053         
00054         
00055         
00056         //printf("raw2=%.0f mV\t", raw2);
00057         mod3 =  map(raw1, 1450, 1000, 0, 60); // 좌회전 //PWM 150을 변환
00058         mod4 =  map(raw1, 1550, 2000, 0, 60); // 우회전 //PWM 150을 변환
00059         
00060         //printf("raw3=%.0f mV\t", raw3);
00061         mod5 =  map(raw3, 920, 2080, 0, 100);
00062         
00063         printf("mod1=%.0f mV\t", mod1);
00064         printf("mod2=%.0f mV\t", mod2);
00065         printf("mod3=%.0f mV\t", mod3);
00066         printf("mod4=%.0f mV\t", mod4);
00067         printf("mod5=%.0f mV\t", mod5);
00068         
00069         mod1 = mod1/100;
00070         mod2 = mod2/100;
00071         mod3 = mod3/100;
00072         mod4 = mod4/100;
00073         mod5 = mod5/100;
00074         
00075         printf("measure1 = %f mV\t", mod1);
00076         printf("measure2 = %f mV\t", mod2);
00077         printf("measure3 = %f mV\t", mod3);
00078         printf("measure4 = %f mV\t", mod4);
00079         printf("measure5 = %f mV\n", mod5);
00080         
00081         
00082         if(mod3 > 0.01) { // 2
00083             mod4 = 0;
00084             }
00085             
00086         if(mod4 > 0.01) { // 2
00087             mod3 = 0;
00088             }
00089             
00090         
00091         float bldc_l, bldc_r; // bldc 왼쪽 오른쪽 선언
00092         bldc_l = mod2 - mod3 + mod4;
00093         bldc_r = mod2 + mod3 - mod4;
00094         
00095         if(bldc_l >= 0.6) { // PWM 150을 변환
00096             bldc_l = 0.6;
00097             }
00098         if(bldc_l <= 0.03) { // PWM 20을 변환
00099             bldc_l = 0;
00100             }
00101             
00102         if(bldc_r >= 0.6) { // PWM 150을 변환
00103             bldc_r = 0.6;
00104             }
00105         if(bldc_r <= 0.03) { // PWM 20을 변환
00106             bldc_r = 0;
00107             }
00108         
00109         bldc_left.write(bldc_l);
00110         bldc_right.write(bldc_r);
00111         
00112 
00113         led_left.write(bldc_l);
00114         led_right.write(bldc_r);
00115         }
00116 }
00117