Working Thruster and Servo RC
Dependencies: PwmIn mbed Servo
Diff: main.cpp
- Revision:
- 2:cc7237f357e4
- Parent:
- 1:1d2b72d09c56
- Child:
- 3:64b5ea1e088f
diff -r 1d2b72d09c56 -r cc7237f357e4 main.cpp --- a/main.cpp Mon May 27 21:56:20 2019 +0000 +++ b/main.cpp Tue May 28 00:27:50 2019 +0000 @@ -3,18 +3,15 @@ #include "PwmIn.h" //Initialize left and right servos -Servo left_servo(p23); -Servo right_servo(p24); +Servo back_servo(p23); +Servo front_servo(p24); + + //Pins for Thruster Power -PwmOut Front(p25); -PwmOut Back(p26); +Servo front_thrust(p25); +Servo back_thrust(p26); -//Initialize LEDs -PwmOut led1(LED1); -PwmOut led2(LED2); -PwmOut led3(LED3); -PwmOut led4(LED4); //Initialize receiver pins PwmIn thro(p17); @@ -34,48 +31,64 @@ int main() { //Calibration Sequence - left_servo = 0.0; - right_servo = 0.0; - led1 = led2 = led3 = led4 = 1; + 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 - left_servo = 1.0; //send longest PWM - right_servo = 1.0; - led1 = led2 = led3 = led4 = 0; - wait(1); - right_servo = 0.0; - wait(1); + 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) { - /*The progam has to switch from autonomous control and receiver - control based of the 'gear_output' value - */ + + //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.2; - throttle_output = (thro.pulsewidth()*1000)-1.2; - rudder_output = (rudd.pulsewidth()*1000)-1.2; - aileron_output = (aile.pulsewidth()*1000)-1.2; + 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) - { - led1 = throttle_output; - led2 = aileron_output; - led3 = elevation_output; - led4 = rudder_output; + { + + //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 { - led1 = led2 = led3 = led4 = 0; + front_thrust = 0.46; + back_thrust = 0.46; } - + wait(0.1); + + printf("\t\t:PWM: %4.2f\t", elevation_output); + printf(" \r\n"); } }