Diff: main.cpp

Revision:

1:1c796b8db63c

Parent:

0:e0ff0fa89d47

Child:

2:22d86efbbada

--- a/main.cpp	Thu Feb 16 14:50:05 2017 +0000
+++ b/main.cpp	Thu Feb 16 20:43:10 2017 +0000
@@ -2,6 +2,9 @@
 
 
 #define MAX 0x0A
+#define HI 0x01
+#define LO 0x00
+
 DigitalOut led_1(LED1);         //program running.
 DigitalOut led_2(LED2);         //sensors operating.
 DigitalOut led_3(LED3);         //is moving.
@@ -9,6 +12,10 @@
 DigitalOut Bit1(p25);
 DigitalOut Bit2(p24);
 DigitalOut Bit3(p23);
+DigitalOut Trig(p6);
+DigitalIn Echo(p7);
+
+Timer US;
 
 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};  
@@ -16,20 +23,35 @@
 int US3_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
 int US4_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
 int US5_mean[MAX]={0,0,0,0,0,0,0,0,0,0};
+int turn_rate[2]={0,0};
 
-void setActiveUS(int chan);
-int getPing(void);
+void setActiveUS(int chan);             //select sensor
+int getPing(void);                      //get measurement
+int turn(int direction, int rate);      //turn car -1 left 0 centre 1 right
+void drive(int direction, int speed);   //drive -1 reverse 1 forward
+void stop(void);
+double measurement_mean(int *Array);
+
 
 int main() {
     int iCount = 0;
     int measured = 0;
+
     while(iCount <= 5){
-        setActiveUS(iCount);
-        measured = getPing();
-        rawUS_data[iCount] = measured;
+        setActiveUS(iCount);            //set mux address
+        measured = getPing();           //get measurement
+        rawUS_data[iCount] = measured;  //get raw measurement
+        wait(0.4);                      //will need calibrating
         }
-    US1_mean[0]=rawUS_data[0];
-    US2_mean[0]=rawUS_data[1];
+    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];
+        }
+    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];
@@ -41,32 +63,80 @@
     switch(chan){
         case 0:
             //ultrasonic 1
-            break;
+            Bit1 = 0;
+            Bit2 = 0;
+            Bit3 = 0;
+            return;
         
         case 1:
             //ultrasonic 2
-            break;
+            Bit1 = 1;
+            Bit2 = 0;
+            Bit3 = 0;
+            return;
             
         case 2:
             //ultrasonic 3
-            break;
+            Bit1 = 0;
+            Bit2 = 1;
+            Bit3 = 0;
+            return;
         
         case 3:
             //ultrasonic 4
-            break;
+            Bit1 = 1;
+            Bit2 = 1;
+            Bit3 = 0;
+            return;
         
         case 4:
             //ultrasonic 5
-            break;
+            Bit1 = 0;
+            Bit2 = 0;
+            Bit3 = 1;
+            return;
         
         }
     }
 
 int getPing(void){
     int result=0;   
-    //write ultrasonic code
-    //return measured value
+    //start positive edge of trigger
+    Trig = HI;
+    US.reset();
+    wait_us(10.0);  //hold it high for 10us
+    Trig = LO;      //set negative going edge
+    while(Echo == 0){}; //while receive is LO
+    US.start();         //start counting received pulse
+    while(Echo == 1){};
+    US.stop();          
+    result = ((US.read_us()*10)/58);    //pulse duration = distance set to cm
+    return result;    
+
+}
+
+int turn(int direction, int rate){
+    
+    return 0;
+    }
+    
+void drive(int direction, int speed){
+    
+    return;
+    }
+    
+void stop(void){
     
     
-    return result;
-}
\ No newline at end of file
+    return;
+    }
+
+double measurement_mean(int *Array){
+    int value=0;
+    int sum = 0;
+    for(int i=0;i<=MAX;i++){
+        sum +=Array[i];
+        }
+    value = sum/MAX;
+    return value;
+    }