Working Thruster and Servo RC
Dependencies: PwmIn mbed Servo
main.cpp@2:cc7237f357e4, 2019-05-28 (annotated)
- Committer:
- WoodyERAU
- Date:
- Tue May 28 00:27:50 2019 +0000
- Revision:
- 2:cc7237f357e4
- Parent:
- 1:1d2b72d09c56
- Child:
- 3:64b5ea1e088f
Working Servo and thruster RC
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WoodyERAU | 0:4fb155dd1c7a | 1 | #include "mbed.h" |
WoodyERAU | 1:1d2b72d09c56 | 2 | #include "Servo.h" |
WoodyERAU | 1:1d2b72d09c56 | 3 | #include "PwmIn.h" |
WoodyERAU | 1:1d2b72d09c56 | 4 | |
WoodyERAU | 1:1d2b72d09c56 | 5 | //Initialize left and right servos |
WoodyERAU | 2:cc7237f357e4 | 6 | Servo back_servo(p23); |
WoodyERAU | 2:cc7237f357e4 | 7 | Servo front_servo(p24); |
WoodyERAU | 2:cc7237f357e4 | 8 | |
WoodyERAU | 2:cc7237f357e4 | 9 | |
WoodyERAU | 1:1d2b72d09c56 | 10 | |
WoodyERAU | 1:1d2b72d09c56 | 11 | //Pins for Thruster Power |
WoodyERAU | 2:cc7237f357e4 | 12 | Servo front_thrust(p25); |
WoodyERAU | 2:cc7237f357e4 | 13 | Servo back_thrust(p26); |
WoodyERAU | 1:1d2b72d09c56 | 14 | |
WoodyERAU | 1:1d2b72d09c56 | 15 | |
WoodyERAU | 1:1d2b72d09c56 | 16 | //Initialize receiver pins |
WoodyERAU | 1:1d2b72d09c56 | 17 | PwmIn thro(p17); |
WoodyERAU | 1:1d2b72d09c56 | 18 | PwmIn elev(p15); |
WoodyERAU | 1:1d2b72d09c56 | 19 | PwmIn gear(p13); |
WoodyERAU | 1:1d2b72d09c56 | 20 | PwmIn aile(p14); |
WoodyERAU | 1:1d2b72d09c56 | 21 | PwmIn rudd(p16); |
WoodyERAU | 1:1d2b72d09c56 | 22 | |
WoodyERAU | 1:1d2b72d09c56 | 23 | //Set all PWM values to a defaalt of 0 |
WoodyERAU | 1:1d2b72d09c56 | 24 | float throttle_output = 0.0; |
WoodyERAU | 1:1d2b72d09c56 | 25 | float elevation_output = 0.0; |
WoodyERAU | 1:1d2b72d09c56 | 26 | float ESTOP_output = 0.0; |
WoodyERAU | 1:1d2b72d09c56 | 27 | float rudder_output = 0.0; |
WoodyERAU | 1:1d2b72d09c56 | 28 | float aileron_output = 0.0; |
WoodyERAU | 0:4fb155dd1c7a | 29 | |
WoodyERAU | 0:4fb155dd1c7a | 30 | |
WoodyERAU | 1:1d2b72d09c56 | 31 | int main() { |
WoodyERAU | 1:1d2b72d09c56 | 32 | |
WoodyERAU | 1:1d2b72d09c56 | 33 | //Calibration Sequence |
WoodyERAU | 2:cc7237f357e4 | 34 | back_servo = 0.0; |
WoodyERAU | 2:cc7237f357e4 | 35 | front_servo = 0.0; |
WoodyERAU | 2:cc7237f357e4 | 36 | back_thrust = 0.0; |
WoodyERAU | 2:cc7237f357e4 | 37 | front_thrust = 0.0; |
WoodyERAU | 1:1d2b72d09c56 | 38 | wait(0.5); //ESC detects signal |
WoodyERAU | 1:1d2b72d09c56 | 39 | //Required ESC Calibration/Arming sequence |
WoodyERAU | 1:1d2b72d09c56 | 40 | //sends longest and shortest PWM pulse to learn and arm at power on |
WoodyERAU | 2:cc7237f357e4 | 41 | back_servo = 1.0; |
WoodyERAU | 2:cc7237f357e4 | 42 | front_servo = 1.0; |
WoodyERAU | 2:cc7237f357e4 | 43 | back_thrust = 1.0; |
WoodyERAU | 2:cc7237f357e4 | 44 | front_thrust = 1.0; |
WoodyERAU | 2:cc7237f357e4 | 45 | wait(0.5); |
WoodyERAU | 2:cc7237f357e4 | 46 | back_servo = 0.0; |
WoodyERAU | 2:cc7237f357e4 | 47 | front_servo = 0.0; |
WoodyERAU | 2:cc7237f357e4 | 48 | back_thrust = 0.0; |
WoodyERAU | 2:cc7237f357e4 | 49 | front_thrust = 0.0; |
WoodyERAU | 2:cc7237f357e4 | 50 | wait(0.5); |
WoodyERAU | 1:1d2b72d09c56 | 51 | //End calibration sequence |
WoodyERAU | 1:1d2b72d09c56 | 52 | |
WoodyERAU | 2:cc7237f357e4 | 53 | |
WoodyERAU | 1:1d2b72d09c56 | 54 | while(1) { |
WoodyERAU | 2:cc7237f357e4 | 55 | |
WoodyERAU | 2:cc7237f357e4 | 56 | //Enable Servo to turn 180 degrees |
WoodyERAU | 2:cc7237f357e4 | 57 | back_servo.calibrate(0.00085,90.0); |
WoodyERAU | 2:cc7237f357e4 | 58 | front_servo.calibrate(0.00085,90.0); |
WoodyERAU | 2:cc7237f357e4 | 59 | |
WoodyERAU | 1:1d2b72d09c56 | 60 | |
WoodyERAU | 1:1d2b72d09c56 | 61 | //Read in all PWM signals and set them to a value between 0 and 1 |
WoodyERAU | 2:cc7237f357e4 | 62 | elevation_output = (elev.pulsewidth()*1000)-1; |
WoodyERAU | 2:cc7237f357e4 | 63 | throttle_output = (thro.pulsewidth()*1000)-1; |
WoodyERAU | 2:cc7237f357e4 | 64 | rudder_output = (rudd.pulsewidth()*1000)-1; |
WoodyERAU | 2:cc7237f357e4 | 65 | aileron_output = (aile.pulsewidth()*1000)-1; |
WoodyERAU | 1:1d2b72d09c56 | 66 | |
WoodyERAU | 1:1d2b72d09c56 | 67 | //ESTOP PWM |
WoodyERAU | 1:1d2b72d09c56 | 68 | ESTOP_output = (gear.pulsewidth()*1000)-1; // >.5 run... <.5 STOP |
WoodyERAU | 1:1d2b72d09c56 | 69 | |
WoodyERAU | 1:1d2b72d09c56 | 70 | |
WoodyERAU | 1:1d2b72d09c56 | 71 | if(ESTOP_output > 0.5) |
WoodyERAU | 2:cc7237f357e4 | 72 | { |
WoodyERAU | 2:cc7237f357e4 | 73 | |
WoodyERAU | 2:cc7237f357e4 | 74 | //Servo Controllers |
WoodyERAU | 2:cc7237f357e4 | 75 | back_servo = aileron_output; |
WoodyERAU | 2:cc7237f357e4 | 76 | front_servo = rudder_output; |
WoodyERAU | 2:cc7237f357e4 | 77 | |
WoodyERAU | 2:cc7237f357e4 | 78 | //Thrust Controllers |
WoodyERAU | 2:cc7237f357e4 | 79 | back_thrust = throttle_output - 0.04; |
WoodyERAU | 2:cc7237f357e4 | 80 | front_thrust = elevation_output - 0.04; |
WoodyERAU | 1:1d2b72d09c56 | 81 | } |
WoodyERAU | 1:1d2b72d09c56 | 82 | else |
WoodyERAU | 1:1d2b72d09c56 | 83 | { |
WoodyERAU | 2:cc7237f357e4 | 84 | front_thrust = 0.46; |
WoodyERAU | 2:cc7237f357e4 | 85 | back_thrust = 0.46; |
WoodyERAU | 1:1d2b72d09c56 | 86 | } |
WoodyERAU | 1:1d2b72d09c56 | 87 | |
WoodyERAU | 2:cc7237f357e4 | 88 | wait(0.1); |
WoodyERAU | 2:cc7237f357e4 | 89 | |
WoodyERAU | 2:cc7237f357e4 | 90 | printf("\t\t:PWM: %4.2f\t", elevation_output); |
WoodyERAU | 2:cc7237f357e4 | 91 | printf(" \r\n"); |
WoodyERAU | 1:1d2b72d09c56 | 92 | } |
WoodyERAU | 1:1d2b72d09c56 | 93 | } |
WoodyERAU | 1:1d2b72d09c56 | 94 |