Working Thruster and Servo RC

Dependencies:   PwmIn mbed Servo

main.cpp

Committer:
WoodyERAU
Date:
2019-05-27
Revision:
1:1d2b72d09c56
Parent:
0:4fb155dd1c7a
Child:
2:cc7237f357e4

File content as of revision 1:1d2b72d09c56:

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

//Initialize left and right servos
Servo left_servo(p23);
Servo right_servo(p24);

//Pins for Thruster Power
PwmOut Front(p25);
PwmOut Back(p26);

//Initialize LEDs
PwmOut led1(LED1);
PwmOut led2(LED2);
PwmOut led3(LED3);
PwmOut led4(LED4);

//Initialize receiver pins
PwmIn thro(p17);
PwmIn elev(p15);
PwmIn gear(p13);
PwmIn aile(p14);
PwmIn rudd(p16);

//Set all PWM values to a defaalt of 0
float throttle_output = 0.0;
float elevation_output = 0.0;
float ESTOP_output = 0.0;
float rudder_output = 0.0;
float aileron_output = 0.0;


int main() {
    
    //Calibration Sequence
    left_servo = 0.0;
    right_servo = 0.0;
    led1 = led2 = led3 = led4 = 1;
    wait(0.5); //ESC detects signal
    //Required ESC Calibration/Arming sequence  
    //sends longest and shortest PWM pulse to learn and arm at power on
    left_servo = 1.0; //send longest PWM
    right_servo = 1.0;
    led1 = led2 = led3 = led4 = 0;
    wait(1);
    right_servo = 0.0;
    wait(1);
    //End calibration sequence
    
    while(1) {
        /*The progam has to switch from autonomous control and receiver
        control based of the 'gear_output' value
        */
        
        //Read in all PWM signals and set them to a value between 0 and 1
        elevation_output = (elev.pulsewidth()*1000)-1.2;
        throttle_output = (thro.pulsewidth()*1000)-1.2;
        rudder_output = (rudd.pulsewidth()*1000)-1.2;
        aileron_output = (aile.pulsewidth()*1000)-1.2;
        
        //ESTOP PWM
        ESTOP_output = (gear.pulsewidth()*1000)-1;  // >.5 run... <.5 STOP
        

        if(ESTOP_output > 0.5)
            {                
                led1 = throttle_output;
                led2 = aileron_output;
                led3 = elevation_output;
                led4 = rudder_output;
            }
        else
            {
                led1 = led2 = led3 = led4 = 0;
            }


        }
}