Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 30:bdb8c92434a0
- Parent:
- 28:b7ce1e3bf08b
- Child:
- 31:2c54f8304ef5
diff -r b7ce1e3bf08b -r bdb8c92434a0 Motion.cpp --- a/Motion.cpp Thu May 17 16:39:03 2018 +0000 +++ b/Motion.cpp Tue May 22 16:40:36 2018 +0000 @@ -8,11 +8,11 @@ const float Motion::RIGHT_MID_VAL = 43.03f; //47.03 const float Motion::KP = 2.0; const float Motion::KD = 0.0f; -const int Motion::MOVE_DIST = 1605; +const int Motion::MOVE_DIST = 1620; const float Motion::MOVE_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 +const float Motion::ACCEL_CONST = 4.0f; //2.212f Motion::Motion(Controller& controller, EncoderCounter& counterLeft, @@ -56,6 +56,7 @@ else accel(MOVE_SPEED); control(); + //Stop at certainn distance before wall if ((distanceC) < 40.0f && speedRun == false) { countsLOld = countsL; countsROld = countsR; @@ -64,6 +65,11 @@ countsL = counterLeft.read(); countsR = counterRight.read(); distanceC = irSensorC.readC(); + + if (speedRun == true) accel(RUN_SPEED); + else accel(MOVE_SPEED); + control(); + if (distanceC > 40.0f) { stop(); break; @@ -71,27 +77,33 @@ } stop(); break; - - }else if (distanceC < 101.0f && speedRun == true) { + + //Stop at certain distance in speed run + }else if (distanceC < 160.0f && speedRun == true && lastMove == true) { stop(); break; + + //Correct distance from wall }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) { countsLOld += 500; - countsROld += 500; + countsROld -= 500; - }/*else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) { + //Stop after certain distance if side wall and front wall missing + }else if ( deceleration == true && speedRun == false && ( distanceL > 60.0f || distanceR > 60.0f ) && distanceC > 200.0f ) { countsLOld = counterLeft.read(); countsROld = counterRight.read(); - while ((countsL - countsLOld) < MOVE_DIST*0.5f + 816.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 816.0f) { + while ((countsL - countsLOld) < MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) { countsL = counterLeft.read(); countsR = counterRight.read(); - accel(MOVE_SPEED); + + if (speedRun == true) accel(RUN_SPEED); + else accel(MOVE_SPEED); control(); } stop(); break; - }*/ + } } @@ -129,8 +141,25 @@ accel(RUN_SPEED); control(); - if (distanceC < 101.0f && lastMove == false) { + //Stop ceratin distance before wall + if (distanceC < 100.0f && lastMove == false) { break; + + //Stop after certain distance if side wall missing + }else if ( (distanceL > 60.0f || distanceR > 60.0f) && lastMove == false && deceleration == true && distanceC < 190.0f) { + countsLOld = counterLeft.read(); + countsROld = counterRight.read(); + + while ((countsL - countsLOld) < 600.0f || (countsR - countsROld) > -600.0f) { + countsL = counterLeft.read(); + countsR = counterRight.read(); + accel(RUN_SPEED); + control(); + } + stop(); + break; + + //Stop before wall at target field }else if (distanceC < 40.0f && lastMove == true) { stop(); break; @@ -406,7 +435,9 @@ errorP = RIGHT_MID_VAL - distanceR; - }*/ if (distanceL < wallLeft && distanceR > wallRight) { + }*/ + + if (distanceL < wallLeft && distanceR > wallRight) { errorP = distanceL - wallLeft; @@ -443,7 +474,8 @@ } void Motion::runTask(int path[],int task, bool reverse, int junction) { - + + //reverse happens only in search run if (reverse == false) speedRun = true; else speedRun = false; @@ -452,13 +484,13 @@ case 1: //Acceleration - if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) { + if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) { //if next move() and previous no move() and step no junction acceleration = true; longMove = true; deceleration = false; - }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) { + }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0) ) { //same as above, also if start field acceleration = true; longMove = true; @@ -466,25 +498,28 @@ }else{ acceleration = false; - deceleration = false; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; } //Deceleration - if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) { + if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) { //next step no move() or junction and speed above 120rpm deceleration = true; acceleration = false; lastMove = true; longMove = false; - }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) { + }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) { //next step no move() and no moveHalf() and speed above 120rpm deceleration = true; acceleration = false; lastMove = true; longMove = false; + }else if (reverse == false && path[task+1] != path[task] && path[task+1] == 4 && avgSpeed > 2.4f*MOVE_SPEED) { + + lastMove = true; + }else{ deceleration = false; } @@ -502,7 +537,7 @@ break; case 4: - if (reverse == false && path[task] == 4 && path[task+1] == 1) { + if (reverse == false && path[task] == 4 && path[task+1] == 1) { //accelerate if next step is move() acceleration = true; longMove = true; @@ -510,15 +545,13 @@ }else{ acceleration = false; - deceleration = false; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; } - if (reverse == false && path[task-1] == 1 && path[task] == 4) { + if (reverse == false && path[task-1] == 1 && path[task] == 4 && path[task+1] != 0) { //decelerate if previous step was move() deceleration = true; acceleration = false; - lastMove = true; longMove = false; }else{ @@ -530,7 +563,6 @@ lastMove = true; } - moveHalf(); break; case 5: