Alexander Lüthard
/
Micromouse
Alle Prinf's auskommentiert
Fork of Micromouse by
Diff: Motion.cpp
- Revision:
- 1:2b5f79285a3e
- Child:
- 2:f898adf2d817
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motion.cpp Thu Apr 19 06:19:43 2018 +0000 @@ -0,0 +1,268 @@ + +#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; +}