Helvijs Kiselis
/
Micromouse
Algorithmus
Diff: Motion.cpp
- Revision:
- 5:e2c0a4388d85
- Parent:
- 4:932eb2d29206
- Child:
- 6:4868f789c223
--- a/Motion.cpp Mon Apr 23 12:14:54 2018 +0000 +++ b/Motion.cpp Mon Apr 23 17:59:13 2018 +0000 @@ -7,7 +7,7 @@ const float Motion::LEFT_MID_VAL = 44.73f; const float Motion::RIGHT_MID_VAL = 47.03f; const float Motion::KP = 2.0f; -const float Motion::KD = 0.00f; +const float Motion::KD = 0.0f; Motion::Motion(Controller& controller, EncoderCounter& counterLeft, @@ -26,7 +26,6 @@ */ void Motion::move() { - controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); @@ -38,14 +37,29 @@ controller.setDesiredSpeedLeft(speedLeft); controller.setDesiredSpeedRight(speedRight); - while ((countsL - countsLOld) < 1565 || (countsR - countsROld) > -1565) { + while ((countsL - countsLOld) < 1605 || (countsR - countsROld) > -1605) { countsL = counterLeft.read(); countsR = counterRight.read(); + distanceC = irSensorC.readC(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} - control(); + control(); + + if ((distanceC) < 40.0f) { + countsLOld = countsL; + countsROld = countsR; + while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) { + countsL = counterLeft.read(); + countsR = counterRight.read(); + } + stop(); + break; + }else if ( ((countsL - countsLOld) >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) { + countsLOld += 500; + countsROld += 500; + } } } @@ -54,7 +68,6 @@ */ void Motion::moveFast() { - controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); @@ -70,11 +83,16 @@ countsL = counterLeft.read(); countsR = counterRight.read(); + distanceC = irSensorC.readC(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} control(); + if (distanceC < 40.0f) { + stop(); + break; + } } } /** @@ -82,7 +100,6 @@ */ void Motion::scanMove() { - controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); @@ -94,7 +111,9 @@ controller.setDesiredSpeedLeft(speedLeft); controller.setDesiredSpeedRight(speedRight); - while ((countsL - countsLOld) < 1565 || (countsR - countsROld) > -1565) { + distanceC = irSensorC.readC(); + + while ((countsL - countsLOld) < 1605 || (countsR - countsROld) > -1605) { countsL = counterLeft.read(); countsR = counterRight.read(); @@ -108,15 +127,22 @@ line = 1; } - control(); + control(); - if (distanceC < 40.0f) { + if ((distanceC) < 40.0f) { + countsLOld = countsL; + countsROld = countsR; + while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) { + countsL = counterLeft.read(); + countsR = counterRight.read(); + } stop(); break; + }else if ( ((countsL - countsLOld) >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) { + countsLOld += 500; + countsROld += 500; } - - } - + } } /** * 90° Rotation nach Links @@ -264,8 +290,9 @@ distanceL = irSensorL.readL(); distanceR = irSensorR.readR(); + if (distanceL < wallLeft && distanceR < wallRight) { - /*if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) { + /* if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) { errorP = LEFT_MID_VAL - distanceL; @@ -275,19 +302,25 @@ }*/ errorP = distanceL - distanceR + 2.30f; + //errorD = errorP - oldErrorP; }else{ - errorP = 0; + errorP = 0; + errorD = 0; } - errorD = errorP - oldErrorP; + //errorD = errorP - oldErrorP; oldErrorP = errorP; - totalError = KP*errorP + KD*errorD; + if (abs(errorP) < 80.0f) { + totalError = KP*errorP + KD*errorD; + }else{ + totalError = 0; + } - controller.setDesiredSpeedLeft(speedLeft + totalError); - controller.setDesiredSpeedRight(speedRight + totalError); + controller.setDesiredSpeedLeft(speedLeft - totalError); + controller.setDesiredSpeedRight(speedRight - totalError); } void Motion::runTask(int task) {