Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 28:b7ce1e3bf08b
- Parent:
- 27:f111ba194412
- Child:
- 29:0419f4873807
- Child:
- 30:bdb8c92434a0
diff -r f111ba194412 -r b7ce1e3bf08b Motion.cpp --- a/Motion.cpp Tue May 15 18:34:39 2018 +0000 +++ b/Motion.cpp Thu May 17 16:39:03 2018 +0000 @@ -56,7 +56,7 @@ else accel(MOVE_SPEED); control(); - if ((distanceC) < 40.0f) { + if ((distanceC) < 40.0f && speedRun == false) { countsLOld = countsL; countsROld = countsR; @@ -72,6 +72,9 @@ stop(); break; + }else if (distanceC < 101.0f && speedRun == true) { + stop(); + break; }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) { countsLOld += 500; countsROld += 500; @@ -127,7 +130,6 @@ control(); if (distanceC < 101.0f && lastMove == false) { - stop(); break; }else if (distanceC < 40.0f && lastMove == true) { stop(); @@ -273,7 +275,16 @@ controller.setDesiredSpeedLeft(24.15f); controller.setDesiredSpeedRight(-115.85f); - while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) { + /* Velocity Settings: + 50rpm -> 17.25 : 82.75 + 60rpm -> 20.7 : 99.3 + 70rpm -> 24.15 : 115.85 + 80rpm -> 27.6 : 132.4 + 90rpm -> 31.05 : 148.95 + 100rpm -> 34.5 : 165.5 + */ + + while ((countsL - countsLOld) < 446 || (countsR - countsROld) > -2141) { countsL = counterLeft.read(); countsR = counterRight.read(); @@ -292,10 +303,10 @@ countsL = counterLeft.read(); countsR = counterRight.read(); - controller.setDesiredSpeedLeft(115.85f); + controller.setDesiredSpeedLeft(115.85f); controller.setDesiredSpeedRight(-24.15f); - while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) { + while ((countsL - countsLOld) < 2141 || (countsR - countsROld) > -446) { countsL = counterLeft.read(); countsR = counterRight.read(); @@ -433,8 +444,8 @@ void Motion::runTask(int path[],int task, bool reverse, int junction) { - if (reverse == true) speedRun = 1; - else speedRun = 0; + if (reverse == false) speedRun = true; + else speedRun = false; switch(path[task]) { @@ -467,7 +478,7 @@ lastMove = true; longMove = false; - }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) { + }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) { deceleration = true; acceleration = false; @@ -517,7 +528,6 @@ if (reverse == false && path[task+1] == 0) { lastMove = true; - } @@ -544,30 +554,35 @@ float fastSpeed; + //Acclerated Target Speed if (speedRun == true) fastSpeed = targetSpeed*2.0f; else fastSpeed = targetSpeed*2.8f; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; - - if (avgSpeed < targetSpeed && deceleration == false && acceleration == false) { + + //Acceleration logic + + //Short distance + if (avgSpeed < targetSpeed && deceleration == false && acceleration == false && longMove == false) { - actSpeed = ACCEL_CONST * t.read()*60.0f; + actSpeed = ACCEL_CONST * t.read()*60.0f; //Acceleration equation }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) { - actSpeed = 55.0f; + actSpeed = targetSpeed+5.0f; //Keep Speed steady in case of overshooting - }else if (avgSpeed < fastSpeed && acceleration == true) { + //Long distance + }else if (avgSpeed < fastSpeed && acceleration == true && deceleration == false) { - actSpeed = ACCEL_CONST * t.read()*60.0f; + actSpeed = ACCEL_CONST * t.read()*60.0f; - }else if (avgSpeed > targetSpeed+5.0f && deceleration == true) { + }else if (avgSpeed > targetSpeed+5.0f && acceleration == false && deceleration == true) { - actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f; + actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f; //Deceleration equation - }else if (avgSpeed < targetSpeed && deceleration == true) { + }else if (avgSpeed < targetSpeed && acceleration == false && deceleration == true) { - actSpeed = 51.0f; + actSpeed = targetSpeed+5.0f; //Limit deceleration in case of overshooting } }