Diff: controller.cpp

Revision:

23:d6d4e8adf7fe

Parent:

22:1464a3f0a290

--- a/controller.cpp	Tue Jul 01 13:09:06 2014 +0000
+++ b/controller.cpp	Wed Jul 02 13:33:49 2014 +0000
@@ -7,7 +7,7 @@
 PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
  
  //BX tune
-    Kp = 0.0004;
+    Kp = 0.0005;
     Ki = 0.0;
     Kd = 0.0;
     setInputLimits(in_min,in_max);
@@ -120,14 +120,117 @@
  
     
      
-float PID::compute(float center ,  float sp) {
+float PID::compute(int centerL, int centerR ,  int sp) {
     //turn right 122~64   122
     //turn left  64~6     8
-    float C = center;
-    float goal = sp;  // center of black
-    float error = goal - C;//
-    return 0.073+Kp*error;
- 
+       int  errorR = sp - centerR;
+       int  errorL = sp - centerL;
+       int error;
+        if(centerL ==128 && centerR == 0){
+            error = 0;
+        }
+        if (centerL == 128 && centerR != 0){
+            error = -30;
+        }
+        else if (centerL != 128 && centerR == 0){
+            error = 30;
+        }
+        else{
+            if(errorR >= 0 && errorL >=0){
+                if(errorR > errorL){
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else 
+                        error = errorR;
+                }
+                else{
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else
+                        error = errorL;
+                }
+                
+            }
+            else if(errorR < 0 && errorL >=0){
+                if(-errorR > errorL){
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else
+                        error = errorR;
+                }
+                else{
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else
+                        error = errorL;
+                }         
+            }
+             else if(errorR >= 0 && errorL <0){
+                if(errorR > -errorL){
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else
+                        error = errorR;
+                }
+                else{ 
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else 
+                        error = errorL; 
+                }         
+            }
+            else{
+                if(errorR > errorL){
+                    if(errorL > -5 && errorL < 5)
+                        error = 0;
+                    else
+                        error = errorL;
+                }
+                else{
+                    if(errorR > -5 && errorR < 5)
+                        error = 0;
+                    else 
+                        error = errorR;
+                }
+            }
+        }
+        return 0.069 + Kp*error;
+}
+
+int PID::getCenter(int centerL, int centerR) {
+    //turn right 122~64   122
+    //turn left  64~6     8
+    
+    int result = 64;
+    int errorR = 64 - centerR;
+    int errorL = 64 - centerL;
+    int negL = 1 , negR = 1;
+    
+    
+        
+
+        if(centerL == 128 && centerR == 0){// no black line
+
+            result = 64;
+
+        } else if(centerL == 128 && centerR != 0){// no left line
+
+            result = centerR;
+
+
+        } else if(centerL != 128 && centerR == 0){// no right line
+
+            result = centerL;
+
+        } else{// two black lines
+
+            negL = (errorL >= 0) ? 1 : -1;
+            negR = (errorR >= 0) ? 1 : -1;
+            errorL *= negL;
+            errorR *= negR;
+
+            result = (errorL >= errorR) ? centerL : centerR;
+        }
 }
  
 float PID::getInMin() {