Diff: LightCommunication.cpp

Revision:

11:e9faca228d30

Parent:

10:7eaaa891ab81

Child:

12:f55b31b5bc4a

--- a/LightCommunication.cpp	Thu May 11 15:27:12 2017 +0200
+++ b/LightCommunication.cpp	Thu May 11 15:43:21 2017 +0200
@@ -53,38 +53,39 @@
 float LightCommunication::getRight(float cur_right)
 {
 
-    float r = right;
-    mut->unlock();
-    /*
+    float r = right - cur_right;
+
+    float cur_speed = 0;
     if (r>0) {
-        return (float)(cur_right + (0.005*SPEED));
+        cur_speed = (float)(cur_right + (0.005*SPEED));
     } else if (r<0) {
 
-        return (float)(cur_right - (0.005*SPEED));
+        cur_speed = (float)(cur_right - (0.005*SPEED));
     } else {
-        return cur_right;
-    }*/
-    return r;
+        cur_speed = right;
+    }
+    mut->unlock();
+    return cur_speed;
 }
 
 float LightCommunication::getLeft(float cur_left)
 {
 
     mut->lock();
-    /*float l = left - cur_left;
+    float l = left - cur_left;
     if (l>0) {
         return (float)(cur_left + (0.005*SPEED));
     } else if (l<0) {
         return (float)(cur_left - (0.005*SPEED));
     } else {
         return cur_left;
-    }*/
-    return left;
+    }
+    //return left;
 }
 
 float LightCommunication::toFloat(int8_t a)
 {
-    return (((float)a)/100.0)*SPEED;
+    return (((float)a)/100.0f)*SPEED;
 }
 
 bool LightCommunication::needsToStop()