Diff: Sources/DistanceSensors.cpp

Revision:

0:d267b248eff4

Child:

1:388c915756f5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/DistanceSensors.cpp	Sat Mar 11 10:14:00 2017 +0000
@@ -0,0 +1,46 @@
+#include <cmath>
+#include "Robot.h"
+
+
+
+Robot::DistanceSensors::DistanceSensors() 
+{
+    //Nothing
+}
+    
+Robot::DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+{   
+    init(sensorVoltage, bit0, bit1, bit2, number);
+}
+
+//initialise
+void Robot::DistanceSensors::init(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+{
+    this->sensorVoltage = sensorVoltage;
+    this->bit0 = bit0;
+    this->bit1 = bit1;
+    this->bit2 = bit2;    
+}
+
+
+Robot::DistanceSensors::~DistanceSensors()
+{ 
+}
+
+
+float Robot::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 Distance=-0.38f*sqrt(Usensor)+0.38f;
+    return Distance;
+}
+
+Robot::DistanceSensors::operator float()
+{ 
+    return read();
+}
+