Diff: Sensor/LaneSensor.cpp

Revision:

0:a0e9db1628f1

diff -r 000000000000 -r a0e9db1628f1 Sensor/LaneSensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensor/LaneSensor.cpp	Mon Nov 16 22:37:13 2020 +0000
@@ -0,0 +1,61 @@
+#include "LaneSensor.h"
+
+LaneSensor::LaneSensor(PinName lll,PinName ll,PinName l,PinName m,PinName r,PinName rr,PinName rrr)
+    : sensorsIn_(lll,ll,l,m,r,rr,rrr)
+{
+    error_=0.0;
+    prevError_=0.0;
+    //sensorsIn_.mode(PullNode);
+}
+float LaneSensor::getError()
+{
+    float errorSum=0;
+    int errorCount=0;
+    float errorConstant =22.86;
+
+    int sensorIn=sensorsIn_ & sensorsIn_.mask();
+    if((sensorIn&0x1)==0x0) {
+        errorSum+=errorConstant*3.0;
+        errorCount++;
+    }
+    if((sensorIn&0x2)==0x0) {
+        errorSum+=errorConstant*2.0;
+        errorCount++;
+    }
+    if((sensorIn&0x4)==0x0) {
+        errorSum+=errorConstant*0.7;
+        errorCount++;
+    }
+    if((sensorIn&0x8)==0x0) {
+        errorSum+=0.0;
+        errorCount++;
+    }
+    if((sensorIn&0x10)==0x0) {
+        errorSum+=errorConstant*-0.7;
+        errorCount++;
+    }
+    if((sensorIn&0x20)==0x0) {
+        errorSum+=errorConstant*-2.0;
+        errorCount++;
+    }
+    if((sensorIn&0x40)==0x0) {
+        errorSum+=errorConstant*-3.0;
+        errorCount++;
+    }
+
+
+    if(errorCount==0) {
+        error_=prevError_;
+    } else {
+        error_=errorSum/((float)errorCount);
+    }
+    prevError_=error_;
+
+    return error_;
+}
+
+int LaneSensor::getData()
+{
+
+    return sensorsIn_ & sensorsIn_.mask();;
+}
\ No newline at end of file