Diff: main.cpp

Revision:

2:cc7237f357e4

Parent:

1:1d2b72d09c56

Child:

3:64b5ea1e088f

--- 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");        
         }
 }