for hikorobo2015

Dependencies:   mbed

main.cpp

Committer:
higedura
Date:
2015-07-18
Revision:
0:0520851aafd9

File content as of revision 0:0520851aafd9:

#include "mbed.h"
 
#define chan    4         // the numbers of channels

Serial pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

InterruptIn ch1(p15);       // yaw
InterruptIn ch2(p16);       // pitch
InterruptIn ch3(p17);       // throttle
InterruptIn ch4(p18);       // roll
//InterruptIn ch5(p29);       // aux1
//InterruptIn ch6(p30);       // aux2

PwmOut esc1(p21);

void ch1_rise();    void ch1_fall();
void ch2_rise();    void ch2_fall();
void ch3_rise();    void ch3_fall();
void ch4_rise();    void ch4_fall();
//void ch5_rise();    void ch5_fall();
//void ch6_rise();    void ch6_fall();

Timer t_ch1;
Timer t_ch2;
Timer t_ch3;
Timer t_ch4;
//Timer t_ch5;
//Timer t_ch6;

// thro roll pitch yaw aux1 aux2
int pwm_pulse[chan]      =   {0};

int main() {
    
    pc.baud(921600);
    pc.printf("rc_reciever starts!\r\n\r\n");
    
    // Reciever function
    ch1.rise(&ch1_rise);    ch1.fall(&ch1_fall);
    ch2.rise(&ch2_rise);    ch2.fall(&ch2_fall);
    ch3.rise(&ch3_rise);    ch3.fall(&ch3_fall);
    ch4.rise(&ch4_rise);    ch4.fall(&ch4_fall);
    //ch5.rise(&ch5_rise);   ch5.fall(&ch5_fall);
    //ch6.rise(&ch6_rise);   ch6.fall(&ch6_fall);
    
    esc1.period(0.020);
    
    while(1) {
        
        for( int i=0;i<4;i++ ){    pc.printf("%4d ", pwm_pulse[i]);  }
        pc.printf("\r\n");
        
        //ch1.enable_irq();
        //ch2.enable_irq();
        //ch3.enable_irq();
        //ch4.enable_irq();
        
        //wait(.1);
        
        //ch1.disable_irq();
        //ch2.disable_irq();
        //ch3.disable_irq();
        //ch4.disable_irq();
        
        for(float offset=0.0; offset<0.001; offset+=0.0001) {
            esc1.pulsewidth(0.001 + offset); // servo position determined by a pulsewidth between 1-2ms
            wait(0.25);
        }
        
    }
}

void ch1_rise(){    t_ch1.start();   }
void ch2_rise(){    t_ch2.start();   }
void ch3_rise(){    t_ch3.start();  }
void ch4_rise(){    t_ch4.start();    }
//void ch5_rise(){    t_ch5.start();   }
//void ch6_rise(){    t_ch6.start();   }

void ch1_fall(){
    t_ch1.stop();
    pwm_pulse[0]    =   (int)(t_ch1.read()*1000000-1000);
    t_ch1.reset();
}

void ch2_fall(){
    t_ch2.stop();
    pwm_pulse[1]    =   (int)(t_ch2.read()*1000000-1000);
    t_ch2.reset();
}

void ch3_fall(){
    t_ch3.stop();
    pwm_pulse[2]    =   (int)(t_ch3.read()*1000000-1000);
    t_ch3.reset();
}

void ch4_fall(){
    t_ch4.stop();
    pwm_pulse[3]    =   (int)(t_ch4.read()*1000000-1000);
    t_ch4.reset();
}
/*
void ch5_fall(){
    t_ch5.stop();
    pwm_pulse[4]    =   (int)(t_ch5.read()*1000000-1000);
    t_ch5.reset();
}

void ch6_fall(){
    t_ch6.stop();
    pwm_pulse[5]    =   (int)(t_ch6.read()*1000000-1000);
    t_ch6.reset();
}
*/