Diff: LineFollowingRobot/IRsensor.cpp

Revision:

5:bb0bec710c91

Parent:

4:645b5d648c64

diff -r 645b5d648c64 -r bb0bec710c91 LineFollowingRobot/IRsensor.cpp
--- a/LineFollowingRobot/IRsensor.cpp	Wed Feb 06 14:02:07 2019 +0000
+++ b/LineFollowingRobot/IRsensor.cpp	Thu Mar 07 14:00:42 2019 +0000
@@ -47,19 +47,27 @@
     
     for(i = 0; i <5; i++){
         
-        if(m_lineSensor[i]){
+        
+        if(!m_lineSensor[i]){
+            
             if(!count){
                 m_error = (i-2)*2;
                 count++;
-            }else if(count == 1){((m_error >= 0) ^ ((i-2)*2<0))? m_error++ : m_error = m_color;}
-            else if(count == 2){ ((m_error >= 0) ^ ((i-2)*2<0))?  : m_error = m_color;}
-            else if(count<2){ printf("error : to many reading");}
+                
+            }else if(count == 1){
+                ((m_error >= 0) ^ ((i-2)*2<0))||((m_error <= 0) ^ ((i-2)*2>0)) ? m_error++ : m_error = m_color;
+                count++;
+                }
+            else if(count == 2){ 
+                ((m_error >= 0) ^ ((i-2)*2<0))?  : m_error = m_color;
+                count++;
+                }
+            else if(count>2){ printf("error : to many reading");}
+            
         }
     }
-        
+
     if(!count){ m_prevError<0 ? m_error = -5 : m_error = 5;}
-
-    m_error = m_prevError;
 }
     
 
@@ -69,43 +77,40 @@
     //*previousError : pointer to the previous error calculated by weightPID
     //returns the PID value
     
-    m_P = m_error;
-    m_I = m_I + m_error;
-    m_D = m_error - m_prevError;
+    m_P = m_error*1.0;
+    m_I = (m_I + m_error)*1.0;
+    m_D = 1.0*(m_error - m_prevError);
         
     m_prevError = m_error;
     
-    m_error = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
+    m_PID = m_Kp*m_P + m_Ki*m_I + m_Kd*m_D;
+    //printf("error is : %i , Kp is :  %f , P is : %f   \r",m_error, m_Kp, m_P);    
+
 }
 
 
-void IRSensor::MotorControl(RobotControl controlLeft, RobotControl controlRight){
+void IRSensor::MotorControl(){
      //assigns the calculated direction to the motors
     //PIDvalue : calculated PID value given by CalculatePID
     //initSpeed : speed without correction 
     //check previousSpeed to make sure the direction is the same
-    float initSpeed = 0.2;
+    float initSpeed = 0.21;
     float error;
     
     //scale and assign speed
-    error = m_error / 5.0f;
+    error = m_PID / 5.0;
+    
+    //printf("m_error : %f \t error : %f \r", m_error, error);
     m_speedL = initSpeed - error;
     m_speedR = initSpeed + error;
     
-    m_speedL >= 0 ? m_dirL = true : m_dirR = false;
+    m_speedL >= 0 ? m_dirL = true : m_dirL = false;
     //if(m_dirL != m_prevDirL) controlLeft.SetDirection(m_dirL);
     
-    m_speedR >= 0 ? m_dirL = true : m_dirR = false;
+    m_speedR >= 0 ? m_dirR = true : m_dirR = false;
     //if(m_dirR != m_prevDirR) controlRight.SetDirection(m_dirR);
     
-    //controlLeft.SetSpeed(abs(m_speedL));
-    //controlRight.SetSpeed(abs(m_speedR));
-    
-    //m_prevDirL = m_dirL;
-    //m_prevDirR = m_dirR;
-    
-    //printf("leftSpeed : %f, rightSpeed %f", m_speedL, m_speedR);
-    
+    printf("leftspeed is :  %f, rightspeed is :  %f \r", m_speedL, m_speedR);
     
     
 }
\ No newline at end of file