Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 27:f111ba194412
- Parent:
- 26:f964408401fa
- Child:
- 28:b7ce1e3bf08b
--- a/Motion.cpp Tue May 15 17:57:59 2018 +0000 +++ b/Motion.cpp Tue May 15 18:34:39 2018 +0000 @@ -10,7 +10,7 @@ const float Motion::KD = 0.0f; const int Motion::MOVE_DIST = 1605; const float Motion::MOVE_SPEED = 50.0f; -const float Motion::SCAN_SPEED = 50.0f; +const float Motion::RUN_SPEED = 70.0f; const float Motion::ROTATE_SPEED = 80.0f; const float Motion::ACCEL_CONST = 3.5f; //2.212f @@ -52,7 +52,8 @@ avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; - accel(MOVE_SPEED); + if (speedRun == true) accel(RUN_SPEED); + else accel(MOVE_SPEED); control(); if ((distanceC) < 40.0f) { @@ -122,7 +123,7 @@ avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; - accel(MOVE_SPEED); + accel(RUN_SPEED); control(); if (distanceC < 101.0f && lastMove == false) { @@ -173,7 +174,7 @@ } - accel(SCAN_SPEED); + accel(MOVE_SPEED); control(); if ((distanceC) < 40.0f) { @@ -269,8 +270,8 @@ countsL = counterLeft.read(); countsR = counterRight.read(); - controller.setDesiredSpeedLeft(17.0f); - controller.setDesiredSpeedRight(-83.0f); + controller.setDesiredSpeedLeft(24.15f); + controller.setDesiredSpeedRight(-115.85f); while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) { @@ -291,8 +292,8 @@ countsL = counterLeft.read(); countsR = counterRight.read(); - controller.setDesiredSpeedLeft(83.0f); - controller.setDesiredSpeedRight(-17.0f); + controller.setDesiredSpeedLeft(115.85f); + controller.setDesiredSpeedRight(-24.15f); while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) { @@ -432,6 +433,9 @@ void Motion::runTask(int path[],int task, bool reverse, int junction) { + if (reverse == true) speedRun = 1; + else speedRun = 0; + switch(path[task]) { case 1: @@ -538,10 +542,14 @@ void Motion::accel(float targetSpeed) { + float fastSpeed; + + 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) { + if (avgSpeed < targetSpeed && deceleration == false && acceleration == false) { actSpeed = ACCEL_CONST * t.read()*60.0f; @@ -549,13 +557,13 @@ actSpeed = 55.0f; - }else if (avgSpeed < targetSpeed*2.8f && acceleration == true) { + }else if (avgSpeed < fastSpeed && acceleration == true) { actSpeed = ACCEL_CONST * t.read()*60.0f; }else if (avgSpeed > targetSpeed+5.0f && deceleration == true) { - actSpeed = targetSpeed*2.8f - ACCEL_CONST * t.read()*60.0f; + actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f; }else if (avgSpeed < targetSpeed && deceleration == true) {