Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@4:932eb2d29206, 2018-04-23 (annotated)

Committer:
Helvis
Date:
Mon Apr 23 12:14:54 2018 +0000
Revision:
4:932eb2d29206
Parent:
3:076dd7ec7eb4
Child:
5:e2c0a4388d85
v0.9.4 f?r Programmstruktur;

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 1:2b5f79285a3e 10 const float Motion::KD = 0.00f;
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 controller.counterReset();
Helvis 1:2b5f79285a3e 30 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 31 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 32 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 33 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 34
Helvis 1:2b5f79285a3e 35 speedLeft = 50.0f;
Helvis 1:2b5f79285a3e 36 speedRight = -50.0f;
Helvis 1:2b5f79285a3e 37
Helvis 1:2b5f79285a3e 38 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 39 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 40
Helvis 4:932eb2d29206 41 while ((countsL - countsLOld) < 1565 || (countsR - countsROld) > -1565) {
Helvis 1:2b5f79285a3e 42
Helvis 1:2b5f79285a3e 43 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 44 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 45
Helvis 1:2b5f79285a3e 46 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 47
Helvis 1:2b5f79285a3e 48 control();
Helvis 1:2b5f79285a3e 49
Helvis 1:2b5f79285a3e 50 }
Helvis 1:2b5f79285a3e 51 }
Helvis 1:2b5f79285a3e 52 /**
Helvis 1:2b5f79285a3e 53 * Eine Feldstrecke mit höherer Geschwindigkeit fahren
Helvis 1:2b5f79285a3e 54 */
Helvis 1:2b5f79285a3e 55 void Motion::moveFast() {
Helvis 1:2b5f79285a3e 56
Helvis 1:2b5f79285a3e 57 controller.counterReset();
Helvis 1:2b5f79285a3e 58 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 59 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 60 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 61 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 62
Helvis 1:2b5f79285a3e 63 speedLeft = 100.0f;
Helvis 1:2b5f79285a3e 64 speedRight = -100.0f;
Helvis 1:2b5f79285a3e 65
Helvis 1:2b5f79285a3e 66 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 67 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 68
Helvis 1:2b5f79285a3e 69 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
Helvis 1:2b5f79285a3e 70
Helvis 1:2b5f79285a3e 71 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 72 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 73
Helvis 1:2b5f79285a3e 74 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 75
Helvis 1:2b5f79285a3e 76 control();
Helvis 1:2b5f79285a3e 77
Helvis 1:2b5f79285a3e 78 }
Helvis 1:2b5f79285a3e 79 }
Helvis 1:2b5f79285a3e 80 /**
Helvis 1:2b5f79285a3e 81 * Eine Feldstrecke mit überprüfung der Ziellinie fahren
Helvis 1:2b5f79285a3e 82 */
Helvis 1:2b5f79285a3e 83 void Motion::scanMove() {
Helvis 1:2b5f79285a3e 84
Helvis 1:2b5f79285a3e 85 controller.counterReset();
Helvis 1:2b5f79285a3e 86 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 87 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 88 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 89 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 90
Helvis 1:2b5f79285a3e 91 speedLeft = 50.0f;
Helvis 1:2b5f79285a3e 92 speedRight = -50.0f;
Helvis 1:2b5f79285a3e 93
Helvis 1:2b5f79285a3e 94 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 95 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 96
Helvis 4:932eb2d29206 97 while ((countsL - countsLOld) < 1565 || (countsR - countsROld) > -1565) {
Helvis 1:2b5f79285a3e 98
Helvis 1:2b5f79285a3e 99 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 100 countsR = counterRight.read();
Helvis 3:076dd7ec7eb4 101 distanceC = irSensorC.readC();
Helvis 1:2b5f79285a3e 102
Helvis 1:2b5f79285a3e 103 if (enableMotorDriver == 0) {
Helvis 1:2b5f79285a3e 104 enableMotorDriver = 1;
Helvis 1:2b5f79285a3e 105 }
Helvis 1:2b5f79285a3e 106
Helvis 3:076dd7ec7eb4 107 if (lineSensor.read() == 1.0f) {
Helvis 1:2b5f79285a3e 108 line = 1;
Helvis 1:2b5f79285a3e 109 }
Helvis 1:2b5f79285a3e 110
Helvis 1:2b5f79285a3e 111 control();
Helvis 1:2b5f79285a3e 112
Helvis 3:076dd7ec7eb4 113 if (distanceC < 40.0f) {
Helvis 3:076dd7ec7eb4 114 stop();
Helvis 3:076dd7ec7eb4 115 break;
Helvis 3:076dd7ec7eb4 116 }
Helvis 3:076dd7ec7eb4 117
Helvis 1:2b5f79285a3e 118 }
Helvis 1:2b5f79285a3e 119
Helvis 1:2b5f79285a3e 120 }
Helvis 1:2b5f79285a3e 121 /**
Helvis 1:2b5f79285a3e 122 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 123 */
Helvis 1:2b5f79285a3e 124 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 125
Helvis 1:2b5f79285a3e 126 stop();
Helvis 1:2b5f79285a3e 127 controller.counterReset();
Helvis 1:2b5f79285a3e 128 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 129 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 130 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 131 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 132
Helvis 1:2b5f79285a3e 133 controller.setDesiredSpeedLeft(-20.0f);
Helvis 1:2b5f79285a3e 134 controller.setDesiredSpeedRight(-20.0f);
Helvis 1:2b5f79285a3e 135
Helvis 1:2b5f79285a3e 136 while ((countsL - countsLOld) > -862 || (countsR - countsROld) > -862) {
Helvis 1:2b5f79285a3e 137
Helvis 1:2b5f79285a3e 138 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 139 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 140
Helvis 1:2b5f79285a3e 141 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 142 }
Helvis 1:2b5f79285a3e 143
Helvis 1:2b5f79285a3e 144 stop();
Helvis 1:2b5f79285a3e 145 }
Helvis 1:2b5f79285a3e 146
Helvis 1:2b5f79285a3e 147 /**
Helvis 1:2b5f79285a3e 148 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 149 */
Helvis 1:2b5f79285a3e 150 void Motion::rotateR() {
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 2:f898adf2d817 172 /**
Helvis 2:f898adf2d817 173 * Links abbiegen
Helvis 2:f898adf2d817 174 */
Helvis 3:076dd7ec7eb4 175 void Motion::turnL() {
Helvis 2:f898adf2d817 176
Helvis 2:f898adf2d817 177 controller.counterReset();
Helvis 2:f898adf2d817 178 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 179 countsROld = counterRight.read();
Helvis 2:f898adf2d817 180 countsL = counterLeft.read();
Helvis 2:f898adf2d817 181 countsR = counterRight.read();
Helvis 2:f898adf2d817 182
Helvis 2:f898adf2d817 183 controller.setDesiredSpeedLeft(17.0f);
Helvis 2:f898adf2d817 184 controller.setDesiredSpeedRight(-83.0f);
Helvis 2:f898adf2d817 185
Helvis 2:f898adf2d817 186 while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
Helvis 2:f898adf2d817 187
Helvis 2:f898adf2d817 188 countsL = counterLeft.read();
Helvis 2:f898adf2d817 189 countsR = counterRight.read();
Helvis 2:f898adf2d817 190
Helvis 2:f898adf2d817 191 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 192 }
Helvis 2:f898adf2d817 193 }
Helvis 2:f898adf2d817 194 /**
Helvis 2:f898adf2d817 195 * Rechts abbiegen
Helvis 2:f898adf2d817 196 */
Helvis 3:076dd7ec7eb4 197 void Motion::turnR() {
Helvis 2:f898adf2d817 198
Helvis 2:f898adf2d817 199 controller.counterReset();
Helvis 2:f898adf2d817 200 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 201 countsROld = counterRight.read();
Helvis 2:f898adf2d817 202 countsL = counterLeft.read();
Helvis 2:f898adf2d817 203 countsR = counterRight.read();
Helvis 2:f898adf2d817 204
Helvis 2:f898adf2d817 205 controller.setDesiredSpeedLeft(83.0f);
Helvis 2:f898adf2d817 206 controller.setDesiredSpeedRight(-17.0f);
Helvis 2:f898adf2d817 207
Helvis 2:f898adf2d817 208 while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
Helvis 2:f898adf2d817 209
Helvis 2:f898adf2d817 210 countsL = counterLeft.read();
Helvis 2:f898adf2d817 211 countsR = counterRight.read();
Helvis 2:f898adf2d817 212
Helvis 2:f898adf2d817 213 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 3:076dd7ec7eb4 214 }
Helvis 2:f898adf2d817 215 }
Helvis 1:2b5f79285a3e 216 /**
Helvis 1:2b5f79285a3e 217 * Motor Stop
Helvis 1:2b5f79285a3e 218 */
Helvis 1:2b5f79285a3e 219 void Motion::stop() {
Helvis 1:2b5f79285a3e 220
Helvis 1:2b5f79285a3e 221 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 222 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 223
Helvis 1:2b5f79285a3e 224 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 225 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 226
Helvis 1:2b5f79285a3e 227 waitStop = 0;
Helvis 1:2b5f79285a3e 228 while( waitStop < ticks) {
Helvis 1:2b5f79285a3e 229 waitStop+= 1;
Helvis 1:2b5f79285a3e 230 }
Helvis 1:2b5f79285a3e 231 }
Helvis 1:2b5f79285a3e 232 /**
Helvis 1:2b5f79285a3e 233 * 180° Rotation
Helvis 1:2b5f79285a3e 234 */
Helvis 1:2b5f79285a3e 235 void Motion::rotate180() {
Helvis 1:2b5f79285a3e 236
Helvis 1:2b5f79285a3e 237 stop();
Helvis 1:2b5f79285a3e 238 controller.counterReset();
Helvis 1:2b5f79285a3e 239 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 240 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 241 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 242 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 243
Helvis 1:2b5f79285a3e 244 controller.setDesiredSpeedLeft(-50.0f);
Helvis 1:2b5f79285a3e 245 controller.setDesiredSpeedRight(-50.0f);
Helvis 1:2b5f79285a3e 246
Helvis 1:2b5f79285a3e 247 while ((countsL - countsLOld) > -1723 || (countsR - countsROld) > -1723) {
Helvis 1:2b5f79285a3e 248
Helvis 1:2b5f79285a3e 249 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 250 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 251
Helvis 1:2b5f79285a3e 252 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 253 }
Helvis 1:2b5f79285a3e 254
Helvis 1:2b5f79285a3e 255 stop();
Helvis 1:2b5f79285a3e 256 }
Helvis 1:2b5f79285a3e 257
Helvis 1:2b5f79285a3e 258
Helvis 1:2b5f79285a3e 259 void Motion::control() {
Helvis 1:2b5f79285a3e 260
Helvis 4:932eb2d29206 261 float wallLeft = 80.0f;
Helvis 4:932eb2d29206 262 float wallRight = 80.0f;
Helvis 1:2b5f79285a3e 263
Helvis 1:2b5f79285a3e 264 distanceL = irSensorL.readL();
Helvis 1:2b5f79285a3e 265 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 266
Helvis 4:932eb2d29206 267 if (distanceL < wallLeft && distanceR < wallRight) {
Helvis 4:932eb2d29206 268 /*if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 4:932eb2d29206 269
Helvis 4:932eb2d29206 270 errorP = LEFT_MID_VAL - distanceL;
Helvis 4:932eb2d29206 271
Helvis 4:932eb2d29206 272 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 4:932eb2d29206 273
Helvis 4:932eb2d29206 274 errorP = distanceR - RIGHT_MID_VAL;
Helvis 4:932eb2d29206 275 }*/
Helvis 1:2b5f79285a3e 276
Helvis 4:932eb2d29206 277 errorP = distanceL - distanceR + 2.30f;
Helvis 1:2b5f79285a3e 278 }else{
Helvis 1:2b5f79285a3e 279
Helvis 1:2b5f79285a3e 280 errorP = 0;
Helvis 1:2b5f79285a3e 281 }
Helvis 1:2b5f79285a3e 282
Helvis 1:2b5f79285a3e 283 errorD = errorP - oldErrorP;
Helvis 1:2b5f79285a3e 284
Helvis 1:2b5f79285a3e 285 oldErrorP = errorP;
Helvis 1:2b5f79285a3e 286
Helvis 1:2b5f79285a3e 287 totalError = KP*errorP + KD*errorD;
Helvis 1:2b5f79285a3e 288
Helvis 1:2b5f79285a3e 289 controller.setDesiredSpeedLeft(speedLeft + totalError);
Helvis 1:2b5f79285a3e 290 controller.setDesiredSpeedRight(speedRight + totalError);
Helvis 1:2b5f79285a3e 291 }
Helvis 1:2b5f79285a3e 292
Helvis 1:2b5f79285a3e 293 void Motion::runTask(int task) {
Helvis 1:2b5f79285a3e 294
Helvis 1:2b5f79285a3e 295 switch(task) {
Helvis 1:2b5f79285a3e 296
Helvis 1:2b5f79285a3e 297 case 1:
Helvis 1:2b5f79285a3e 298 move();
Helvis 1:2b5f79285a3e 299 break;
Helvis 1:2b5f79285a3e 300 case 2:
Helvis 1:2b5f79285a3e 301 rotateL();
Helvis 1:2b5f79285a3e 302 break;
Helvis 1:2b5f79285a3e 303 case 3:
Helvis 1:2b5f79285a3e 304 rotateR();
Helvis 1:2b5f79285a3e 305 break;
Helvis 1:2b5f79285a3e 306 case 4:
Helvis 1:2b5f79285a3e 307 moveFast();
Helvis 1:2b5f79285a3e 308 break;
Helvis 1:2b5f79285a3e 309 case 5:
Helvis 1:2b5f79285a3e 310 stop();
Helvis 1:2b5f79285a3e 311 break;
Helvis 1:2b5f79285a3e 312 }
Helvis 1:2b5f79285a3e 313 }
Helvis 1:2b5f79285a3e 314
Helvis 1:2b5f79285a3e 315 int Motion::finish() {
Helvis 1:2b5f79285a3e 316
Helvis 1:2b5f79285a3e 317 return line;
Helvis 1:2b5f79285a3e 318 }