File content as of revision 0:7663c81cdf71:

#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);
       
    }
}