Diff: LongRangeSensor.cpp

Revision:

0:c8c67fabc804

Child:

1:aed20a8e1da0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LongRangeSensor.cpp	Tue Mar 22 03:00:35 2016 +0000
@@ -0,0 +1,165 @@
+#include "LongRangeSensor.h"
+#include "mbed.h"
+#include <cmath> // pow
+
+// FUNCTION:
+//      LongRangeSensor(Serial &pc1, PinName _sensor)
+// IN-PARAMETERS:
+//      Serial &pc1, PinName _sensor
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Constructor.
+LongRangeSensor::LongRangeSensor(Serial &pc1, PinName _sensor) : 
+    pc(pc1), sensor(_sensor) 
+{}
+
+// FUNCTION:
+//      distL()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      Returns distance in cm
+// DESCRIPTION:
+//      Returns average of 3 readings in cm.
+float LongRangeSensor::distL()
+{
+    double sensorRead = 0.0;
+    
+    for(int i = 0; i < 3; i++)
+                {
+                    sensorRead += sensor.read();
+                    wait(20e-3);
+                }
+    sensorRead = sensorRead/3.0;
+    
+    double distMeas = 3.5097*pow(sensorRead, -1.425);
+    
+    return distMeas;
+}
+
+// FUNCTION:
+//      distInchesL()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      Returns distance in cm
+// DESCRIPTION:
+//      Returns average of 3 readings in inches.
+float LongRangeSensor::distInchesL()
+{
+    double sensorRead = 0.0;
+    
+    for(int i = 0; i < 3; i++)
+                {
+                    sensorRead += sensor.read();
+                    wait(20e-3);
+                }
+    sensorRead = sensorRead/3.0;
+    
+    double distMeas = 3.5097*pow(sensorRead, -1.425);
+    
+    return distMeas*0.393701;
+}
+
+// FUNCTION:
+//      distR()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      Returns distance in cm
+// DESCRIPTION:
+//      Returns average of 3 readings in cm.
+float LongRangeSensor::distR()
+{
+    double sensorRead = 0.0;
+    
+    for(int i = 0; i < 3; i++)
+                {
+                    sensorRead += sensor.read();
+                    wait(20e-3);
+                }
+    sensorRead = sensorRead/3.0;
+    
+    double distMeas = 3.4617*pow(sensorRead, -1.506);
+    
+    return distMeas;
+}
+
+// FUNCTION:
+//      distInchesR()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      Returns distance in cm
+// DESCRIPTION:
+//      Returns average of 3 readings in inches.
+float LongRangeSensor::distInchesR()
+{
+    double sensorRead = 0.0;
+    
+    for(int i = 0; i < 3; i++)
+                {
+                    sensorRead += sensor.read();
+                    wait(20e-3);
+                }
+    sensorRead = sensorRead/3.0;
+    
+    double distMeas = 3.4617*pow(sensorRead, -1.506);
+    
+    return distMeas*0.393701;
+}
+
+// FUNCTION:
+//      distAdjL()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      Returns adjusted distance in cm
+// DESCRIPTION:
+//      Gets dist from sensor L and adjusts for error.
+float LongRangeSensor::distAdjL()
+{
+    double sensorRead = 0.0;
+    
+    for(int i = 0; i < 3; i++)
+                {
+                    sensorRead += sensor.read();
+                    wait(20e-3);
+                }
+    sensorRead = sensorRead/3.0;
+    
+    double distMeas = 3.5097*pow(sensorRead, -1.425);
+    
+    double readError = (0.0052*pow(distMeas, 2.0)) - (0.3218*distMeas) 
+                + 2.6653;
+    
+    return float(distMeas + readError);
+}
+
+// FUNCTION:
+//      distAdjR()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      Returns adjusted distance in cm.
+// DESCRIPTION:
+//      Gets dist for sensor R and adjusts for error.
+float LongRangeSensor::distAdjR()
+{
+    double sensorRead = 0.0;
+    
+    for(int i = 0; i < 3; i++)
+                {
+                    sensorRead += sensor.read();
+                    wait(20e-3);
+                }
+    sensorRead = sensorRead/3.0;
+    
+    double distMeas = 3.4617*pow(sensorRead, -1.506);
+    
+    double readError = (0.0062*pow(distMeas, 2.0)) - (0.385*distMeas)
+                + 3.1692;
+                
+    return float(distMeas + readError);
+}
\ No newline at end of file