Diff: main.cpp

Revision:

7:7cdb0381e1b8

Parent:

6:ce498700a28c

--- a/main.cpp	Mon Nov 27 21:42:00 2017 +0000
+++ b/main.cpp	Tue Nov 28 19:58:02 2017 +0000
@@ -25,6 +25,11 @@
 float rightError = 0;
 float prevrightError = 0;
 float prevleftError = 0;
+float prevLeftFrontValue= 0;
+float prevRightFrontValue = 0;
+float changeLeftFrontValue = 0;
+float changeRightFrontValue = 0;
+float totalFrontChange = 0;
 float leftFrontError = 0;
 float rightFrontError = 0;
 float totalLeftError = 0;
@@ -36,9 +41,10 @@
 float rightIRBase;
 float leftFrontIRBase;
 float rightFrontIRBase;
-float p =0.42f;
-float i =0.0f;
-float d=0.1f; 
+float p =0.2f;    //.32
+float i =0.0001f;
+float d=0.15f; 
+float frontP = 1.0;
     LeftIR = 1;
             RightIR = 1;
             leftIRBase = LeftReceiver;
@@ -82,6 +88,8 @@
     while(1){       
        float dt = t_time.read();
        float lspeed = 0; float rspeed = 0;
+       prevRightFrontValue = rightFrontValue;
+       prevLeftFrontValue = leftFrontValue;
             LeftIR = 1;
             RightIR = 1;
             wait(.01);
@@ -98,7 +106,7 @@
             FrontRightIR = 0;
            // pc.printf( "%f \n" , leftValue);
             //pc.printf("%f \n", rightValue);
-        if(leftValue> 0.35f && leftFrontValue > 0.16f)
+        if(leftValue> 0.34f && leftFrontValue > 0.15f)
         {
             turn_right();
         }
@@ -116,22 +124,34 @@
        totalLeftError += leftError;
        leftFrontError = leftFrontIRBase - leftFrontValue;
        rightFrontError = rightFrontIRBase - rightFrontValue;
+       changeLeftFrontValue = leftFrontValue-prevLeftFrontValue;
+       changeRightFrontValue = rightFrontValue - prevRightFrontValue;
+       
+       if(changeLeftFrontValue <0){
+           changeLeftFrontValue = 0;
+           }
+           
+        if(changeRightFrontValue <0){
+           changeRightFrontValue = 0;
+           }
+           totalFrontChange = changeRightFrontValue + changeLeftFrontValue;
+           
        
        adjust_l = p*leftError-d*prevleftError-i*totalLeftError ;
        adjust_r = p*rightError-d*prevrightError-i*totalRightError ;
        
        prevleftError = leftError;
        prevrightError = rightError;
-       if(adjust_l >0)
+      // if(adjust_l >0)
        {
-           adjust_l=0;
+        //   adjust_l=0;
        }
-       if(adjust_r >0)
+      // if(adjust_r >0)
        {
-           adjust_r=0;
+        //   adjust_r=0;
        }
-        lspeed = constrain(0.05f, 1.0f, lbase - adjust_l);
-    rspeed = constrain(0.05f, 1.0f, rbase-adjust_r);
+        lspeed = constrain(0.05f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
+    rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
        lpwmb = 0; rpwmb = 0;
       lpwmf = lspeed; rpwmf = rspeed;
        //pc.printf("left %f \n", leftValue);