Median of 3 filter for multiple time of flight sensors translated from arduino by pololu

Dependencies:   mbed

Dependents:   wheelchaircontrol wheelchaircontrol2 wheelchaircontrol3 wheelchaircontrol4 ... more

Revision:
4:f6f0c2b9120a
Parent:
1:0038ad0a63af
Child:
5:a75d46c735c7
diff -r 0d2f2f31469b -r f6f0c2b9120a main.cpp
--- a/main.cpp	Sun May 05 02:40:10 2019 +0000
+++ b/main.cpp	Thu Aug 08 16:15:17 2019 +0000
@@ -1,34 +1,39 @@
 #include "mbed.h"
 #include "VL53L1X.h"
 
-Serial pc(USBTX,USBRX, 115200);
+Serial pc(USBTX,USBRX);
 
 VL53L1X sensor1(D4, D5, D6);
-VL53L1X sensor2(D4, D5, D9);
-VL53L1X sensor3(D4, D5, D10);
-VL53L1X sensor4(D4, D5, D11);
-VL53L1X sensor5(D4, D5, D12);
-VL53L1X sensor6(D4, D5, D13);
-
+VL53L1X sensor2(D4, D5, D7);
+VL53L1X sensor3(D4, D5, D8);
+VL53L1X sensor4(D4, D5, D9);
+/*VL53L1X frontRight(PD_13, PD_12, D3);    //(SCL, SDA, shutdown)
+VL53L1X frontRight2(PD_13, PD_12, D2);    //(SCL, SDA, shutdown)
+VL53L1X frontRight3(PD_13, PD_12, D4);    //(SCL, SDA, shutdown)
+VL53L1X frontRight4(PD_13, PD_12, D5);    //(SCL, SDA, shutdown)
+*/
 int main()
 {  
-    sensor1.initReading(0x25,50000);
-    sensor2.initReading(0x27,50000);
-    sensor3.initReading(0x35,50000);
-    sensor4.initReading(0x39,50000);
-    sensor5.initReading(0x41,50000);
-    sensor6.initReading(0x43,50000);
+wait(5);
+
+  /*  frontRight.initReading(0x25,50000);
+    frontRight2.initReading(0x27,50000);
+    frontRight3.initReading(0x35,50000);
+    frontRight4.initReading(0x37,50000);/*
+    */sensor1.initReading(0x27,50000);
+    sensor2.initReading(0x35,50000);
+    sensor3.initReading(0x37,50000);
 
     while(1)
     {
-      pc.printf("6,");
-      pc.printf("%d,", sensor1.readFromOneSensor());
-      pc.printf("%d,", sensor2.readFromOneSensor());
-      pc.printf("%d,", sensor3.readFromOneSensor());
-      pc.printf("%d,", sensor4.readFromOneSensor()); 
-      pc.printf("%d,", sensor5.readFromOneSensor()); 
-      pc.printf("%d", sensor6.readFromOneSensor()); 
-      
+      pc.printf("%d, ", sensor1.readFromOneSensor());
+      pc.printf("%d, ", sensor2.readFromOneSensor());
+      pc.printf("%d, ", sensor3.readFromOneSensor());
       pc.printf("\r\n");
+      /*
+      pc.printf("%d hello\r\n", frontRight.readFromOneSensor()); 
+      pc.printf("%d hello\r\n", frontRight2.readFromOneSensor()); 
+      pc.printf("%d hello\r\n", frontRight3.readFromOneSensor()); 
+      pc.printf("%d hello\r\n", frontRight4.readFromOneSensor()); */
     }
 }