Anon Anon
/
MicroMousewithFloodFill
Updated with the Algorithm
Fork of MM_rat_Assignment4-newwest by
Diff: main.cpp
- Revision:
- 7:7cdb0381e1b8
- Parent:
- 6:ce498700a28c
- Child:
- 8:22e399fe87a4
--- 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);