Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 17:8a8758bfe3c5
- Parent:
- 16:c5b864804632
- Child:
- 18:3309329d5f42
--- a/Motion.cpp Mon May 07 14:03:36 2018 +0000 +++ b/Motion.cpp Mon May 07 18:28:34 2018 +0000 @@ -12,7 +12,7 @@ const float Motion::MOVE_SPEED = 50.0f; const float Motion::SCAN_SPEED = 50.0f; const float Motion::ROTATE_SPEED = 80.0f; -const float Motion::ACCEL_CONST = 2.212f; // +const float Motion::ACCEL_CONST = 2.5f; //2.212f Motion::Motion(Controller& controller, EncoderCounter& counterLeft, @@ -138,6 +138,7 @@ void Motion::scanMove() { acceleration = false; + deceleration = false; countsLOld = counterLeft.read(); countsROld = counterRight.read(); @@ -420,33 +421,33 @@ switch(path[task]) { case 1: - if (reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) { + if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) { + acceleration = true; + deceleration = false; + }else if (reverse == false && path[task+1] == path[task] && path[task-1] != path[task]) { acceleration = true; deceleration = false; }else{ - acceleration = false; - } - - if (reverse == false && path[task+1] == path[task] && path[task-1] != path[task]) { - acceleration = true; + acceleration = false; deceleration = false; - }else{ - acceleration = false; + avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; } - if (reverse == true && path[task-1] != path[task]) { + if (reverse == true && path[task-1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) { + deceleration = true; + acceleration = false; + }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) { deceleration = true; acceleration = false; }else{ deceleration = false; } - if (reverse == false && path[task+1] != path[task]) { - deceleration = true; - acceleration = false; - }else{ - deceleration = false; - } + //printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration); + //printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed); + + + move(); break; case 2: @@ -477,7 +478,7 @@ avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; - if ( avgSpeed < targetSpeed) { + if ( avgSpeed < targetSpeed && deceleration == false) { actSpeed = ACCEL_CONST * t.read()*60.0f;