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

Dependencies:   mbed

Dependents:   wheelchaircontrol wheelchaircontrol2 wheelchaircontrol3 wheelchaircontrol4 ... more

Revision:
5:a75d46c735c7
Parent:
4:f6f0c2b9120a
--- a/main.cpp	Thu Aug 08 16:15:17 2019 +0000
+++ b/main.cpp	Wed Jun 03 20:11:31 2020 +0000
@@ -2,33 +2,49 @@
 #include "VL53L1X.h"
 
 Serial pc(USBTX,USBRX);
+VL53L1X sensor1(PD_13, PD_12, PA_15);   // Block 1
+VL53L1X sensor2(PD_13, PD_12, PC_7);
+VL53L1X sensor3(PD_13, PD_12, PB_5);
 
-VL53L1X sensor1(D4, D5, D6);
-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)
-*/
+VL53L1X sensor4(PD_13, PD_12, PE_11);   // Block 2
+VL53L1X sensor5(PD_13, PD_12, PF_14);
+VL53L1X sensor6(PD_13, PD_12, PE_13);
+
+VL53L1X sensor7(PD_13, PD_12, PG_15);   // Block 3
+VL53L1X sensor8(PD_13, PD_12, PG_14);
+VL53L1X sensor9(PD_13, PD_12, PG_9);
+
+VL53L1X sensor10(PD_13, PD_12, PE_10);  // Block 4
+VL53L1X sensor11(PD_13, PD_12, PE_12);     
+VL53L1X sensor12(PD_13, PD_12, PE_14);
 int main()
 {  
-wait(5);
-
-  /*  frontRight.initReading(0x25,50000);
-    frontRight2.initReading(0x27,50000);
-    frontRight3.initReading(0x35,50000);
-    frontRight4.initReading(0x37,50000);/*
-    */sensor1.initReading(0x27,50000);
+    sensor1.initReading(0x27,50000);
     sensor2.initReading(0x35,50000);
     sensor3.initReading(0x37,50000);
-
+    sensor4.initReading(0x39,50000);
+    sensor5.initReading(0x41,50000);
+    sensor6.initReading(0x43,50000);
+    sensor7.initReading(0x45,50000);
+    sensor8.initReading(0x47,50000);
+    sensor9.initReading(0x49,50000);
+    sensor10.initReading(0x51,50000);    
+    sensor11.initReading(0x53,50000);
+    sensor12.initReading(0x55,50000);
     while(1)
     {
       pc.printf("%d, ", sensor1.readFromOneSensor());
       pc.printf("%d, ", sensor2.readFromOneSensor());
-      pc.printf("%d, ", sensor3.readFromOneSensor());
+      pc.printf("%d, ", sensor3.readFromOneSensor());      
+      pc.printf("%d, ", sensor4.readFromOneSensor());
+      pc.printf("%d, ", sensor5.readFromOneSensor());
+      pc.printf("%d, ", sensor6.readFromOneSensor());     
+      pc.printf("%d, ", sensor7.readFromOneSensor());
+      pc.printf("%d, ", sensor8.readFromOneSensor());
+      pc.printf("%d, ", sensor9.readFromOneSensor());
+      pc.printf("%d, ", sensor10.readFromOneSensor());
+      pc.printf("%d, ", sensor11.readFromOneSensor());
+      pc.printf("%d, ", sensor12.readFromOneSensor());
       pc.printf("\r\n");
       /*
       pc.printf("%d hello\r\n", frontRight.readFromOneSensor()); 
@@ -36,4 +52,4 @@
       pc.printf("%d hello\r\n", frontRight3.readFromOneSensor()); 
       pc.printf("%d hello\r\n", frontRight4.readFromOneSensor()); */
     }
-}
+}
\ No newline at end of file