hige dura
/
quad_1
for hikorobo2015
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(); } */