Evan Brown
/
MM_rat_Assignment4-newwest
p
Fork of MM_rat_Assignment4 by
Revision 7:7cdb0381e1b8, committed 2017-11-28
- Comitter:
- evenbrownie
- Date:
- Tue Nov 28 19:58:02 2017 +0000
- Parent:
- 6:ce498700a28c
- Commit message:
- PID with front stopping capability
Changed in this revision
header.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/header.h Mon Nov 27 21:42:00 2017 +0000 +++ b/header.h Tue Nov 28 19:58:02 2017 +0000 @@ -29,8 +29,8 @@ Serial pc(SERIAL_TX, SERIAL_RX); QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING); QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING); -const float rbase = 0.09f; -const float lbase = 0.085f; +const float rbase = 0.15f; //.09 +const float lbase = 0.163f; //.1 const float WALL_IR_L = 0.74f; const float WALL_IR_R = 0.74f; const float WALL_IR_FL = 0.74f;
--- 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);