Diff: tof.h

Revision:

3:9b2f15b0d47b

Parent:

1:b188e27eb7da

Child:

4:538efc40388e

--- a/tof.h	Mon Sep 30 08:51:52 2019 +0000
+++ b/tof.h	Mon Sep 30 10:19:31 2019 +0000
@@ -1,26 +1,76 @@
 void InitTOFSensors()
 {
-    sensorFront.init();
-    sensorFront.setTimeout(500);
-    sensorFront.setMeasurementTimingBudget(40000);
-    sensorFront.setSignalRateLimit(0.9);
-    sensorFront.startContinuous();
+    DigitalOut xshut1(XSHUT_pin1);
+    DigitalOut xshut2(XSHUT_pin2);
+    DigitalOut xshut3(XSHUT_pin3);
+    xshut1 = 0;
+    xshut2 = 0;
+    xshut3 = 0;
+
+    xshut3 = 1;
+    wait(0.01);
+    Sensor3.setAddress(Sensor3_newAddress);
+
+    xshut2 = 1;
+    wait(0.01);
+    Sensor2.setAddress(Sensor2_newAddress);
+
+    xshut1 = 1;
+    wait(0.01);
+    Sensor1.setAddress(Sensor1_newAddress);
+
+    if (Sensor1.init())
+    {
+        PrintSerial("Sensor 1 : Configured");
+    }
+    else
+    {
+        PrintSerial("Sensor 1 : Configuration Failed");
+    }
+    if (Sensor2.init())
+    {
+        PrintSerial("Sensor 2 : Configured");
+    }
+    else
+    {
+        PrintSerial("Sensor 2 : Configuration Failed");
+    }
+    if (Sensor3.init())
+    {
+        PrintSerial("Sensor 3 : Configured");
+    }
+    else
+    {
+        PrintSerial("Sensor 3 : Configuration Failed");
+    }
+
+    Sensor1.setSignalRateLimit(SignalRateLimit);
+    Sensor2.setSignalRateLimit(SignalRateLimit);
+    Sensor3.setSignalRateLimit(SignalRateLimit);
+
+    Sensor1.setMeasurementTimingBudget(MeasurementBudget);
+    Sensor2.setMeasurementTimingBudget(MeasurementBudget);
+    Sensor3.setMeasurementTimingBudget(MeasurementBudget);
+
+    Sensor1.setTimeout(Timeout);
+    Sensor2.setTimeout(Timeout);
+    Sensor3.setTimeout(Timeout);
+
+    Sensor1.startContinuous();
+    Sensor2.startContinuous();
+    Sensor3.startContinuous();
 }
 
-void ReadTOFDistance()
-{
-    int distance = sensorFront.readRangeContinuousMillimeters();
-    float distanceCM = (float)(distance / 10.0);
-    frontDistance = distanceCM;
-}
-
-void PrintTOFDistance()
+void PrintTOFDistances()
 {
-    int distance = sensorFront.readRangeContinuousMillimeters();
-    float distanceCM = (float)(distance / 10.0);
-    printf("%.2f\n",distanceCM);
-    if (sensorFront.timeoutOccurred())
-    {
-        PrintSerial(" TIMEOUT");
-    }
-}
\ No newline at end of file
+    front_Distance = Sensor1.readRangeContinuousMillimeters();
+    float front_DistanceCM = (float)( front_Distance  / 10.0);
+
+    left_Distance = Sensor2.readRangeContinuousMillimeters() ;
+    float left_DistanceCM = (float)(left_Distance / 10.0);
+
+    right_Distance = Sensor3.readRangeContinuousMillimeters();
+    float right_DistanceCM = (float)(right_Distance / 10.0);
+    
+    printf("F: %.2f L: %.2f R: %.2f",front_DistanceCM,left_DistanceCM,right_DistanceCM);
+}