Diff: Sources/DistanceSensors.cpp

Revision:

4:67d7177c213f

Parent:

1:388c915756f5

Child:

17:4e1be70bdedb

--- a/Sources/DistanceSensors.cpp	Wed Mar 22 13:33:07 2017 +0000
+++ b/Sources/DistanceSensors.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -1,5 +1,6 @@
 #include <cmath>
 #include "Robot.h"
+#include "Declarations.h"
 
 
 
@@ -8,15 +9,19 @@
     //Nothing
 }
     
-DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
 {   
-    init(sensorVoltage, bit0, bit1, bit2, number);
+    init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number);
 }
 
 //initialise
-void DistanceSensors::init(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+void DistanceSensors::init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
 {
     this->sensorVoltage = sensorVoltage;
+    this->frontS = frontS;
+    this->leftS = leftS;
+    this->rightS = rightS;
+    
     this->bit0 = bit0;
     this->bit1 = bit1;
     this->bit2 = bit2;
@@ -30,12 +35,32 @@
 
 
 float DistanceSensors::read()//Return the distance of an object
-{  
-    *bit0 = number & 1; // Set the first bit of the Sensors MUX
-    *bit1 = number & 2; // Set the second bit of the Sensors MUX
-    *bit2 = number & 4; // Set the third bit of the Sensors MUX
+{
     
-    float Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor   
+    float Usensor;
+  
+    if (number < 6){
+        *bit0 = number & 1; // Set the first bit of the Sensors MUX
+        *bit1 = number & 2; // Set the second bit of the Sensors MUX
+        *bit2 = number & 4; // Set the third bit of the Sensors MUX
+        
+        Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
+    }
+    else{
+        switch(number){
+            case 6:
+                Usensor=frontS->read();
+                break;
+            case 7:
+                Usensor=leftS->read();
+                break;
+            case 8:
+                Usensor=rightS->read();
+                break;
+        }
+    }
+    
+    //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
     float Distance=-0.38f*sqrt(Usensor)+0.38f;
     return Distance;
 }