Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
Motion.cpp@5:47262622a9bf, 2018-04-16 (annotated)
- Committer:
- Helvis
- Date:
- Mon Apr 16 12:44:48 2018 +0000
- Revision:
- 5:47262622a9bf
- Parent:
- 4:e74c06e43485
- Child:
- 8:862bf9225953
testtestest;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Helvis | 0:9a3e7847a4be | 1 | |
Helvis | 0:9a3e7847a4be | 2 | #include <cmath> |
Helvis | 0:9a3e7847a4be | 3 | #include "Motion.h" |
Helvis | 0:9a3e7847a4be | 4 | |
Helvis | 0:9a3e7847a4be | 5 | using namespace std; |
Helvis | 1:1adf5dfcc7bb | 6 | |
Helvis | 4:e74c06e43485 | 7 | const float Motion::SPEEDLEFT = 50.0f; |
Helvis | 4:e74c06e43485 | 8 | const float Motion::SPEEDRIGHT = 50.0f; |
Helvis | 1:1adf5dfcc7bb | 9 | |
Helvis | 1:1adf5dfcc7bb | 10 | |
Helvis | 1:1adf5dfcc7bb | 11 | Motion::Motion(Controller& controller, EncoderCounter& counterLeft, |
Helvis | 1:1adf5dfcc7bb | 12 | EncoderCounter& counterRight, IRSensor& irSensorL, |
Helvis | 5:47262622a9bf | 13 | IRSensor& irSensorC, IRSensor& irSensorR, |
Helvis | 5:47262622a9bf | 14 | DigitalOut& enableMotorDriver) : |
Helvis | 1:1adf5dfcc7bb | 15 | controller(controller), counterLeft(counterLeft), |
Helvis | 1:1adf5dfcc7bb | 16 | counterRight(counterRight), irSensorL(irSensorL), |
Helvis | 5:47262622a9bf | 17 | irSensorC(irSensorC), irSensorR(irSensorR), |
Helvis | 5:47262622a9bf | 18 | enableMotorDriver(enableMotorDriver) { |
Helvis | 1:1adf5dfcc7bb | 19 | |
Helvis | 5:47262622a9bf | 20 | countsL = 0; |
Helvis | 5:47262622a9bf | 21 | countsR = 0; |
Helvis | 5:47262622a9bf | 22 | countsLOld = 0; |
Helvis | 5:47262622a9bf | 23 | countsROld = 0 ; |
Helvis | 1:1adf5dfcc7bb | 24 | } |
Helvis | 1:1adf5dfcc7bb | 25 | |
Helvis | 1:1adf5dfcc7bb | 26 | Motion::~Motion() {} |
Helvis | 1:1adf5dfcc7bb | 27 | |
Helvis | 5:47262622a9bf | 28 | /** |
Helvis | 5:47262622a9bf | 29 | * Eine Feldbewegung druchführen |
Helvis | 5:47262622a9bf | 30 | */ |
Helvis | 1:1adf5dfcc7bb | 31 | void Motion::move() { |
Helvis | 1:1adf5dfcc7bb | 32 | |
Helvis | 5:47262622a9bf | 33 | //distanceL = irSensorL.read(); |
Helvis | 5:47262622a9bf | 34 | //distanceC = irSensorC.read(); |
Helvis | 5:47262622a9bf | 35 | //distanceR = irSensorR.read(); |
Helvis | 5:47262622a9bf | 36 | |
Helvis | 5:47262622a9bf | 37 | while ((countsL - countsLOld) < 1630 || (countsR - countsROld) > -1630) { |
Helvis | 5:47262622a9bf | 38 | |
Helvis | 5:47262622a9bf | 39 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 40 | countsR = counterRight.read(); |
Helvis | 1:1adf5dfcc7bb | 41 | |
Helvis | 5:47262622a9bf | 42 | controller.setDesiredSpeedLeft(50.0f); |
Helvis | 5:47262622a9bf | 43 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 5:47262622a9bf | 44 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 45 | } |
Helvis | 5:47262622a9bf | 46 | //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); |
Helvis | 5:47262622a9bf | 47 | |
Helvis | 5:47262622a9bf | 48 | stop(); |
Helvis | 5:47262622a9bf | 49 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 50 | countsLOld = countsL + 150; |
Helvis | 5:47262622a9bf | 51 | countsROld = countsR - 150; |
Helvis | 1:1adf5dfcc7bb | 52 | } |
Helvis | 5:47262622a9bf | 53 | |
Helvis | 5:47262622a9bf | 54 | /** |
Helvis | 5:47262622a9bf | 55 | * 90° Rotation nach Links |
Helvis | 5:47262622a9bf | 56 | */ |
Helvis | 1:1adf5dfcc7bb | 57 | void Motion::rotateL() { |
Helvis | 1:1adf5dfcc7bb | 58 | |
Helvis | 5:47262622a9bf | 59 | while ((countsL - countsLOld) > -760 || (countsR - countsROld) > -760) { |
Helvis | 5:47262622a9bf | 60 | |
Helvis | 5:47262622a9bf | 61 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 62 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 63 | |
Helvis | 5:47262622a9bf | 64 | controller.setDesiredSpeedLeft(-50.0f); |
Helvis | 5:47262622a9bf | 65 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 5:47262622a9bf | 66 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 67 | } |
Helvis | 5:47262622a9bf | 68 | |
Helvis | 5:47262622a9bf | 69 | stop(); |
Helvis | 5:47262622a9bf | 70 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 71 | countsLOld = countsL - 100; |
Helvis | 5:47262622a9bf | 72 | countsROld = countsR - 100; |
Helvis | 1:1adf5dfcc7bb | 73 | } |
Helvis | 1:1adf5dfcc7bb | 74 | |
Helvis | 5:47262622a9bf | 75 | /** |
Helvis | 5:47262622a9bf | 76 | * 90° Rotation nach Rechts |
Helvis | 5:47262622a9bf | 77 | */ |
Helvis | 1:1adf5dfcc7bb | 78 | void Motion::rotateR() { |
Helvis | 1:1adf5dfcc7bb | 79 | |
Helvis | 5:47262622a9bf | 80 | while ((countsL - countsLOld) < 760 || (countsR - countsROld) < 760) { |
Helvis | 5:47262622a9bf | 81 | |
Helvis | 5:47262622a9bf | 82 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 83 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 84 | |
Helvis | 5:47262622a9bf | 85 | controller.setDesiredSpeedLeft(50.0f); |
Helvis | 5:47262622a9bf | 86 | controller.setDesiredSpeedRight(50.0f); |
Helvis | 5:47262622a9bf | 87 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 88 | } |
Helvis | 5:47262622a9bf | 89 | |
Helvis | 5:47262622a9bf | 90 | stop(); |
Helvis | 5:47262622a9bf | 91 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 92 | countsLOld = countsL + 100; |
Helvis | 5:47262622a9bf | 93 | countsROld = countsR + 100; |
Helvis | 1:1adf5dfcc7bb | 94 | } |
Helvis | 1:1adf5dfcc7bb | 95 | |
Helvis | 5:47262622a9bf | 96 | /** |
Helvis | 5:47262622a9bf | 97 | * Motor Stop |
Helvis | 5:47262622a9bf | 98 | */ |
Helvis | 5:47262622a9bf | 99 | void Motion::stop() { |
Helvis | 5:47262622a9bf | 100 | |
Helvis | 5:47262622a9bf | 101 | controller.setDesiredSpeedLeft(0.0f); |
Helvis | 5:47262622a9bf | 102 | controller.setDesiredSpeedRight(0.0f); |
Helvis | 5:47262622a9bf | 103 | } |
Helvis | 5:47262622a9bf | 104 | /** |
Helvis | 5:47262622a9bf | 105 | * 180° Rotation |
Helvis | 5:47262622a9bf | 106 | */ |
Helvis | 5:47262622a9bf | 107 | void Motion::rotate180() { |
Helvis | 1:1adf5dfcc7bb | 108 | |
Helvis | 5:47262622a9bf | 109 | while ((countsL - countsLOld) > -1560 || (countsR - countsROld) > -1560) { |
Helvis | 5:47262622a9bf | 110 | |
Helvis | 5:47262622a9bf | 111 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 112 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 113 | |
Helvis | 5:47262622a9bf | 114 | controller.setDesiredSpeedLeft(-50.0f); |
Helvis | 5:47262622a9bf | 115 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 5:47262622a9bf | 116 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 117 | printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); |
Helvis | 5:47262622a9bf | 118 | } |
Helvis | 5:47262622a9bf | 119 | |
Helvis | 5:47262622a9bf | 120 | stop(); |
Helvis | 5:47262622a9bf | 121 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 122 | countsLOld = countsL - 150; |
Helvis | 5:47262622a9bf | 123 | countsROld = countsR - 150; |
Helvis | 5:47262622a9bf | 124 | |
Helvis | 5:47262622a9bf | 125 | } |
Helvis | 5:47262622a9bf | 126 | |
Helvis | 5:47262622a9bf | 127 | void Motion::test() { |
Helvis | 5:47262622a9bf | 128 | |
Helvis | 5:47262622a9bf | 129 | while (countsL > -1560 || countsR > - 1560) { |
Helvis | 5:47262622a9bf | 130 | |
Helvis | 5:47262622a9bf | 131 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 132 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 133 | |
Helvis | 5:47262622a9bf | 134 | controller.setDesiredSpeedLeft(-50.0f); |
Helvis | 5:47262622a9bf | 135 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 5:47262622a9bf | 136 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 137 | printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); |
Helvis | 5:47262622a9bf | 138 | } |
Helvis | 5:47262622a9bf | 139 | |
Helvis | 5:47262622a9bf | 140 | counterLeft.reset(); |
Helvis | 5:47262622a9bf | 141 | counterRight.reset(); |
Helvis | 5:47262622a9bf | 142 | stop(); |
Helvis | 5:47262622a9bf | 143 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 144 | } |
Helvis | 5:47262622a9bf | 145 | |
Helvis | 1:1adf5dfcc7bb | 146 |