Alexander Lüthard
/
Micromouse
Alle Prinf's auskommentiert
Fork of Micromouse by
Diff: Motion.cpp
- Revision:
- 13:845e49f20426
- Parent:
- 12:75d0291a9785
- Child:
- 14:2926932e26df
--- a/Motion.cpp Wed May 02 14:58:48 2018 +0000 +++ b/Motion.cpp Fri May 04 19:10:36 2018 +0000 @@ -9,9 +9,10 @@ const float Motion::KP = 2.5; const float Motion::KD = 0.0f; const int Motion::MOVE_DIST = 1605; -const float Motion::MOVE_SPEED = 100.0f; -const float Motion::ROTATE_SPEED = 20.0f; -const float Motion::ACCEL_SCALE = 0.5f; +const float Motion::MOVE_SPEED = 130.0f; +const float Motion::SCAN_SPEED = 50.0f; +const float Motion::ROTATE_SPEED = 80.0f; +const float Motion::ACCEL_CONST = 2.212f; Motion::Motion(Controller& controller, EncoderCounter& counterLeft, @@ -41,6 +42,8 @@ controller.setDesiredSpeedLeft(MOVE_SPEED); controller.setDesiredSpeedRight(-MOVE_SPEED); + t.start(); + while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) { countsL = counterLeft.read(); @@ -53,7 +56,7 @@ control(); /* - accel(); + accel(MOVE_SPEED); controller.setDesiredSpeedLeft(actSpeed); controller.setDesiredSpeedRight(-actSpeed); */ @@ -82,6 +85,9 @@ }*/ } + + t.stop(); + t.reset(); } /** * Eine Feldstrecke mit höherer Geschwindigkeit fahren @@ -125,13 +131,15 @@ countsL = counterLeft.read(); countsR = counterRight.read(); - speedLeft = MOVE_SPEED; - speedRight = -MOVE_SPEED; + speedLeft = SCAN_SPEED; + speedRight = -SCAN_SPEED; - controller.setDesiredSpeedLeft(MOVE_SPEED); - controller.setDesiredSpeedRight(-MOVE_SPEED); + controller.setDesiredSpeedLeft(actSpeed); + controller.setDesiredSpeedRight(-actSpeed); + acceleration = true; distanceC = irSensorC.readC(); + t.start(); while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) { @@ -149,7 +157,12 @@ line = 1; } - control(); + + control(); + accel(SCAN_SPEED); + + //controller.setDesiredSpeedLeft(actSpeed); + //controller.setDesiredSpeedRight(-actSpeed); if ((distanceC) < 40.0f) { countsLOld = countsL; @@ -174,6 +187,9 @@ break; }*/ } + t.stop(); + t.reset(); + acceleration = false; } /** * 90° Rotation nach Links @@ -190,7 +206,7 @@ controller.setDesiredSpeedLeft(-ROTATE_SPEED); controller.setDesiredSpeedRight(-ROTATE_SPEED); - while ((countsL - countsLOld) > -862 || (countsR - countsROld) > -862) { + while ((countsL - countsLOld) > -870 || (countsR - countsROld) > -870) { //accel(); @@ -219,7 +235,7 @@ controller.setDesiredSpeedLeft(ROTATE_SPEED); controller.setDesiredSpeedRight(ROTATE_SPEED); - while ((countsL - countsLOld) < 862 || (countsR - countsROld) < 862) { + while ((countsL - countsLOld) < 870 || (countsR - countsROld) < 870) { //accel(); @@ -296,7 +312,7 @@ * 180° Rotation */ void Motion::rotate180() { - + //1723 stop(); controller.counterReset(); countsLOld = counterLeft.read(); @@ -304,12 +320,17 @@ countsL = counterLeft.read(); countsR = counterRight.read(); - controller.setDesiredSpeedLeft(-ROTATE_SPEED); - controller.setDesiredSpeedRight(-ROTATE_SPEED); + //controller.setDesiredSpeedLeft(-ROTATE_SPEED); + //controller.setDesiredSpeedRight(-ROTATE_SPEED); + + t.start(); - while ((countsL - countsLOld) > -1723 || (countsR - countsROld) > -1723) { + while ((countsL - countsLOld) > -900 || (countsR - countsROld) > -900) { - //accel(); + actSpeed = 3.5f * t.read()*60.0f; + + controller.setDesiredSpeedLeft(-actSpeed); + controller.setDesiredSpeedRight(-actSpeed); countsL = counterLeft.read(); countsR = counterRight.read(); @@ -317,6 +338,24 @@ if (enableMotorDriver == 0) {enableMotorDriver = 1;} } + t.reset(); + + avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; + + while ((countsL - countsLOld) > -1720 || (countsR - countsROld) > -1720) { + + actSpeed = avgSpeed + (-3.5f * t.read()*60.0f); + + controller.setDesiredSpeedLeft(-actSpeed); + controller.setDesiredSpeedRight(-actSpeed); + + countsL = counterLeft.read(); + countsR = counterRight.read(); + + if (enableMotorDriver == 0) {enableMotorDriver = 1;} + } + t.stop(); + t.reset(); stop(); } @@ -358,8 +397,8 @@ totalError = 0; } - controller.setDesiredSpeedLeft(speedLeft - totalError); - controller.setDesiredSpeedRight(speedRight - totalError); + controller.setDesiredSpeedLeft(actSpeed - totalError); + controller.setDesiredSpeedRight(-actSpeed - totalError); } void Motion::runTask(int path[],int task, bool reverse) { @@ -413,19 +452,21 @@ } -void Motion::accel() { +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 < MOVE_SPEED && acceleration == true) { + if ( avgSpeed < targetSpeed && acceleration == true) { - actSpeed = avgCounts * ACCEL_SCALE; + actSpeed = ACCEL_CONST * t.read()*60.0f; } - if ( MOVE_DIST - avgCounts < MOVE_SPEED*2 && deceleration == true) { + if ( MOVE_DIST - avgCounts < targetSpeed*2 && deceleration == true) { - actSpeed = -ACCEL_SCALE * (avgCounts - (MOVE_DIST - MOVE_SPEED*2)) + MOVE_SPEED; + actSpeed = -ACCEL_CONST * (avgCounts - (MOVE_DIST - targetSpeed*2)) + targetSpeed; }