Working Thruster and Servo RC
Dependencies: PwmIn mbed Servo
main.cpp
- Committer:
- WoodyERAU
- Date:
- 2019-05-28
- Revision:
- 2:cc7237f357e4
- Parent:
- 1:1d2b72d09c56
- Child:
- 3:64b5ea1e088f
File content as of revision 2:cc7237f357e4:
#include "mbed.h" #include "Servo.h" #include "PwmIn.h" //Initialize left and right servos Servo back_servo(p23); Servo front_servo(p24); //Pins for Thruster Power Servo front_thrust(p25); Servo back_thrust(p26); //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 back_servo = 0.0; front_servo = 0.0; back_thrust = 0.0; front_thrust = 0.0; wait(0.5); //ESC detects signal //Required ESC Calibration/Arming sequence //sends longest and shortest PWM pulse to learn and arm at power on back_servo = 1.0; front_servo = 1.0; back_thrust = 1.0; front_thrust = 1.0; wait(0.5); back_servo = 0.0; front_servo = 0.0; back_thrust = 0.0; front_thrust = 0.0; wait(0.5); //End calibration sequence while(1) { //Enable Servo to turn 180 degrees back_servo.calibrate(0.00085,90.0); front_servo.calibrate(0.00085,90.0); //Read in all PWM signals and set them to a value between 0 and 1 elevation_output = (elev.pulsewidth()*1000)-1; throttle_output = (thro.pulsewidth()*1000)-1; rudder_output = (rudd.pulsewidth()*1000)-1; aileron_output = (aile.pulsewidth()*1000)-1; //ESTOP PWM ESTOP_output = (gear.pulsewidth()*1000)-1; // >.5 run... <.5 STOP if(ESTOP_output > 0.5) { //Servo Controllers back_servo = aileron_output; front_servo = rudder_output; //Thrust Controllers back_thrust = throttle_output - 0.04; front_thrust = elevation_output - 0.04; } else { front_thrust = 0.46; back_thrust = 0.46; } wait(0.1); printf("\t\t:PWM: %4.2f\t", elevation_output); printf(" \r\n"); } }