Helvijs Kiselis
/
Micromouse
Algorithmus
Motion.cpp
- Committer:
- Helvis
- Date:
- 2018-04-19
- Revision:
- 1:2b5f79285a3e
- Child:
- 2:f898adf2d817
File content as of revision 1:2b5f79285a3e:
#include <cmath> #include "Motion.h" using namespace std; const float Motion::LEFT_MID_VAL = 45.0f; const float Motion::RIGHT_MID_VAL = 45.0f; const float Motion::KP = 2.5f; const float Motion::KD = 0.00f; 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() { controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); speedLeft = 50.0f; speedRight = -50.0f; controller.setDesiredSpeedLeft(speedLeft); controller.setDesiredSpeedRight(speedRight); while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) { countsL = counterLeft.read(); countsR = counterRight.read(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} control(); } } /** * Eine Feldstrecke mit höherer Geschwindigkeit fahren */ void Motion::moveFast() { controller.counterReset(); 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(); if (enableMotorDriver == 0) {enableMotorDriver = 1;} control(); } } /** * Eine Feldstrecke mit überprüfung der Ziellinie fahren */ void Motion::scanMove() { controller.counterReset(); countsLOld = counterLeft.read(); countsROld = counterRight.read(); countsL = counterLeft.read(); countsR = counterRight.read(); speedLeft = 50.0f; speedRight = -50.0f; controller.setDesiredSpeedLeft(speedLeft); controller.setDesiredSpeedRight(speedRight); while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) { countsL = counterLeft.read(); countsR = counterRight.read(); if (enableMotorDriver == 0) { enableMotorDriver = 1; } if (lineSensor.read() > 0.9f) { line = 1; }else{ line = 0; } control(); } } /** * 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(); } /** * 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 = 70.0f; //float wallRight = 70.0f; distanceL = irSensorL.readL(); distanceR = irSensorR.readR(); if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) { errorP = LEFT_MID_VAL - distanceL; }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) { errorP = distanceR - RIGHT_MID_VAL; }else{ errorP = 0; } errorD = errorP - oldErrorP; oldErrorP = errorP; totalError = KP*errorP + KD*errorD; 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; }