Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
Motion.cpp
- Committer:
- luethale
- Date:
- 2018-04-16
- Revision:
- 8:862bf9225953
- Parent:
- 5:47262622a9bf
File content as of revision 8:862bf9225953:
#include <cmath> #include "Motion.h" using namespace std; const float Motion::SPEEDLEFT = 50.0f; const float Motion::SPEEDRIGHT = 50.0f; Motion::Motion(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, IRSensor& irSensorL, IRSensor& irSensorC, IRSensor& irSensorR, DigitalOut& enableMotorDriver) : controller(controller), counterLeft(counterLeft), counterRight(counterRight), irSensorL(irSensorL), irSensorC(irSensorC), irSensorR(irSensorR), enableMotorDriver(enableMotorDriver) { countsL = 0; countsR = 0; countsLOld = 0; countsROld = 0 ; } Motion::~Motion() {} /** * Eine Feldbewegung druchführen */ void Motion::move() { //distanceL = irSensorL.readL(); //distanceC = irSensorC.readC(); //distanceR = irSensorR.readR(); float korr = 1; const float kp = 0.28f; float speedRight = -50.0f; float speedLeft = 50.0f; while ((countsL - countsLOld) < 1630 || (countsR - countsROld) > -1630) { countsL = counterLeft.read(); countsR = counterRight.read(); distanceL = irSensorL.readL(); distanceR = irSensorR.readR(); korr = (distanceL-distanceR)*kp; if (korr <= 1){ speedRight = -50.0f * korr; speedLeft = 50.0f } else speedLeft = 50.0f * -korr; speedRight = -50.0f; controller.setDesiredSpeedLeft(speedLeft); controller.setDesiredSpeedRight(speedRight); enableMotorDriver = 1; } //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); stop(); enableMotorDriver = 0; countsLOld = countsL + 150; countsROld = countsR - 150; } /** * 90° Rotation nach Links */ void Motion::rotateL() { while ((countsL - countsLOld) > -760 || (countsR - countsROld) > -760) { countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(-50.0f); controller.setDesiredSpeedRight(-50.0f); enableMotorDriver = 1; } stop(); enableMotorDriver = 0; countsLOld = countsL - 100; countsROld = countsR - 100; } /** * 90° Rotation nach Rechts */ void Motion::rotateR() { while ((countsL - countsLOld) < 760 || (countsR - countsROld) < 760) { countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(50.0f); controller.setDesiredSpeedRight(50.0f); enableMotorDriver = 1; } stop(); enableMotorDriver = 0; countsLOld = countsL + 100; countsROld = countsR + 100; } /** * Motor Stop */ void Motion::stop() { controller.setDesiredSpeedLeft(0.0f); controller.setDesiredSpeedRight(0.0f); } /** * 180° Rotation */ void Motion::rotate180() { while ((countsL - countsLOld) > -1560 || (countsR - countsROld) > -1560) { countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(-50.0f); controller.setDesiredSpeedRight(-50.0f); enableMotorDriver = 1; printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); } stop(); enableMotorDriver = 0; countsLOld = countsL - 150; countsROld = countsR - 150; } void Motion::test() { while (countsL > -1560 || countsR > - 1560) { countsL = counterLeft.read(); countsR = counterRight.read(); controller.setDesiredSpeedLeft(-50.0f); controller.setDesiredSpeedRight(-50.0f); enableMotorDriver = 1; printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); } counterLeft.reset(); counterRight.reset(); stop(); enableMotorDriver = 0; }