Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@5:e2c0a4388d85, 2018-04-23 (annotated)

Committer:
Helvis
Date:
Mon Apr 23 17:59:13 2018 +0000
Revision:
5:e2c0a4388d85
Parent:
4:932eb2d29206
Child:
6:4868f789c223
v0.9.4 Regelung bei Pfosten noch zu korrigieren

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Helvis 1:2b5f79285a3e 1
Helvis 1:2b5f79285a3e 2 #include <cmath>
Helvis 1:2b5f79285a3e 3 #include "Motion.h"
Helvis 1:2b5f79285a3e 4
Helvis 1:2b5f79285a3e 5 using namespace std;
Helvis 1:2b5f79285a3e 6
Helvis 3:076dd7ec7eb4 7 const float Motion::LEFT_MID_VAL = 44.73f;
Helvis 3:076dd7ec7eb4 8 const float Motion::RIGHT_MID_VAL = 47.03f;
Helvis 4:932eb2d29206 9 const float Motion::KP = 2.0f;
Helvis 5:e2c0a4388d85 10 const float Motion::KD = 0.0f;
Helvis 1:2b5f79285a3e 11
Helvis 1:2b5f79285a3e 12
Helvis 1:2b5f79285a3e 13 Motion::Motion(Controller& controller, EncoderCounter& counterLeft,
Helvis 1:2b5f79285a3e 14 EncoderCounter& counterRight, IRSensor& irSensorL,
Helvis 1:2b5f79285a3e 15 IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
Helvis 1:2b5f79285a3e 16 DigitalOut& enableMotorDriver) :
Helvis 1:2b5f79285a3e 17 controller(controller), counterLeft(counterLeft),
Helvis 1:2b5f79285a3e 18 counterRight(counterRight), irSensorL(irSensorL),
Helvis 1:2b5f79285a3e 19 irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
Helvis 1:2b5f79285a3e 20 enableMotorDriver(enableMotorDriver) {}
Helvis 1:2b5f79285a3e 21
Helvis 1:2b5f79285a3e 22 Motion::~Motion() {}
Helvis 1:2b5f79285a3e 23
Helvis 1:2b5f79285a3e 24 /**
Helvis 1:2b5f79285a3e 25 * Eine Feldstrecke druchführen
Helvis 1:2b5f79285a3e 26 */
Helvis 1:2b5f79285a3e 27 void Motion::move() {
Helvis 1:2b5f79285a3e 28
Helvis 1:2b5f79285a3e 29 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 30 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 31 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 32 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 33
Helvis 1:2b5f79285a3e 34 speedLeft = 50.0f;
Helvis 1:2b5f79285a3e 35 speedRight = -50.0f;
Helvis 1:2b5f79285a3e 36
Helvis 1:2b5f79285a3e 37 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 38 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 39
Helvis 5:e2c0a4388d85 40 while ((countsL - countsLOld) < 1605 || (countsR - countsROld) > -1605) {
Helvis 1:2b5f79285a3e 41
Helvis 1:2b5f79285a3e 42 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 43 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 44 distanceC = irSensorC.readC();
Helvis 1:2b5f79285a3e 45
Helvis 1:2b5f79285a3e 46 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 47
Helvis 5:e2c0a4388d85 48 control();
Helvis 5:e2c0a4388d85 49
Helvis 5:e2c0a4388d85 50 if ((distanceC) < 40.0f) {
Helvis 5:e2c0a4388d85 51 countsLOld = countsL;
Helvis 5:e2c0a4388d85 52 countsROld = countsR;
Helvis 5:e2c0a4388d85 53 while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
Helvis 5:e2c0a4388d85 54 countsL = counterLeft.read();
Helvis 5:e2c0a4388d85 55 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 56 }
Helvis 5:e2c0a4388d85 57 stop();
Helvis 5:e2c0a4388d85 58 break;
Helvis 5:e2c0a4388d85 59 }else if ( ((countsL - countsLOld) >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
Helvis 5:e2c0a4388d85 60 countsLOld += 500;
Helvis 5:e2c0a4388d85 61 countsROld += 500;
Helvis 5:e2c0a4388d85 62 }
Helvis 1:2b5f79285a3e 63
Helvis 1:2b5f79285a3e 64 }
Helvis 1:2b5f79285a3e 65 }
Helvis 1:2b5f79285a3e 66 /**
Helvis 1:2b5f79285a3e 67 * Eine Feldstrecke mit höherer Geschwindigkeit fahren
Helvis 1:2b5f79285a3e 68 */
Helvis 1:2b5f79285a3e 69 void Motion::moveFast() {
Helvis 1:2b5f79285a3e 70
Helvis 1:2b5f79285a3e 71 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 72 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 73 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 74 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 75
Helvis 1:2b5f79285a3e 76 speedLeft = 100.0f;
Helvis 1:2b5f79285a3e 77 speedRight = -100.0f;
Helvis 1:2b5f79285a3e 78
Helvis 1:2b5f79285a3e 79 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 80 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 81
Helvis 1:2b5f79285a3e 82 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
Helvis 1:2b5f79285a3e 83
Helvis 1:2b5f79285a3e 84 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 85 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 86 distanceC = irSensorC.readC();
Helvis 1:2b5f79285a3e 87
Helvis 1:2b5f79285a3e 88 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 89
Helvis 1:2b5f79285a3e 90 control();
Helvis 1:2b5f79285a3e 91
Helvis 5:e2c0a4388d85 92 if (distanceC < 40.0f) {
Helvis 5:e2c0a4388d85 93 stop();
Helvis 5:e2c0a4388d85 94 break;
Helvis 5:e2c0a4388d85 95 }
Helvis 1:2b5f79285a3e 96 }
Helvis 1:2b5f79285a3e 97 }
Helvis 1:2b5f79285a3e 98 /**
Helvis 1:2b5f79285a3e 99 * Eine Feldstrecke mit überprüfung der Ziellinie fahren
Helvis 1:2b5f79285a3e 100 */
Helvis 1:2b5f79285a3e 101 void Motion::scanMove() {
Helvis 1:2b5f79285a3e 102
Helvis 1:2b5f79285a3e 103 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 104 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 105 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 106 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 107
Helvis 1:2b5f79285a3e 108 speedLeft = 50.0f;
Helvis 1:2b5f79285a3e 109 speedRight = -50.0f;
Helvis 1:2b5f79285a3e 110
Helvis 1:2b5f79285a3e 111 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 112 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 113
Helvis 5:e2c0a4388d85 114 distanceC = irSensorC.readC();
Helvis 5:e2c0a4388d85 115
Helvis 5:e2c0a4388d85 116 while ((countsL - countsLOld) < 1605 || (countsR - countsROld) > -1605) {
Helvis 1:2b5f79285a3e 117
Helvis 1:2b5f79285a3e 118 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 119 countsR = counterRight.read();
Helvis 3:076dd7ec7eb4 120 distanceC = irSensorC.readC();
Helvis 1:2b5f79285a3e 121
Helvis 1:2b5f79285a3e 122 if (enableMotorDriver == 0) {
Helvis 1:2b5f79285a3e 123 enableMotorDriver = 1;
Helvis 1:2b5f79285a3e 124 }
Helvis 1:2b5f79285a3e 125
Helvis 3:076dd7ec7eb4 126 if (lineSensor.read() == 1.0f) {
Helvis 1:2b5f79285a3e 127 line = 1;
Helvis 1:2b5f79285a3e 128 }
Helvis 1:2b5f79285a3e 129
Helvis 5:e2c0a4388d85 130 control();
Helvis 1:2b5f79285a3e 131
Helvis 5:e2c0a4388d85 132 if ((distanceC) < 40.0f) {
Helvis 5:e2c0a4388d85 133 countsLOld = countsL;
Helvis 5:e2c0a4388d85 134 countsROld = countsR;
Helvis 5:e2c0a4388d85 135 while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
Helvis 5:e2c0a4388d85 136 countsL = counterLeft.read();
Helvis 5:e2c0a4388d85 137 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 138 }
Helvis 3:076dd7ec7eb4 139 stop();
Helvis 3:076dd7ec7eb4 140 break;
Helvis 5:e2c0a4388d85 141 }else if ( ((countsL - countsLOld) >= 1647 || (countsR - countsROld) >= -1647) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
Helvis 5:e2c0a4388d85 142 countsLOld += 500;
Helvis 5:e2c0a4388d85 143 countsROld += 500;
Helvis 3:076dd7ec7eb4 144 }
Helvis 5:e2c0a4388d85 145 }
Helvis 1:2b5f79285a3e 146 }
Helvis 1:2b5f79285a3e 147 /**
Helvis 1:2b5f79285a3e 148 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 149 */
Helvis 1:2b5f79285a3e 150 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 151
Helvis 1:2b5f79285a3e 152 stop();
Helvis 1:2b5f79285a3e 153 controller.counterReset();
Helvis 1:2b5f79285a3e 154 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 155 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 156 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 157 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 158
Helvis 1:2b5f79285a3e 159 controller.setDesiredSpeedLeft(-20.0f);
Helvis 1:2b5f79285a3e 160 controller.setDesiredSpeedRight(-20.0f);
Helvis 1:2b5f79285a3e 161
Helvis 1:2b5f79285a3e 162 while ((countsL - countsLOld) > -862 || (countsR - countsROld) > -862) {
Helvis 1:2b5f79285a3e 163
Helvis 1:2b5f79285a3e 164 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 165 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 166
Helvis 1:2b5f79285a3e 167 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 168 }
Helvis 1:2b5f79285a3e 169
Helvis 1:2b5f79285a3e 170 stop();
Helvis 1:2b5f79285a3e 171 }
Helvis 1:2b5f79285a3e 172
Helvis 1:2b5f79285a3e 173 /**
Helvis 1:2b5f79285a3e 174 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 175 */
Helvis 1:2b5f79285a3e 176 void Motion::rotateR() {
Helvis 1:2b5f79285a3e 177
Helvis 1:2b5f79285a3e 178 stop();
Helvis 1:2b5f79285a3e 179 controller.counterReset();
Helvis 1:2b5f79285a3e 180 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 181 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 182 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 183 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 184
Helvis 1:2b5f79285a3e 185 controller.setDesiredSpeedLeft(20.0f);
Helvis 1:2b5f79285a3e 186 controller.setDesiredSpeedRight(20.0f);
Helvis 1:2b5f79285a3e 187
Helvis 1:2b5f79285a3e 188 while ((countsL - countsLOld) < 862 || (countsR - countsROld) < 862) {
Helvis 1:2b5f79285a3e 189
Helvis 1:2b5f79285a3e 190 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 191 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 192
Helvis 1:2b5f79285a3e 193 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 194 }
Helvis 1:2b5f79285a3e 195
Helvis 1:2b5f79285a3e 196 stop();
Helvis 1:2b5f79285a3e 197 }
Helvis 2:f898adf2d817 198 /**
Helvis 2:f898adf2d817 199 * Links abbiegen
Helvis 2:f898adf2d817 200 */
Helvis 3:076dd7ec7eb4 201 void Motion::turnL() {
Helvis 2:f898adf2d817 202
Helvis 2:f898adf2d817 203 controller.counterReset();
Helvis 2:f898adf2d817 204 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 205 countsROld = counterRight.read();
Helvis 2:f898adf2d817 206 countsL = counterLeft.read();
Helvis 2:f898adf2d817 207 countsR = counterRight.read();
Helvis 2:f898adf2d817 208
Helvis 2:f898adf2d817 209 controller.setDesiredSpeedLeft(17.0f);
Helvis 2:f898adf2d817 210 controller.setDesiredSpeedRight(-83.0f);
Helvis 2:f898adf2d817 211
Helvis 2:f898adf2d817 212 while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
Helvis 2:f898adf2d817 213
Helvis 2:f898adf2d817 214 countsL = counterLeft.read();
Helvis 2:f898adf2d817 215 countsR = counterRight.read();
Helvis 2:f898adf2d817 216
Helvis 2:f898adf2d817 217 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 218 }
Helvis 2:f898adf2d817 219 }
Helvis 2:f898adf2d817 220 /**
Helvis 2:f898adf2d817 221 * Rechts abbiegen
Helvis 2:f898adf2d817 222 */
Helvis 3:076dd7ec7eb4 223 void Motion::turnR() {
Helvis 2:f898adf2d817 224
Helvis 2:f898adf2d817 225 controller.counterReset();
Helvis 2:f898adf2d817 226 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 227 countsROld = counterRight.read();
Helvis 2:f898adf2d817 228 countsL = counterLeft.read();
Helvis 2:f898adf2d817 229 countsR = counterRight.read();
Helvis 2:f898adf2d817 230
Helvis 2:f898adf2d817 231 controller.setDesiredSpeedLeft(83.0f);
Helvis 2:f898adf2d817 232 controller.setDesiredSpeedRight(-17.0f);
Helvis 2:f898adf2d817 233
Helvis 2:f898adf2d817 234 while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
Helvis 2:f898adf2d817 235
Helvis 2:f898adf2d817 236 countsL = counterLeft.read();
Helvis 2:f898adf2d817 237 countsR = counterRight.read();
Helvis 2:f898adf2d817 238
Helvis 2:f898adf2d817 239 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 3:076dd7ec7eb4 240 }
Helvis 2:f898adf2d817 241 }
Helvis 1:2b5f79285a3e 242 /**
Helvis 1:2b5f79285a3e 243 * Motor Stop
Helvis 1:2b5f79285a3e 244 */
Helvis 1:2b5f79285a3e 245 void Motion::stop() {
Helvis 1:2b5f79285a3e 246
Helvis 1:2b5f79285a3e 247 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 248 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 249
Helvis 1:2b5f79285a3e 250 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 251 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 252
Helvis 1:2b5f79285a3e 253 waitStop = 0;
Helvis 1:2b5f79285a3e 254 while( waitStop < ticks) {
Helvis 1:2b5f79285a3e 255 waitStop+= 1;
Helvis 1:2b5f79285a3e 256 }
Helvis 1:2b5f79285a3e 257 }
Helvis 1:2b5f79285a3e 258 /**
Helvis 1:2b5f79285a3e 259 * 180° Rotation
Helvis 1:2b5f79285a3e 260 */
Helvis 1:2b5f79285a3e 261 void Motion::rotate180() {
Helvis 1:2b5f79285a3e 262
Helvis 1:2b5f79285a3e 263 stop();
Helvis 1:2b5f79285a3e 264 controller.counterReset();
Helvis 1:2b5f79285a3e 265 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 266 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 267 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 268 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 269
Helvis 1:2b5f79285a3e 270 controller.setDesiredSpeedLeft(-50.0f);
Helvis 1:2b5f79285a3e 271 controller.setDesiredSpeedRight(-50.0f);
Helvis 1:2b5f79285a3e 272
Helvis 1:2b5f79285a3e 273 while ((countsL - countsLOld) > -1723 || (countsR - countsROld) > -1723) {
Helvis 1:2b5f79285a3e 274
Helvis 1:2b5f79285a3e 275 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 276 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 277
Helvis 1:2b5f79285a3e 278 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 279 }
Helvis 1:2b5f79285a3e 280
Helvis 1:2b5f79285a3e 281 stop();
Helvis 1:2b5f79285a3e 282 }
Helvis 1:2b5f79285a3e 283
Helvis 1:2b5f79285a3e 284
Helvis 1:2b5f79285a3e 285 void Motion::control() {
Helvis 1:2b5f79285a3e 286
Helvis 4:932eb2d29206 287 float wallLeft = 80.0f;
Helvis 4:932eb2d29206 288 float wallRight = 80.0f;
Helvis 1:2b5f79285a3e 289
Helvis 1:2b5f79285a3e 290 distanceL = irSensorL.readL();
Helvis 1:2b5f79285a3e 291 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 292
Helvis 5:e2c0a4388d85 293
Helvis 4:932eb2d29206 294 if (distanceL < wallLeft && distanceR < wallRight) {
Helvis 5:e2c0a4388d85 295 /* if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 4:932eb2d29206 296
Helvis 4:932eb2d29206 297 errorP = LEFT_MID_VAL - distanceL;
Helvis 4:932eb2d29206 298
Helvis 4:932eb2d29206 299 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 4:932eb2d29206 300
Helvis 4:932eb2d29206 301 errorP = distanceR - RIGHT_MID_VAL;
Helvis 4:932eb2d29206 302 }*/
Helvis 1:2b5f79285a3e 303
Helvis 4:932eb2d29206 304 errorP = distanceL - distanceR + 2.30f;
Helvis 5:e2c0a4388d85 305 //errorD = errorP - oldErrorP;
Helvis 1:2b5f79285a3e 306 }else{
Helvis 1:2b5f79285a3e 307
Helvis 5:e2c0a4388d85 308 errorP = 0;
Helvis 5:e2c0a4388d85 309 errorD = 0;
Helvis 1:2b5f79285a3e 310 }
Helvis 1:2b5f79285a3e 311
Helvis 5:e2c0a4388d85 312 //errorD = errorP - oldErrorP;
Helvis 1:2b5f79285a3e 313
Helvis 1:2b5f79285a3e 314 oldErrorP = errorP;
Helvis 1:2b5f79285a3e 315
Helvis 5:e2c0a4388d85 316 if (abs(errorP) < 80.0f) {
Helvis 5:e2c0a4388d85 317 totalError = KP*errorP + KD*errorD;
Helvis 5:e2c0a4388d85 318 }else{
Helvis 5:e2c0a4388d85 319 totalError = 0;
Helvis 5:e2c0a4388d85 320 }
Helvis 1:2b5f79285a3e 321
Helvis 5:e2c0a4388d85 322 controller.setDesiredSpeedLeft(speedLeft - totalError);
Helvis 5:e2c0a4388d85 323 controller.setDesiredSpeedRight(speedRight - totalError);
Helvis 1:2b5f79285a3e 324 }
Helvis 1:2b5f79285a3e 325
Helvis 1:2b5f79285a3e 326 void Motion::runTask(int task) {
Helvis 1:2b5f79285a3e 327
Helvis 1:2b5f79285a3e 328 switch(task) {
Helvis 1:2b5f79285a3e 329
Helvis 1:2b5f79285a3e 330 case 1:
Helvis 1:2b5f79285a3e 331 move();
Helvis 1:2b5f79285a3e 332 break;
Helvis 1:2b5f79285a3e 333 case 2:
Helvis 1:2b5f79285a3e 334 rotateL();
Helvis 1:2b5f79285a3e 335 break;
Helvis 1:2b5f79285a3e 336 case 3:
Helvis 1:2b5f79285a3e 337 rotateR();
Helvis 1:2b5f79285a3e 338 break;
Helvis 1:2b5f79285a3e 339 case 4:
Helvis 1:2b5f79285a3e 340 moveFast();
Helvis 1:2b5f79285a3e 341 break;
Helvis 1:2b5f79285a3e 342 case 5:
Helvis 1:2b5f79285a3e 343 stop();
Helvis 1:2b5f79285a3e 344 break;
Helvis 1:2b5f79285a3e 345 }
Helvis 1:2b5f79285a3e 346 }
Helvis 1:2b5f79285a3e 347
Helvis 1:2b5f79285a3e 348 int Motion::finish() {
Helvis 1:2b5f79285a3e 349
Helvis 1:2b5f79285a3e 350 return line;
Helvis 1:2b5f79285a3e 351 }