Diff: main.cpp

Revision:

5:0382ed1bf13d

Parent:

3:9902261b1156

Child:

6:e15a4b0c8cbf

--- a/main.cpp	Sat Mar 11 13:08:06 2017 +0000
+++ b/main.cpp	Sat Mar 11 13:31:50 2017 +0000
@@ -1,6 +1,5 @@
 #include "mbed.h"
 
-
 #define MAX 0x0A
 #define HI 0x01
 #define LO 0x00
@@ -9,18 +8,18 @@
 DigitalOut led_2(LED2);         //sensors operating.
 DigitalOut led_3(LED3);         //is moving.
 DigitalOut led_4(LED4);         //is complete.
+
 DigitalOut Bit1(p25);
 DigitalOut Bit2(p24);
 DigitalOut Bit3(p23);
 DigitalOut Trig(p6);
 DigitalIn Echo(p7);
+
 PwmOut steering(p21);
 PwmOut velocity(p22);
- 
 
 Timer US;
 
-
 //global variable dec
 int rawUS_data[5]={0,0,0,0,0};  //raw data{chan1,chan2,chan3,chan4,chan5}
 int US1_mean[MAX]={0,0,0,0,0,0,0,0,0,0};  
@@ -39,10 +38,9 @@
 void stop(void);
 int measurement_mean(int chan);
 
-
 int main() {
     int iCount = 0;
-    int measured = 0;
+    int measured = 0;       
     int iMean = 0;
     double mean_measured1[2] = {0,0};
     double mean_measured2[2] = {0,0};
@@ -56,24 +54,25 @@
             }
         while(iCount <= 5){
             setActiveUS(iCount);            //set mux address
-            measured = getPing();           //get measurement
-            rawUS_data[iCount] = measured;  //get raw measurement
+            rawUS_data[iCount] = getPing();  //get raw measurement
             iCount +=1;
             wait(0.4);                      //will need calibrating
-            }
+        }
+        
         US1_mean[0]=rawUS_data[0];          //assign new value
         US2_mean[0]=rawUS_data[1];          //to respective sensor
         US3_mean[0]=rawUS_data[2];
         US4_mean[0]=rawUS_data[3];
         US5_mean[0]=rawUS_data[4];
-    
+        
         for(int i=MAX;i>0;i--){
             US1_mean[i] = US1_mean[i-1];    //index data along window
             US2_mean[i] = US1_mean[i-1];    //wrt size of window
             US3_mean[i] = US1_mean[i-1];
             US4_mean[i] = US1_mean[i-1];
             US5_mean[i] = US1_mean[i-1];
-            }
+        }
+        
         mean_measured1[0] = measurement_mean(1);    //calc mean 1
         mean_measured2[0] = measurement_mean(2);    //calc mean 2
         mean_measured3[0] = measurement_mean(3);    //calc mean 3
@@ -86,8 +85,8 @@
             mean_measured3[1] = mean_measured3[0];
             mean_measured4[1] = mean_measured4[0];
             mean_measured5[1] = mean_measured5[0];
-            }
-            
+        }
+        
         //if the second value is smaller than the first value
         //steer right
         
@@ -137,14 +136,13 @@
             Bit2 = 0;
             Bit3 = 1;
             return;
-        
         }
     }
 
 int getPing(void){
-    int result=0;   
-                        //start positive edge of trigger
-    Trig = HI;
+    int result=0;
+    
+    Trig = HI;          //start positive edge of trigger
     US.reset();
     wait_us(10.0);      //hold it high for 10us
     Trig = LO;          //set negative going edge
@@ -154,33 +152,24 @@
     US.stop();          
     result = ((US.read_us()*10)/58);    //pulse duration = distance set to cm
     return result;    
-
 }
 
-void turn(float s){
-    s=s+1;
-    if (s>=0 && s<=2) {
-        steering.pulsewidth(s/2000+0.001);
-    }
-
-    return;
-    }
+void turn(float t){
+    if (t++>=0&&s<=2) {steering.pulsewidth(t/2000+0.001);}                      // calculate steering angle to pulsewidth conversion
+    telemUpdate();                                                              // save telem file after angle change
+    return;    
+}
     
 void drive(float v){
-     v=v+1;
-    if (v>=0 && v<=2) {
-        //no reverse code
-        velocity.pulsewidth(v/2000+0.001);
-        vo=v;
-        }
+    if (v++>=0&&s<=2) {velocity.pulsewidth(v/2000+0.001);}                      // calculate velocity to pulsewidth conversion
+    telemUpdate(0.0);                                                           // save telem file after speed change
     return;
-    }
-    
+}
+
 void stop(void){
-    drive(0.0);
-    
+    drive(0.0);                                                                 // Reduce speed to 0
     return;
-    }
+}
 
 int measurement_mean(int chan){
     int value=0;