Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 19:6cd6cc5c8b4c
- Parent:
- 18:3309329d5f42
- Child:
- 20:20573f55a5fd
--- a/Motion.cpp Mon May 07 19:52:54 2018 +0000 +++ b/Motion.cpp Thu May 10 18:10:15 2018 +0000 @@ -4,8 +4,8 @@ using namespace std; -const float Motion::LEFT_MID_VAL = 41.73f; //44.73 -const float Motion::RIGHT_MID_VAL = 44.03f; //47.03 +const float Motion::LEFT_MID_VAL = 40.73f; //44.73 +const float Motion::RIGHT_MID_VAL = 43.03f; //47.03 const float Motion::KP = 2.5; const float Motion::KD = 0.0f; const int Motion::MOVE_DIST = 1605; @@ -36,18 +36,7 @@ countsL = counterLeft.read(); countsR = counterRight.read(); - speedLeft = MOVE_SPEED; - speedRight = -MOVE_SPEED; - - //controller.setDesiredSpeedLeft(actSpeed); - //controller.setDesiredSpeedRight(-actSpeed); - avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; - - /*int partytime; - - if (avgSpeed == 0.0f) partytime = 1; - else partytime = 0;*/ t.start(); @@ -63,75 +52,50 @@ avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; - // if (partytime == 1) accel(MOVE_SPEED); control(); - /* - accel(MOVE_SPEED); - controller.setDesiredSpeedLeft(actSpeed); - controller.setDesiredSpeedRight(-actSpeed); - */ if ((distanceC) < 40.0f) { countsLOld = countsL; countsROld = countsR; + while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) { countsL = counterLeft.read(); - countsR = counterRight.read(); + countsR = counterRight.read(); + distanceC = irSensorC.readC(); + if (distanceC > 40.0f) { + stop(); + break; + } } stop(); break; - }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 130.0f) && (distanceC > 40.0f)) { + + }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) { countsLOld += 500; - countsROld += 500; - }/* else if ( distanceL < 80.0f || distanceR < 80.0f ) { - countsLOld = countsL; - countsROld = countsR; - while ((countsL - countsLOld) < MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) { + countsROld += 500; + + }else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) { + countsLOld = counterLeft.read(); + countsROld = counterRight.read(); + + while ((countsL - countsLOld) < MOVE_DIST*0.5f + 816.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 816.0f) { countsL = counterLeft.read(); countsR = counterRight.read(); + accel(MOVE_SPEED); + control(); } stop(); break; - }*/ + } } t.stop(); t.reset(); + lastMove = false; } -/** - * Eine Feldstrecke mit höherer Geschwindigkeit fahren - */ -void Motion::moveFast() { - - countsLOld = counterLeft.read(); - countsROld = counterRight.read(); - countsL = counterLeft.read(); - countsR = counterRight.read(); - - speedLeft = 100.0f; - speedRight = -100.0f; - - controller.setDesiredSpeedLeft(speedLeft); - controller.setDesiredSpeedRight(speedRight); - - while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) { - - countsL = counterLeft.read(); - countsR = counterRight.read(); - distanceC = irSensorC.readC(); - - if (enableMotorDriver == 0) {enableMotorDriver = 1;} - - control(); - - if (distanceC < 40.0f) { - stop(); - break; - } - } -} + /** * Eine Feldstrecke mit überprüfung der Ziellinie fahren */ @@ -145,13 +109,6 @@ countsL = counterLeft.read(); countsR = counterRight.read(); - speedLeft = SCAN_SPEED; - speedRight = -SCAN_SPEED; - - //controller.setDesiredSpeedLeft(actSpeed); - //controller.setDesiredSpeedRight(-actSpeed); - - distanceC = irSensorC.readC(); t.start(); while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) { @@ -166,7 +123,7 @@ enableMotorDriver = 1; } - if (lineSensor.read() == 1.0f) { + if (lineSensor.read() > 0.85f) { line = 1; } @@ -183,18 +140,24 @@ while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) { countsL = counterLeft.read(); countsR = counterRight.read(); + if (distanceC > 40.0f) { + stop(); + break; + } } stop(); break; }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) { countsLOld += 500; countsROld += 500; - }/*else if ( distanceL > 80.0f || distanceR > 80.0f ) { - countsLOld = countsL; - countsROld = countsR; - while ((countsL - countsLOld) < MOVE_DIST*0.5f + 330.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 330.0f) { + }/*else if ( ( distanceL > 80.0f || distanceR > 80.0f ) ) { + countsLOld = counterRight.read(); + countsROld = counterRight.read(); + while ((countsL - countsLOld) < MOVE_DIST*0.5f + 404.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 404.0f) { countsL = counterLeft.read(); - countsR = counterRight.read(); + countsR = counterRight.read(); + accel(SCAN_SPEED); + control(); } stop(); break; @@ -202,6 +165,7 @@ } t.stop(); t.reset(); + lastMove = false; } /** * 90° Rotation nach Links @@ -436,9 +400,11 @@ if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) { deceleration = true; acceleration = false; + lastMove = true; }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) { deceleration = true; acceleration = false; + lastMove = true; }else{ deceleration = false; } @@ -457,11 +423,8 @@ rotateR(); break; case 4: - moveFast(); - break; - case 5: stop(); - break; + break; } } @@ -474,14 +437,12 @@ void Motion::accel(float targetSpeed) { - - avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; - if ( avgSpeed < targetSpeed && deceleration == false) { + if ( avgSpeed < targetSpeed && deceleration == false && acceleration == false) { actSpeed = ACCEL_CONST * t.read()*60.0f; - + }else if (avgSpeed < targetSpeed*2.6f && acceleration == true) { actSpeed = 2.5f * t.read()*60.0f;