Helvijs Kiselis
/
Micromouse
Algorithmus
Motion.cpp
- Committer:
- Helvis
- Date:
- 2018-04-27
- Revision:
- 7:22392ed60534
- Parent:
- 6:4868f789c223
- Child:
- 11:2960fc540616
File content as of revision 7:22392ed60534:
#include <cmath> #include "Motion.h" using namespace std; const float Motion::LEFT_MID_VAL = 41.73f; //44.73 const float Motion::RIGHT_MID_VAL = 44.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; 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(); speedLeft = 50.0f; speedRight = -50.0f; controller.setDesiredSpeedLeft(MOVE_SPEED); controller.setDesiredSpeedRight(-MOVE_SPEED); while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) { countsL = counterLeft.read(); countsR = counterRight.read(); distanceC = irSensorC.readC(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} 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) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 150.0f) && (distanceC > 40.0f)) { countsLOld += 500; countsROld += 500; } } } /** * Eine Feldstrecke mit höherer Geschwindigkeit fahren */ void Motion::moveFast() { countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); speedLeft = 100.0f; speedRight = -100.0f; controller.setDesiredSpeedLeft(speedLeft); controller.setDesiredSpeedRight(speedRight); while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) { countsL = counterLeft.read(); countsR = counterRight.read(); distanceC = irSensorC.readC(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} control(); if (distanceC < 40.0f) { stop(); break; } } } /** * Eine Feldstrecke mit überprüfung der Ziellinie fahren */ void Motion::scanMove() { countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); speedLeft = 50.0f; speedRight = -50.0f; controller.setDesiredSpeedLeft(MOVE_SPEED); controller.setDesiredSpeedRight(-MOVE_SPEED); distanceC = irSensorC.readC(); while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) { countsL = counterLeft.read(); countsR = counterRight.read(); distanceC = irSensorC.readC(); if (enableMotorDriver == 0) { enableMotorDriver = 1; } if (lineSensor.read() == 1.0f) { line = 1; } 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) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) { countsLOld += 500; countsROld += 500; } } } /** * 90° Rotation nach Links */ void Motion::rotateL() { stop(); controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(-20.0f); controller.setDesiredSpeedRight(-20.0f); while ((countsL - countsLOld) > -862 || (countsR - countsROld) > -862) { 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(20.0f); controller.setDesiredSpeedRight(20.0f); while ((countsL - countsLOld) < 862 || (countsR - countsROld) < 862) { 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); float sL = controller.getSpeedLeft(); float ticks = 0.08f*sL; waitStop = 0; while( waitStop < ticks) { waitStop+= 1; } } /** * 180° Rotation */ void Motion::rotate180() { stop(); 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) > -1723 || (countsR - countsROld) > -1723) { countsL = counterLeft.read(); countsR = counterRight.read(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} } 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(speedLeft - totalError); controller.setDesiredSpeedRight(speedRight - totalError); } void Motion::runTask(int task) { switch(task) { case 1: move(); break; case 2: rotateL(); break; case 3: rotateR(); break; case 4: moveFast(); break; case 5: stop(); break; } } int Motion::finish() { return line; }