Diff: main.cpp

Revision:

4:e02ad2d9ba69

Parent:

3:64b5ea1e088f

--- a/main.cpp	Wed Jun 05 16:28:00 2019 +0000
+++ b/main.cpp	Sun Jun 16 19:35:05 2019 +0000
@@ -2,12 +2,20 @@
 #include "Servo.h"
 #include "PwmIn.h"
 
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////        setup        //////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+
 //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);
@@ -19,6 +27,14 @@
 PwmIn gear(p13);
 PwmIn aile(p14);
 PwmIn rudd(p16);
+PwmIn aux(p18);
+
+Serial pc(USBTX, USBRX);
+
+
+////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////  global variables   //////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
 
 //Set all PWM values to a defaalt of 0
 float throttle_output = 0.0;
@@ -26,16 +42,77 @@
 float ESTOP_output = 0.0;
 float rudder_output = 0.0;
 float aileron_output = 0.0;
+float aux_output = 0.0;
+
+//Variables for Serial Communcation with Labview
+volatile bool newData = false;
+volatile float inputs[4];
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////     Functions      ///////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+
+//Function changes 0 t 1 input into 0.08 to 0.92
+float RangeChange(float x)
+{
+    float y;
+    y = ((x * 0.84) + 0.08);
+    return y;
+}
+
+
+
+
 
 
-int main() {
-    
+//Function Reads Serial com
+void onSerialRx()
+{
+    static char serialInBuffer[100]; 
+    static int serialCount = 0;
+ 
+    while (pc.readable())// in case two bytes are ready
+    {
+        char byteIn = pc.getc();//Read serial message
+        
+        if (byteIn == 0x65)// if an end of line is found
+        { 
+            serialInBuffer[serialCount] == 0; // null terminate the input                
+            float w,x,y,z;
+            if (sscanf(serialInBuffer,"%f,%f,%f,%fe",&w,&x,&y,&z) == 4)// managed to read all 4 values
+            { 
+                inputs[0] = w;
+                inputs[1] = x;
+                inputs[2] = y;
+                inputs[3] = z;
+                newData = true;
+            }
+            serialCount = 0; // reset the buffer
+        } 
+        else
+        {
+            serialInBuffer[serialCount] = byteIn;// store the character
+            if (serialCount<100)
+            {
+                serialCount++;// increase the counter.
+            }
+        }
+    }
+}
+
+
+
+
+void Calibrate()
+{
     //Calibration Sequence
     back_servo = 0.0;
     front_servo = 0.0;
     back_thrust = 0.0;
     front_thrust = 0.0;
-    wait(0.5); //ESC detects signal
+    wait(0.1); //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;
@@ -47,12 +124,26 @@
     front_servo = 0.0;
     back_thrust = 0.0;
     front_thrust = 0.0;
-    wait(0.5);
+    wait(0.1);
     //End calibration sequence
+}
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////        Main        ///////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+
+int main() {
     
+    Calibrate();
+    
+    pc.attach(&onSerialRx);
+    unsigned int expired = 0;
     
     while(1) {
         
+        
         //Enable Servo to turn 180 degrees
         back_servo.calibrate(0.00085,90.0);
         front_servo.calibrate(0.00085,90.0);
@@ -64,13 +155,19 @@
         rudder_output = (rudd.pulsewidth()*1000)-1;
         aileron_output = (aile.pulsewidth()*1000)-1;
         
+        //RC vs Auto PWM
+        aux_output = (aux.pulsewidth()*1000)-1; // >.5 RC... <.5 Auto    
+        
         //ESTOP PWM
         ESTOP_output = (gear.pulsewidth()*1000)-1;  // >.5 run... <.5 STOP
         
-
+        
+        
         if(ESTOP_output > 0.5)
-            {   
+        {   
                 
+            if(aux_output > 0.5) //RC
+            {
                 //Servo Controllers
                 back_servo = aileron_output;
                 front_servo = rudder_output;
@@ -78,17 +175,40 @@
                 //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);
+            else //Auto
+            {   
+                if (newData)
+                {  
+                    newData = false;//Reset NewData Boolean
+                    
+                    front_servo = inputs[0];//Set thruster values
+                    front_thrust = RangeChange(inputs[1]) - 0.04;
+                    back_servo = inputs[2];
+                    back_thrust = RangeChange(inputs[3]) - 0.04;
+                    
+                    expired = 0;//Reset Expried
+                }
+                else
+                {
+                    expired++; //Count the number of loops without new data
+                }
         
-        printf("\t\t:PWM: %4.2f\t", elevation_output); 
-        printf(" \r\n");        
+                if(expired > 10000000) //If too many loops have passed with no new data, assume Labview crashed
+                {
+                    //indicate loss of labview com
+                    back_thrust = 0.46;
+                    front_thrust = 0.46;
+                }                
+            }
         }
+        else
+        {
+            front_thrust = 0.46;
+            back_thrust = 0.46;
+            
+        }         
+    }
 }