Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 22:91526c8d15ba
- Parent:
- 21:41997651337a
- Child:
- 23:accd07ca2da7
--- a/Motion.cpp Sat May 12 16:30:18 2018 +0000 +++ b/Motion.cpp Tue May 15 11:19:15 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.5f; //2.212f +const float Motion::ACCEL_CONST = 3.5f; //2.212f Motion::Motion(Controller& controller, EncoderCounter& counterLeft, @@ -103,6 +103,7 @@ acceleration = false; deceleration = false; + longMove = false; countsLOld = counterLeft.read(); countsROld = counterRight.read(); @@ -386,13 +387,17 @@ case 1: if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) { + acceleration = true; longMove = true; deceleration = false; + }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) { + acceleration = true; longMove = true; deceleration = false; + }else{ acceleration = false; deceleration = false; @@ -400,15 +405,19 @@ } if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) { + deceleration = true; acceleration = false; lastMove = true; longMove = false; + }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) { + deceleration = true; acceleration = false; lastMove = true; longMove = false; + }else{ deceleration = false; } @@ -416,8 +425,6 @@ //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: @@ -451,13 +458,13 @@ actSpeed = 55.0f; - }else if (avgSpeed < targetSpeed*2.6f && acceleration == true) { + }else if (avgSpeed < targetSpeed*2.8f && acceleration == true) { - actSpeed = 2.5f * t.read()*60.0f; + actSpeed = ACCEL_CONST * t.read()*60.0f; - }else if ( avgSpeed > targetSpeed && deceleration == true) { + }else if (avgSpeed > targetSpeed && deceleration == true) { - actSpeed = targetSpeed*2.6f - ACCEL_CONST * t.read()*60.0f; - } + actSpeed = targetSpeed*2.8f - ACCEL_CONST * t.read()*60.0f; + } }