Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
Motion.cpp@8:862bf9225953, 2018-04-16 (annotated)
- Committer:
- luethale
- Date:
- Mon Apr 16 15:23:42 2018 +0000
- Revision:
- 8:862bf9225953
- Parent:
- 5:47262622a9bf
Regelung f?r Spurhaltung
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 | |
luethale | 8:862bf9225953 | 33 | //distanceL = irSensorL.readL(); |
luethale | 8:862bf9225953 | 34 | //distanceC = irSensorC.readC(); |
luethale | 8:862bf9225953 | 35 | //distanceR = irSensorR.readR(); |
luethale | 8:862bf9225953 | 36 | float korr = 1; |
luethale | 8:862bf9225953 | 37 | const float kp = 0.28f; |
luethale | 8:862bf9225953 | 38 | float speedRight = -50.0f; |
luethale | 8:862bf9225953 | 39 | float speedLeft = 50.0f; |
Helvis | 5:47262622a9bf | 40 | |
Helvis | 5:47262622a9bf | 41 | while ((countsL - countsLOld) < 1630 || (countsR - countsROld) > -1630) { |
Helvis | 5:47262622a9bf | 42 | |
Helvis | 5:47262622a9bf | 43 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 44 | countsR = counterRight.read(); |
luethale | 8:862bf9225953 | 45 | distanceL = irSensorL.readL(); |
luethale | 8:862bf9225953 | 46 | distanceR = irSensorR.readR(); |
luethale | 8:862bf9225953 | 47 | |
luethale | 8:862bf9225953 | 48 | korr = (distanceL-distanceR)*kp; |
luethale | 8:862bf9225953 | 49 | |
luethale | 8:862bf9225953 | 50 | if (korr <= 1){ |
luethale | 8:862bf9225953 | 51 | speedRight = -50.0f * korr; |
luethale | 8:862bf9225953 | 52 | speedLeft = 50.0f |
luethale | 8:862bf9225953 | 53 | } |
luethale | 8:862bf9225953 | 54 | else |
luethale | 8:862bf9225953 | 55 | speedLeft = 50.0f * -korr; |
luethale | 8:862bf9225953 | 56 | speedRight = -50.0f; |
Helvis | 1:1adf5dfcc7bb | 57 | |
luethale | 8:862bf9225953 | 58 | controller.setDesiredSpeedLeft(speedLeft); |
luethale | 8:862bf9225953 | 59 | controller.setDesiredSpeedRight(speedRight); |
Helvis | 5:47262622a9bf | 60 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 61 | } |
Helvis | 5:47262622a9bf | 62 | //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); |
Helvis | 5:47262622a9bf | 63 | |
Helvis | 5:47262622a9bf | 64 | stop(); |
Helvis | 5:47262622a9bf | 65 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 66 | countsLOld = countsL + 150; |
Helvis | 5:47262622a9bf | 67 | countsROld = countsR - 150; |
Helvis | 1:1adf5dfcc7bb | 68 | } |
Helvis | 5:47262622a9bf | 69 | |
Helvis | 5:47262622a9bf | 70 | /** |
Helvis | 5:47262622a9bf | 71 | * 90° Rotation nach Links |
Helvis | 5:47262622a9bf | 72 | */ |
Helvis | 1:1adf5dfcc7bb | 73 | void Motion::rotateL() { |
Helvis | 1:1adf5dfcc7bb | 74 | |
Helvis | 5:47262622a9bf | 75 | while ((countsL - countsLOld) > -760 || (countsR - countsROld) > -760) { |
Helvis | 5:47262622a9bf | 76 | |
Helvis | 5:47262622a9bf | 77 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 78 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 79 | |
Helvis | 5:47262622a9bf | 80 | controller.setDesiredSpeedLeft(-50.0f); |
Helvis | 5:47262622a9bf | 81 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 5:47262622a9bf | 82 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 83 | } |
Helvis | 5:47262622a9bf | 84 | |
Helvis | 5:47262622a9bf | 85 | stop(); |
Helvis | 5:47262622a9bf | 86 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 87 | countsLOld = countsL - 100; |
Helvis | 5:47262622a9bf | 88 | countsROld = countsR - 100; |
Helvis | 1:1adf5dfcc7bb | 89 | } |
Helvis | 1:1adf5dfcc7bb | 90 | |
Helvis | 5:47262622a9bf | 91 | /** |
Helvis | 5:47262622a9bf | 92 | * 90° Rotation nach Rechts |
Helvis | 5:47262622a9bf | 93 | */ |
Helvis | 1:1adf5dfcc7bb | 94 | void Motion::rotateR() { |
Helvis | 1:1adf5dfcc7bb | 95 | |
Helvis | 5:47262622a9bf | 96 | while ((countsL - countsLOld) < 760 || (countsR - countsROld) < 760) { |
Helvis | 5:47262622a9bf | 97 | |
Helvis | 5:47262622a9bf | 98 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 99 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 100 | |
Helvis | 5:47262622a9bf | 101 | controller.setDesiredSpeedLeft(50.0f); |
Helvis | 5:47262622a9bf | 102 | controller.setDesiredSpeedRight(50.0f); |
Helvis | 5:47262622a9bf | 103 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 104 | } |
Helvis | 5:47262622a9bf | 105 | |
Helvis | 5:47262622a9bf | 106 | stop(); |
Helvis | 5:47262622a9bf | 107 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 108 | countsLOld = countsL + 100; |
Helvis | 5:47262622a9bf | 109 | countsROld = countsR + 100; |
Helvis | 1:1adf5dfcc7bb | 110 | } |
Helvis | 1:1adf5dfcc7bb | 111 | |
Helvis | 5:47262622a9bf | 112 | /** |
Helvis | 5:47262622a9bf | 113 | * Motor Stop |
Helvis | 5:47262622a9bf | 114 | */ |
Helvis | 5:47262622a9bf | 115 | void Motion::stop() { |
Helvis | 5:47262622a9bf | 116 | |
Helvis | 5:47262622a9bf | 117 | controller.setDesiredSpeedLeft(0.0f); |
Helvis | 5:47262622a9bf | 118 | controller.setDesiredSpeedRight(0.0f); |
Helvis | 5:47262622a9bf | 119 | } |
Helvis | 5:47262622a9bf | 120 | /** |
Helvis | 5:47262622a9bf | 121 | * 180° Rotation |
Helvis | 5:47262622a9bf | 122 | */ |
Helvis | 5:47262622a9bf | 123 | void Motion::rotate180() { |
Helvis | 1:1adf5dfcc7bb | 124 | |
Helvis | 5:47262622a9bf | 125 | while ((countsL - countsLOld) > -1560 || (countsR - countsROld) > -1560) { |
Helvis | 5:47262622a9bf | 126 | |
Helvis | 5:47262622a9bf | 127 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 128 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 129 | |
Helvis | 5:47262622a9bf | 130 | controller.setDesiredSpeedLeft(-50.0f); |
Helvis | 5:47262622a9bf | 131 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 5:47262622a9bf | 132 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 133 | printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); |
Helvis | 5:47262622a9bf | 134 | } |
Helvis | 5:47262622a9bf | 135 | |
Helvis | 5:47262622a9bf | 136 | stop(); |
Helvis | 5:47262622a9bf | 137 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 138 | countsLOld = countsL - 150; |
Helvis | 5:47262622a9bf | 139 | countsROld = countsR - 150; |
Helvis | 5:47262622a9bf | 140 | |
Helvis | 5:47262622a9bf | 141 | } |
Helvis | 5:47262622a9bf | 142 | |
Helvis | 5:47262622a9bf | 143 | void Motion::test() { |
Helvis | 5:47262622a9bf | 144 | |
Helvis | 5:47262622a9bf | 145 | while (countsL > -1560 || countsR > - 1560) { |
Helvis | 5:47262622a9bf | 146 | |
Helvis | 5:47262622a9bf | 147 | countsL = counterLeft.read(); |
Helvis | 5:47262622a9bf | 148 | countsR = counterRight.read(); |
Helvis | 5:47262622a9bf | 149 | |
Helvis | 5:47262622a9bf | 150 | controller.setDesiredSpeedLeft(-50.0f); |
Helvis | 5:47262622a9bf | 151 | controller.setDesiredSpeedRight(-50.0f); |
Helvis | 5:47262622a9bf | 152 | enableMotorDriver = 1; |
Helvis | 5:47262622a9bf | 153 | printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR); |
Helvis | 5:47262622a9bf | 154 | } |
Helvis | 5:47262622a9bf | 155 | |
Helvis | 5:47262622a9bf | 156 | counterLeft.reset(); |
Helvis | 5:47262622a9bf | 157 | counterRight.reset(); |
Helvis | 5:47262622a9bf | 158 | stop(); |
Helvis | 5:47262622a9bf | 159 | enableMotorDriver = 0; |
Helvis | 5:47262622a9bf | 160 | } |
Helvis | 5:47262622a9bf | 161 | |
Helvis | 1:1adf5dfcc7bb | 162 |