Diff: LineFollowingRobot/IRsensor.cpp

Revision:

4:645b5d648c64

Parent:

3:54c41af9e119

Child:

5:bb0bec710c91

--- a/LineFollowingRobot/IRsensor.cpp	Sun Feb 03 16:42:44 2019 +0000
+++ b/LineFollowingRobot/IRsensor.cpp	Wed Feb 06 14:02:07 2019 +0000
@@ -36,7 +36,7 @@
     m_lineSensor[3] = m_midRightIR;
     m_lineSensor[4] = m_rightIR;
     
-    m_toggle = m_toggle;
+    m_toggle = !m_toggle;
 }
 
 
@@ -85,24 +85,26 @@
     //initSpeed : speed without correction 
     //check previousSpeed to make sure the direction is the same
     float initSpeed = 0.2;
-    
+    float error;
     
     //scale and assign speed
-    m_error = m_error / 5;
-    m_speedL = initSpeed - m_error;
-    m_speedR = initSpeed + m_error;
+    error = m_error / 5.0f;
+    m_speedL = initSpeed - error;
+    m_speedR = initSpeed + error;
     
-    m_speedL > 0 ? m_dirL = true : m_dirR = false;
-    if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
+    m_speedL >= 0 ? m_dirL = true : m_dirR = false;
+    //if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
     
-    m_speedR > 0 ? m_dirL = true : m_dirR = false;
-    if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
+    m_speedR >= 0 ? m_dirL = true : m_dirR = false;
+    //if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
     
-    controlLeft.SetSpeed(abs(m_speedL));
-    controlRight.SetSpeed(abs(m_speedR));
+    //controlLeft.SetSpeed(abs(m_speedL));
+    //controlRight.SetSpeed(abs(m_speedR));
     
-    m_prevDirL = m_dirL;
-    m_prevDirR = m_dirR;
+    //m_prevDirL = m_dirL;
+    //m_prevDirR = m_dirR;
+    
+    //printf("leftSpeed : %f, rightSpeed %f", m_speedL, m_speedR);