Diff: main.cpp

Revision:

3:4e34437daef4

Parent:

2:7da65637e512

Child:

4:c545d63ee454

--- a/main.cpp	Wed Nov 22 01:40:27 2017 +0000
+++ b/main.cpp	Mon Nov 27 17:33:34 2017 +0000
@@ -23,8 +23,12 @@
 float rightFrontValue;
 float leftError = 0;
 float rightError = 0;
+float prevrightError = 0;
+float prevleftError = 0;
 float leftFrontError = 0;
 float rightFrontError = 0;
+float totalLeftError = 0;
+float totalRightError =0;
 float adjust_l = 0;
 float adjust_r = 0;
 
@@ -32,9 +36,9 @@
 float rightIRBase;
 float leftFrontIRBase;
 float rightFrontIRBase;
-float p =0.45f;
+float p =0.5f;
 float i;
-float d; 
+float d=0.1f; 
     LeftIR = 1;
             RightIR = 1;
             leftIRBase = LeftReceiver;
@@ -92,20 +96,52 @@
             wait(.01);
             FrontLeftIR = 0;
             FrontRightIR = 0;
+           // pc.printf( "%f \n" , leftValue);
+            //pc.printf("%f \n", rightValue);
+        if(leftValue> 0.4f && leftFrontValue > 0.24f)
+        {
+            turn_right();
+        }
+        else if(rightFrontValue > 0.22f && rightValue > 0.4f)
+        {
+            turn_left();
+        }
+            
             
             
        //ProcessIR(dt, ir_lman, lspeed, rspeed);
        leftError = leftIRBase - leftValue;
        rightError = rightIRBase - rightValue;
+       totalRightError += rightError;
+       totalLeftError += leftError;
        leftFrontError = leftFrontIRBase - leftFrontValue;
        rightFrontError = rightFrontIRBase - rightFrontValue;
        
-       adjust_l = p*leftError ;
-       adjust_r = p*rightError ;
+       adjust_l = p*leftError-d*prevleftError ;
+       adjust_r = p*rightError-d*prevrightError ;
+       
+       prevleftError = leftError;
+       prevrightError = rightError;
+       if(adjust_l >0)
+       {
+           adjust_l=0;
+       }
+       if(adjust_r >0)
+       {
+           adjust_r=0;
+       }
         lspeed = constrain(0.05f, 1.0f, lbase - adjust_l);
     rspeed = constrain(0.05f, 1.0f, rbase-adjust_r);
        lpwmb = 0; rpwmb = 0;
-       lpwmf = lspeed; rpwmf = rspeed;
+      lpwmf = lspeed; rpwmf = rspeed;
+       //pc.printf("left %f \n", leftValue);
+       //pc.printf("right %f \n\n", rightValue);
+       //pc.printf("front left%f \n", leftFrontValue);
+       //pc.printf("front right%f \n", rightFrontValue);
+       //pc.printf(" \n");
        t_time.reset();
     }
 }
+
+
+