Diff: main.cpp

Revision:

1:1d2b72d09c56

Parent:

0:4fb155dd1c7a

Child:

2:cc7237f357e4

--- a/main.cpp	Mon May 27 21:05:13 2019 +0000
+++ b/main.cpp	Mon May 27 21:56:20 2019 +0000
@@ -1,4 +1,81 @@
 #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() {}
\ No newline at end of file
+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;
+            }
+
+
+        }
+}
+