Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 23:accd07ca2da7
- Parent:
- 22:91526c8d15ba
- Child:
- 24:11c5fb5280eb
--- a/Motion.cpp Tue May 15 11:19:15 2018 +0000 +++ b/Motion.cpp Tue May 15 13:35:50 2018 +0000 @@ -75,7 +75,7 @@ countsLOld += 500; countsROld += 500; - }else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) { + }/*else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) { countsLOld = counterLeft.read(); countsROld = counterRight.read(); @@ -87,6 +87,50 @@ } stop(); break; + }*/ + + } + + t.stop(); + t.reset(); + lastMove = false; +} + +/** + * Eine Feldstrecke druchführen + */ +void Motion::moveHalf() { + + countsLOld = counterLeft.read(); + countsROld = counterRight.read(); + countsL = counterLeft.read(); + countsR = counterRight.read(); + + avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; + + t.start(); + + while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) { + + countsL = counterLeft.read(); + countsR = counterRight.read(); + distanceC = irSensorC.readC(); + distanceL = irSensorL.readL(); + distanceR = irSensorR.readR(); + + if (enableMotorDriver == 0) {enableMotorDriver = 1;} + + avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; + + accel(MOVE_SPEED); + control(); + + if (distanceC < 101.0f && lastMove == false) { + stop(); + break; + }else if (distanceC < 40.0 && lastMove == true) { + stop(); + break; } } @@ -132,9 +176,6 @@ accel(SCAN_SPEED); control(); - //controller.setDesiredSpeedLeft(actSpeed); - //controller.setDesiredSpeedRight(-actSpeed); - if ((distanceC) < 40.0f) { countsLOld = countsL; countsROld = countsR; @@ -146,24 +187,16 @@ break; } } + stop(); - break; + 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 = 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(); - accel(SCAN_SPEED); - control(); - } - stop(); - break; - }*/ - } + } + } + t.stop(); t.reset(); lastMove = false; @@ -386,6 +419,8 @@ switch(path[task]) { case 1: + + //Acceleration if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) { acceleration = true; @@ -403,7 +438,8 @@ 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) { deceleration = true; @@ -434,8 +470,47 @@ rotateR(); break; case 4: - stop(); - break; + + if (reverse == false && path[task] == 4 && path[task+1] == 1) { + + acceleration = true; + longMove = true; + deceleration = false; + + }else{ + acceleration = false; + deceleration = false; + avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; + } + + if (reverse == false && path[task-1] == 1 && path[task] == 4) { + + deceleration = true; + acceleration = false; + lastMove = true; + longMove = false; + + }else{ + deceleration = false; + } + + if (reverse == false && path[task+1] == 0) { + + lastMove = true; + + } + + + moveHalf(); + break; + case 5: + turnL(); + break; + case 6: + turnR(); + break; + case 7: + break; } }