Helvijs Kiselis
/
Micromouse
Algorithmus
Motion.cpp
- Committer:
- Helvis
- Date:
- 2018-05-23
- Revision:
- 33:836ab2089565
- Parent:
- 32:e984b7959cb0
- Child:
- 34:0587c0943263
File content as of revision 33:836ab2089565:
#include <cmath> #include "Motion.h" using namespace std; 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.0; const float Motion::KD = 0.0f; const int Motion::MOVE_DIST = 1620; const float Motion::MOVE_SPEED = 50.0f; const float Motion::RUN_SPEED = 70.0f; const float Motion::ROTATE_SPEED = 80.0f; const float Motion::ACCEL_CONST = 4.0f; //2.212f Motion::Motion(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, IRSensor& irSensorL, IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor, DigitalOut& enableMotorDriver) : controller(controller), counterLeft(counterLeft), counterRight(counterRight), irSensorL(irSensorL), irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor), enableMotorDriver(enableMotorDriver) {} Motion::~Motion() {} /** * Eine Feldstrecke druchführen */ void Motion::move() { 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) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) { 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; if (speedRun == true) accel(RUN_SPEED); else accel(MOVE_SPEED); control(); //Stop at certainn distance before wall if ((distanceC) < 40.0f && speedRun == false) { countsLOld = countsL; countsROld = countsR; while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) { countsL = counterLeft.read(); countsR = counterRight.read(); distanceC = irSensorC.readC(); if (speedRun == true) accel(RUN_SPEED); else accel(MOVE_SPEED); control(); if (distanceC > 40.0f) { stop(); break; } } stop(); break; //Stop at certain distance in speed run }else if (distanceC < 160.0f && speedRun == true && lastMove == true) { stop(); break; //Correct distance from wall }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) { countsLOld += 1000; countsROld -= 1000; //Stop after certain distance if side wall and front wall missing }else if ( deceleration == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 200.0f ) { controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); while ((countsL - countsLOld) < MOVE_DIST*0.5f + 400.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 400.0f) { countsL = counterLeft.read(); countsR = counterRight.read(); deceleration = true; acceleration = false; accel(MOVE_SPEED); control(); } stop(); break; } } t.stop(); t.reset(); lastMove = false; acceleration = false; deceleration = 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(RUN_SPEED); control(); //Stop ceratin distance before wall if (distanceC < 100.0f && lastMove == false) { break; }else if ( ((countsL - countsLOld) >= 824 || (countsR - countsROld) <= -824) && (distanceC >= 100.0f) && (distanceC < 120.0f) ) { countsLOld += 100; countsROld -= 100; //Stop after certain distance if side wall missing }else if ( (distanceL > 80.0f || distanceR > 80.0f) && lastMove == false && deceleration == true && distanceC < 190.0f) { controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); while ((countsL - countsLOld) < 250.0f || (countsR - countsROld) > -250.0f) { countsL = counterLeft.read(); countsR = counterRight.read(); accel(RUN_SPEED); control(); } stop(); break; //Stop before wall at target field }else if (distanceC < 40.0f && lastMove == true) { stop(); break; } } t.stop(); t.reset(); lastMove = false; acceleration = false; deceleration = false; } /** * Eine Feldstrecke mit überprüfung der Ziellinie fahren */ void Motion::scanMove() { acceleration = false; deceleration = false; longMove = false; countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); bool sideWalls = false; t.start(); while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) { countsL = counterLeft.read(); countsR = counterRight.read(); distanceC = irSensorC.readC(); distanceL = irSensorL.readL(); distanceR = irSensorR.readR(); //for distance correcture with side wall if (distanceL < 80.0f && distanceR < 80.0f) sideWalls = true; if (enableMotorDriver == 0) { enableMotorDriver = 1; } if (lineSensor.read() > 0.85f) { line = 1; } accel(MOVE_SPEED); control(); if ((distanceC) < 40.0f) { countsLOld = countsL; countsROld = countsR; 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; //Stop after certain distance if side wall and front wall missing }else if (sideWalls == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 200.0f ) { countsLOld = counterLeft.read(); countsROld = counterRight.read(); while ((countsL - countsLOld) < MOVE_DIST*0.5f + 320.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 320.0f) { countsL = counterLeft.read(); countsR = counterRight.read(); if (speedRun == true) accel(RUN_SPEED); else accel(MOVE_SPEED); control(); } break; } } t.stop(); t.reset(); lastMove = false; } /** * 90° Rotation nach Links */ void Motion::rotateL() { stop(); controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(-ROTATE_SPEED); controller.setDesiredSpeedRight(-ROTATE_SPEED); while ((countsL - countsLOld) > -870 || (countsR - countsROld) > -870) { //accel(); countsL = counterLeft.read(); countsR = counterRight.read(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} } stop(); } /** * 90° Rotation nach Rechts */ void Motion::rotateR() { stop(); controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(ROTATE_SPEED); controller.setDesiredSpeedRight(ROTATE_SPEED); while ((countsL - countsLOld) < 870 || (countsR - countsROld) < 870) { //accel(); countsL = counterLeft.read(); countsR = counterRight.read(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} } stop(); } /** * Links abbiegen */ void Motion::turnL() { controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(24.15f); controller.setDesiredSpeedRight(-115.85f); /* Velocity Settings: 50rpm -> 17.25 : 82.75 60rpm -> 20.7 : 99.3 70rpm -> 24.15 : 115.85 80rpm -> 27.6 : 132.4 90rpm -> 31.05 : 148.95 100rpm -> 34.5 : 165.5 */ while ((countsL - countsLOld) < 446 || (countsR - countsROld) > -2141) { 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(115.85f); controller.setDesiredSpeedRight(-24.15f); while ((countsL - countsLOld) < 2141 || (countsR - countsROld) > -446) { countsL = counterLeft.read(); countsR = counterRight.read(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} } } /** * Motor Stop */ void Motion::stop() { controller.setDesiredSpeedLeft(0.0f); controller.setDesiredSpeedRight(0.0f); actSpeed = 0.0f; float sL = controller.getSpeedLeft(); float ticks = 0.08f*sL; waitStop = 0; while( waitStop < ticks) { controller.setDesiredSpeedLeft(0.0f); controller.setDesiredSpeedRight(0.0f); waitStop+= 1; } } /** * 180° Rotation */ void Motion::rotate180() { //1723 stop(); controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); //controller.setDesiredSpeedLeft(-ROTATE_SPEED); //controller.setDesiredSpeedRight(-ROTATE_SPEED); t.start(); while ((countsL - countsLOld) > -900 || (countsR - countsROld) > -900) { actSpeed = 3.5f * t.read()*60.0f; controller.setDesiredSpeedLeft(-actSpeed); controller.setDesiredSpeedRight(-actSpeed); countsL = counterLeft.read(); countsR = counterRight.read(); 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(); } void Motion::control() { float wallLeft = 47.0f; //48.73 float wallRight = 44.0f; //51.03f; distanceL = irSensorL.readL(); distanceR = irSensorR.readR(); /*if (distanceL < wallLeft-1.0f && distanceR < wallRight-1.0f) { errorP = distanceL - distanceR - 3.0f; }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) { errorP = distanceL - LEFT_MID_VAL; }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) { errorP = RIGHT_MID_VAL - distanceR; }*/ if (distanceL < wallLeft && distanceR > wallRight) { errorP = distanceL - wallLeft; }else if (distanceL > wallLeft && distanceR < wallRight) { errorP = wallRight - distanceR; }else if (distanceL < wallLeft+10.0f && distanceL > wallLeft && distanceR > wallRight) { errorP = distanceL - wallLeft; }else if (distanceR < wallRight+10.0f && distanceL > wallLeft && distanceR > wallRight) { errorP = wallRight - distanceR; }else{ errorP = 0; errorD = 0; } errorD = errorP - oldErrorP; oldErrorP = errorP; if (abs(errorP) < 80.0f) { totalError = KP*errorP + KD*errorD; }/*else{ totalError = 0; }*/ controller.setDesiredSpeedLeft(actSpeed - totalError); controller.setDesiredSpeedRight(-actSpeed - totalError); } void Motion::runTask(int path[],int task, bool reverse, int junction) { //reverse happens only in search run if (reverse == false) speedRun = true; else speedRun = false; switch(path[task]) { case 1: //Acceleration if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction && task-1 != junction) { //if next move() and previous no move() and step no junction acceleration = true; longMove = true; deceleration = false; }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0 || path[task-1] != 4) ) { //same as above, also if start field acceleration = true; longMove = true; deceleration = false; }else{ acceleration = 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) { //next step no move() or junction and speed above 120rpm deceleration = true; acceleration = false; lastMove = true; longMove = false; }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) { //next step no move() and no moveHalf() and speed above 120rpm deceleration = true; acceleration = false; lastMove = true; longMove = false; }else if (reverse == false && path[task+1] != path[task] && path[task+1] == 4 && avgSpeed > 2.4f*MOVE_SPEED) { lastMove = true; }else{ deceleration = false; } //printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration); //printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed); move(); break; case 2: rotateL(); break; case 3: rotateR(); break; case 4: if (reverse == false && path[task] == 4 && path[task+1] == 1) { //accelerate if next step is move() acceleration = true; longMove = true; deceleration = false; }else{ acceleration = false; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; } if (reverse == false && path[task-1] == 1 && path[task] == 4 && path[task+1] != 0) { //decelerate if previous step was move() deceleration = true; acceleration = false; 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; } } int Motion::finish() { return line; } void Motion::accel(float targetSpeed) { float fastSpeed; //Acclerated Target Speed if (speedRun == true) fastSpeed = targetSpeed*2.0f; else fastSpeed = targetSpeed*2.8f; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; //Acceleration logic //Short distance if (avgSpeed < targetSpeed && deceleration == false && acceleration == false && longMove == false) { actSpeed = ACCEL_CONST * t.read()*60.0f; //Acceleration equation }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) { actSpeed = targetSpeed+5.0f; //Keep Speed steady in case of overshooting //Long distance }else if (avgSpeed < fastSpeed && acceleration == true && deceleration == false) { actSpeed = ACCEL_CONST * t.read()*60.0f; }else if (avgSpeed > targetSpeed+5.0f && acceleration == false && deceleration == true) { actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f; //Deceleration equation }else if (avgSpeed < targetSpeed && acceleration == false && deceleration == true) { actSpeed = targetSpeed+5.0f; //Limit deceleration in case of overshooting } }