File content as of revision 0:926c1833fd08:

#include "mbed.h"
#include "Servo.h"

PwmOut ledf(LED1); //throttle up test led with PWM dimming
PwmOut ledr(LED2); //throttle down test led with PWM dimming
//Serial pc(USBTX, USBRX); // tx, rx

Servo Motor1(p21);
Servo Motor2(p22);
Servo Motor3(p23);
Servo Motor4(p24);
Servo Motor5(p25);
Servo Motor6(p26);


class PwmIn {
public:
    PwmIn(PinName p) : _p(p) {
        _p.rise(this, &PwmIn::rise);
        _p.fall(this, &PwmIn::fall);
        _period = 0.0;
        _pulsewidth = 0.0;
        _t.start();
    }
    
    void rise() {
        _period = _t.read();
        _t.reset();
    }
    
    void fall() {
        _pulsewidth = _t.read();
    }
    
    float period() { return _period; }
    float pulsewidth() { return _pulsewidth; }
    float dutycycle() { return _pulsewidth / _period; }

protected:        
    InterruptIn _p;
    Timer _t;
    float _pulsewidth, _period;
};

PwmIn e(p11);
PwmIn f(p12);
PwmIn a(p5);
PwmIn b(p6);
PwmIn c(p7);
PwmIn d(p8);


float A_PWM_AUX1 = 0.0;
float A_PWM_AUX2 = 0.0;
float A_PWM_Roll = 0.0;
float A_PWM_Pitch = 0.0;
float A_PWM_Throttle = 0.0;
float A_PWM_Yaw = 0.0;


float T = 0;
float R = 0;
float P = 0;
float Y = 0; 
/*
float a1 = 0;
float a2 = 0;
float a3 = 0;
float a4 = 0; 
float a5 = 0;
float a6 = 0;       
*/
int main() {
  
    
    while(1) {
    
    float raw_PWM_AUX1=(e.pulsewidth()*1000000);
    float raw_PWM_AUX2=(f.pulsewidth()*1000000);  
    float raw_PWM_Roll=(a.pulsewidth()*1000000);
    float raw_PWM_Pitch=(b.pulsewidth()*1000000);
    float raw_PWM_Throttle=(c.pulsewidth()*1000000);
    float raw_PWM_Yaw=(d.pulsewidth()*1000000);

    
    if(raw_PWM_AUX2>=1000 && raw_PWM_AUX2<=1200) // AUX1 2일 때 정
    
    {    
     A_PWM_Roll = 0.0;
     A_PWM_Pitch = 0.0;
     A_PWM_Throttle = 0.0;
     A_PWM_Yaw = 0.0;
     A_PWM_AUX1 = 0.0;
     A_PWM_AUX2 = 0.0;
    }
        
   else
    {   
     A_PWM_Roll = (((raw_PWM_Roll)/420)-3.55)*10 ;                   //(-10~10)
     A_PWM_Pitch = (((raw_PWM_Pitch)/420)-3.525)*10 ;                //(-10~10)
     A_PWM_Throttle = (((raw_PWM_Throttle)/420)-2.5)*50 ;          //( 0~100)
     A_PWM_Yaw = (((raw_PWM_Yaw)/420)-3.55)*10 ;                    //(-10~10)  
     
    }
       
       
   
    if (A_PWM_Roll>=10) A_PWM_Roll=10;         //(-10~10)                                                                                 
    else if (A_PWM_Roll<=-10) A_PWM_Roll=-10; 

     if (A_PWM_Pitch>=10) A_PWM_Pitch=10;      //(-10~10)
    else if (A_PWM_Pitch<=-10) A_PWM_Pitch=-10;    
    
    if (A_PWM_Throttle>=100) A_PWM_Throttle=100;//(0~1000)
    else if (A_PWM_Throttle<=2) A_PWM_Throttle=0;     
    
    if (A_PWM_Yaw>=10) A_PWM_Yaw=10;           //(-10~10)
    else if (A_PWM_Yaw<=-10) A_PWM_Yaw=-10;
    
    if (raw_PWM_AUX1>=1750 && raw_PWM_AUX1<=1900) A_PWM_AUX1=1;
    else if (raw_PWM_AUX1>=1650 && raw_PWM_AUX1<=1730) A_PWM_AUX1=2;
    
    if (raw_PWM_AUX2>=1000 && raw_PWM_AUX2<=1200) A_PWM_AUX2=1;
    else if (raw_PWM_AUX2>=1400 && raw_PWM_AUX2<=1500) A_PWM_AUX2=2;
    else if (raw_PWM_AUX2>=1900 && raw_PWM_AUX2<=2000) A_PWM_AUX2=3;    
      
/*
       pc.printf("Roll: pw = %.00f \n", A_PWM_Roll);
       pc.printf("Pitch: pw = %.00f \n", A_PWM_Pitch);
       pc.printf("Throttle: pw = %.00f \n", A_PWM_Throttle);
       pc.printf("Yaw: pw = %.00f \n\n", A_PWM_Yaw);
       pc.printf("AUX1: pw = %.00f \n", A_PWM_AUX1);
       pc.printf("AUX2: pw = %.00f \n\n", raw_PWM_AUX2);     
*/
       
    T = A_PWM_Throttle;
    R = A_PWM_Roll;
    P = A_PWM_Pitch;
    Y = A_PWM_Yaw;

    if(T>0){ 
    
    
       Motor1 = (T -(R/2) +(P*7/8) + Y)/150;
       Motor2 = (T -(R/2) -(P*7/8) + Y)/150;
       Motor3 = (T +(R/2) +(P*7/8) - Y)/150;
       Motor4 = (T +(R/2) -(P*7/8) - Y)/150;
       Motor5 = (T - R - Y)/150;
       Motor6 = (T + R + Y)/150;
       /*
       a1 = T -(1/2)*R +(7/8)*P + Y;
       a2 = T -(1/2)*R -(7/8)*P + Y;
       a3 = T +(1/2)*R +(7/8)*P - Y;
       a4 = T +(1/2)*R -(7/8)*P - Y;
       a5 = T - R + 0*P - Y;
       a6 = T + R + 0*P + Y;
       
       
       
       
       Motor1 = a1;
       Motor2 = a2;
       Motor3 = a3;
       Motor4 = a4;
       Motor5 = a5;
       Motor6 = a6;
       
       
       Motor1 = a1;
       Motor2 = a2;
       Motor3 = a3;
       Motor4 = a4;
       Motor5 = a5;
       Motor6 = a6;
       
       pc.printf("Motor1 = %.00f \n", a1);
       pc.printf("Motor1 = %.00f \n", a2);
       pc.printf("Motor1 = %.00f \n", a3);
       pc.printf("Motor1 = %.00f \n", a4);
       pc.printf("Motor5 = %.00f \n", a5);
       pc.printf("Motor6 = %.00f \n\n", a6);
       */
        }
     else{
        Motor1 = 0.0;
        Motor2 = 0.0;
        Motor3 = 0.0;
        Motor4 = 0.0;
        Motor5 = 0.0;
        Motor6 = 0.0; //Motor off
        /*
        ledf = ledr = 0;
        pc.printf("F");
        */
    }
            
        
        
    }

}