Helvijs Kiselis
/
Micromouse
Algorithmus
Motion.cpp
- Committer:
- Helvis
- Date:
- 2018-05-15
- Revision:
- 22:91526c8d15ba
- Parent:
- 21:41997651337a
- Child:
- 23:accd07ca2da7
File content as of revision 22:91526c8d15ba:
#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.5; const float Motion::KD = 0.0f; const int Motion::MOVE_DIST = 1605; const float Motion::MOVE_SPEED = 50.0f; const float Motion::SCAN_SPEED = 50.0f; const float Motion::ROTATE_SPEED = 80.0f; const float Motion::ACCEL_CONST = 3.5f; //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; 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(); distanceC = irSensorC.readC(); if (distanceC > 40.0f) { stop(); break; } } stop(); break; }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) { countsLOld += 500; countsROld += 500; }else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) { countsLOld = counterLeft.read(); countsROld = counterRight.read(); while ((countsL - countsLOld) < MOVE_DIST*0.5f + 816.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 816.0f) { countsL = counterLeft.read(); countsR = counterRight.read(); accel(MOVE_SPEED); control(); } stop(); break; } } t.stop(); t.reset(); lastMove = 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(); 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; } if (lineSensor.read() > 0.85f) { line = 1; } accel(SCAN_SPEED); control(); //controller.setDesiredSpeedLeft(actSpeed); //controller.setDesiredSpeedRight(-actSpeed); 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; }/*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; } /** * 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(17.0f); controller.setDesiredSpeedRight(-83.0f); while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) { 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(83.0f); controller.setDesiredSpeedRight(-17.0f); while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) { 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 = 48.73f; float wallRight = 51.03f; distanceL = irSensorL.readL(); distanceR = irSensorR.readR(); if (distanceL < wallLeft && distanceR < wallRight) { errorP = distanceL - distanceR + 2.30f; }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; }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) { switch(path[task]) { case 1: if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) { acceleration = true; longMove = true; deceleration = false; }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) { acceleration = true; longMove = true; deceleration = false; }else{ acceleration = false; deceleration = false; avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; } if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) { deceleration = true; acceleration = false; lastMove = true; longMove = false; }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) { deceleration = true; acceleration = false; lastMove = true; longMove = false; }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: stop(); break; } } int Motion::finish() { return line; } void Motion::accel(float targetSpeed) { avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f; if ( avgSpeed < targetSpeed && deceleration == false && acceleration == false) { actSpeed = ACCEL_CONST * t.read()*60.0f; }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) { actSpeed = 55.0f; }else if (avgSpeed < targetSpeed*2.8f && acceleration == true) { actSpeed = ACCEL_CONST * t.read()*60.0f; }else if (avgSpeed > targetSpeed && deceleration == true) { actSpeed = targetSpeed*2.8f - ACCEL_CONST * t.read()*60.0f; } }