Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 3:076dd7ec7eb4
- Parent:
- 2:f898adf2d817
- Child:
- 4:932eb2d29206
diff -r f898adf2d817 -r 076dd7ec7eb4 Motion.cpp --- a/Motion.cpp Thu Apr 19 11:26:51 2018 +0000 +++ b/Motion.cpp Fri Apr 20 13:37:21 2018 +0000 @@ -4,9 +4,9 @@ using namespace std; -const float Motion::LEFT_MID_VAL = 45.0f; -const float Motion::RIGHT_MID_VAL = 45.0f; -const float Motion::KP = 2.5f; +const float Motion::LEFT_MID_VAL = 44.73f; +const float Motion::RIGHT_MID_VAL = 47.03f; +const float Motion::KP = 3.0f; const float Motion::KD = 0.00f; @@ -98,12 +98,13 @@ countsL = counterLeft.read(); countsR = counterRight.read(); + distanceC = irSensorC.readC(); if (enableMotorDriver == 0) { enableMotorDriver = 1; } - if (lineSensor.read() > 0.9f) { + if (lineSensor.read() == 1.0f) { line = 1; }else{ line = 0; @@ -111,6 +112,11 @@ control(); + if (distanceC < 40.0f) { + stop(); + break; + } + } } @@ -168,25 +174,7 @@ /** * Links abbiegen */ -void Motion::turnL() { - - - controller.counterReset(); - countsLOld = counterLeft.read(); - countsROld = counterRight.read(); - countsL = counterLeft.read(); - countsR = counterRight.read(); - - controller.setDesiredSpeedLeft(50.0f); - controller.setDesiredSpeedRight(-50.0f); - - while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) { - - countsL = counterLeft.read(); - countsR = counterRight.read(); - - if (enableMotorDriver == 0) {enableMotorDriver = 1;} - } +void Motion::turnL() { controller.counterReset(); countsLOld = counterLeft.read(); @@ -203,47 +191,12 @@ countsR = counterRight.read(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} - } - - controller.counterReset(); - countsLOld = counterLeft.read(); - countsROld = counterRight.read(); - countsL = counterLeft.read(); - countsR = counterRight.read(); - - controller.setDesiredSpeedLeft(50.0f); - controller.setDesiredSpeedRight(-50.0f); - - while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) { - - countsL = counterLeft.read(); - countsR = counterRight.read(); - - if (enableMotorDriver == 0) {enableMotorDriver = 1;} } } /** * Rechts abbiegen */ -void Motion::turnR() { - - - controller.counterReset(); - countsLOld = counterLeft.read(); - countsROld = counterRight.read(); - countsL = counterLeft.read(); - countsR = counterRight.read(); - - controller.setDesiredSpeedLeft(50.0f); - controller.setDesiredSpeedRight(-50.0f); - - while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) { - - countsL = counterLeft.read(); - countsR = counterRight.read(); - - if (enableMotorDriver == 0) {enableMotorDriver = 1;} - } +void Motion::turnR() { controller.counterReset(); countsLOld = counterLeft.read(); @@ -260,24 +213,7 @@ countsR = counterRight.read(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} - } - - controller.counterReset(); - countsLOld = counterLeft.read(); - countsROld = counterRight.read(); - countsL = counterLeft.read(); - countsR = counterRight.read(); - - controller.setDesiredSpeedLeft(50.0f); - controller.setDesiredSpeedRight(-50.0f); - - while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) { - - countsL = counterLeft.read(); - countsR = counterRight.read(); - - if (enableMotorDriver == 0) {enableMotorDriver = 1;} - } + } } /** * Motor Stop