Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@3:076dd7ec7eb4, 2018-04-20 (annotated)

Committer:
Helvis
Date:
Fri Apr 20 13:37:21 2018 +0000
Revision:
3:076dd7ec7eb4
Parent:
2:f898adf2d817
Child:
4:932eb2d29206
v0.9.2

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 3:076dd7ec7eb4 9 const float Motion::KP = 3.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 1:2b5f79285a3e 41 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
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 1:2b5f79285a3e 97 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
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 }else{
Helvis 1:2b5f79285a3e 110 line = 0;
Helvis 1:2b5f79285a3e 111 }
Helvis 1:2b5f79285a3e 112
Helvis 1:2b5f79285a3e 113 control();
Helvis 1:2b5f79285a3e 114
Helvis 3:076dd7ec7eb4 115 if (distanceC < 40.0f) {
Helvis 3:076dd7ec7eb4 116 stop();
Helvis 3:076dd7ec7eb4 117 break;
Helvis 3:076dd7ec7eb4 118 }
Helvis 3:076dd7ec7eb4 119
Helvis 1:2b5f79285a3e 120 }
Helvis 1:2b5f79285a3e 121
Helvis 1:2b5f79285a3e 122 }
Helvis 1:2b5f79285a3e 123 /**
Helvis 1:2b5f79285a3e 124 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 125 */
Helvis 1:2b5f79285a3e 126 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 127
Helvis 1:2b5f79285a3e 128 stop();
Helvis 1:2b5f79285a3e 129 controller.counterReset();
Helvis 1:2b5f79285a3e 130 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 131 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 132 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 133 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 134
Helvis 1:2b5f79285a3e 135 controller.setDesiredSpeedLeft(-20.0f);
Helvis 1:2b5f79285a3e 136 controller.setDesiredSpeedRight(-20.0f);
Helvis 1:2b5f79285a3e 137
Helvis 1:2b5f79285a3e 138 while ((countsL - countsLOld) > -862 || (countsR - countsROld) > -862) {
Helvis 1:2b5f79285a3e 139
Helvis 1:2b5f79285a3e 140 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 141 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 142
Helvis 1:2b5f79285a3e 143 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 144 }
Helvis 1:2b5f79285a3e 145
Helvis 1:2b5f79285a3e 146 stop();
Helvis 1:2b5f79285a3e 147 }
Helvis 1:2b5f79285a3e 148
Helvis 1:2b5f79285a3e 149 /**
Helvis 1:2b5f79285a3e 150 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 151 */
Helvis 1:2b5f79285a3e 152 void Motion::rotateR() {
Helvis 1:2b5f79285a3e 153
Helvis 1:2b5f79285a3e 154 stop();
Helvis 1:2b5f79285a3e 155 controller.counterReset();
Helvis 1:2b5f79285a3e 156 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 157 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 158 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 159 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 160
Helvis 1:2b5f79285a3e 161 controller.setDesiredSpeedLeft(20.0f);
Helvis 1:2b5f79285a3e 162 controller.setDesiredSpeedRight(20.0f);
Helvis 1:2b5f79285a3e 163
Helvis 1:2b5f79285a3e 164 while ((countsL - countsLOld) < 862 || (countsR - countsROld) < 862) {
Helvis 1:2b5f79285a3e 165
Helvis 1:2b5f79285a3e 166 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 167 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 168
Helvis 1:2b5f79285a3e 169 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 170 }
Helvis 1:2b5f79285a3e 171
Helvis 1:2b5f79285a3e 172 stop();
Helvis 1:2b5f79285a3e 173 }
Helvis 2:f898adf2d817 174 /**
Helvis 2:f898adf2d817 175 * Links abbiegen
Helvis 2:f898adf2d817 176 */
Helvis 3:076dd7ec7eb4 177 void Motion::turnL() {
Helvis 2:f898adf2d817 178
Helvis 2:f898adf2d817 179 controller.counterReset();
Helvis 2:f898adf2d817 180 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 181 countsROld = counterRight.read();
Helvis 2:f898adf2d817 182 countsL = counterLeft.read();
Helvis 2:f898adf2d817 183 countsR = counterRight.read();
Helvis 2:f898adf2d817 184
Helvis 2:f898adf2d817 185 controller.setDesiredSpeedLeft(17.0f);
Helvis 2:f898adf2d817 186 controller.setDesiredSpeedRight(-83.0f);
Helvis 2:f898adf2d817 187
Helvis 2:f898adf2d817 188 while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
Helvis 2:f898adf2d817 189
Helvis 2:f898adf2d817 190 countsL = counterLeft.read();
Helvis 2:f898adf2d817 191 countsR = counterRight.read();
Helvis 2:f898adf2d817 192
Helvis 2:f898adf2d817 193 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 194 }
Helvis 2:f898adf2d817 195 }
Helvis 2:f898adf2d817 196 /**
Helvis 2:f898adf2d817 197 * Rechts abbiegen
Helvis 2:f898adf2d817 198 */
Helvis 3:076dd7ec7eb4 199 void Motion::turnR() {
Helvis 2:f898adf2d817 200
Helvis 2:f898adf2d817 201 controller.counterReset();
Helvis 2:f898adf2d817 202 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 203 countsROld = counterRight.read();
Helvis 2:f898adf2d817 204 countsL = counterLeft.read();
Helvis 2:f898adf2d817 205 countsR = counterRight.read();
Helvis 2:f898adf2d817 206
Helvis 2:f898adf2d817 207 controller.setDesiredSpeedLeft(83.0f);
Helvis 2:f898adf2d817 208 controller.setDesiredSpeedRight(-17.0f);
Helvis 2:f898adf2d817 209
Helvis 2:f898adf2d817 210 while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
Helvis 2:f898adf2d817 211
Helvis 2:f898adf2d817 212 countsL = counterLeft.read();
Helvis 2:f898adf2d817 213 countsR = counterRight.read();
Helvis 2:f898adf2d817 214
Helvis 2:f898adf2d817 215 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 3:076dd7ec7eb4 216 }
Helvis 2:f898adf2d817 217 }
Helvis 1:2b5f79285a3e 218 /**
Helvis 1:2b5f79285a3e 219 * Motor Stop
Helvis 1:2b5f79285a3e 220 */
Helvis 1:2b5f79285a3e 221 void Motion::stop() {
Helvis 1:2b5f79285a3e 222
Helvis 1:2b5f79285a3e 223 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 224 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 225
Helvis 1:2b5f79285a3e 226 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 227 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 228
Helvis 1:2b5f79285a3e 229 waitStop = 0;
Helvis 1:2b5f79285a3e 230 while( waitStop < ticks) {
Helvis 1:2b5f79285a3e 231 waitStop+= 1;
Helvis 1:2b5f79285a3e 232 }
Helvis 1:2b5f79285a3e 233 }
Helvis 1:2b5f79285a3e 234 /**
Helvis 1:2b5f79285a3e 235 * 180° Rotation
Helvis 1:2b5f79285a3e 236 */
Helvis 1:2b5f79285a3e 237 void Motion::rotate180() {
Helvis 1:2b5f79285a3e 238
Helvis 1:2b5f79285a3e 239 stop();
Helvis 1:2b5f79285a3e 240 controller.counterReset();
Helvis 1:2b5f79285a3e 241 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 242 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 243 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 244 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 245
Helvis 1:2b5f79285a3e 246 controller.setDesiredSpeedLeft(-50.0f);
Helvis 1:2b5f79285a3e 247 controller.setDesiredSpeedRight(-50.0f);
Helvis 1:2b5f79285a3e 248
Helvis 1:2b5f79285a3e 249 while ((countsL - countsLOld) > -1723 || (countsR - countsROld) > -1723) {
Helvis 1:2b5f79285a3e 250
Helvis 1:2b5f79285a3e 251 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 252 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 253
Helvis 1:2b5f79285a3e 254 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 255 }
Helvis 1:2b5f79285a3e 256
Helvis 1:2b5f79285a3e 257 stop();
Helvis 1:2b5f79285a3e 258 }
Helvis 1:2b5f79285a3e 259
Helvis 1:2b5f79285a3e 260
Helvis 1:2b5f79285a3e 261 void Motion::control() {
Helvis 1:2b5f79285a3e 262
Helvis 1:2b5f79285a3e 263 //float wallLeft = 70.0f;
Helvis 1:2b5f79285a3e 264 //float wallRight = 70.0f;
Helvis 1:2b5f79285a3e 265
Helvis 1:2b5f79285a3e 266 distanceL = irSensorL.readL();
Helvis 1:2b5f79285a3e 267 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 268
Helvis 1:2b5f79285a3e 269 if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 1:2b5f79285a3e 270
Helvis 1:2b5f79285a3e 271 errorP = LEFT_MID_VAL - distanceL;
Helvis 1:2b5f79285a3e 272
Helvis 1:2b5f79285a3e 273 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 1:2b5f79285a3e 274
Helvis 1:2b5f79285a3e 275 errorP = distanceR - RIGHT_MID_VAL;
Helvis 1:2b5f79285a3e 276
Helvis 1:2b5f79285a3e 277 }else{
Helvis 1:2b5f79285a3e 278
Helvis 1:2b5f79285a3e 279 errorP = 0;
Helvis 1:2b5f79285a3e 280 }
Helvis 1:2b5f79285a3e 281
Helvis 1:2b5f79285a3e 282 errorD = errorP - oldErrorP;
Helvis 1:2b5f79285a3e 283
Helvis 1:2b5f79285a3e 284 oldErrorP = errorP;
Helvis 1:2b5f79285a3e 285
Helvis 1:2b5f79285a3e 286 totalError = KP*errorP + KD*errorD;
Helvis 1:2b5f79285a3e 287
Helvis 1:2b5f79285a3e 288 controller.setDesiredSpeedLeft(speedLeft + totalError);
Helvis 1:2b5f79285a3e 289 controller.setDesiredSpeedRight(speedRight + totalError);
Helvis 1:2b5f79285a3e 290 }
Helvis 1:2b5f79285a3e 291
Helvis 1:2b5f79285a3e 292 void Motion::runTask(int task) {
Helvis 1:2b5f79285a3e 293
Helvis 1:2b5f79285a3e 294 switch(task) {
Helvis 1:2b5f79285a3e 295
Helvis 1:2b5f79285a3e 296 case 1:
Helvis 1:2b5f79285a3e 297 move();
Helvis 1:2b5f79285a3e 298 break;
Helvis 1:2b5f79285a3e 299 case 2:
Helvis 1:2b5f79285a3e 300 rotateL();
Helvis 1:2b5f79285a3e 301 break;
Helvis 1:2b5f79285a3e 302 case 3:
Helvis 1:2b5f79285a3e 303 rotateR();
Helvis 1:2b5f79285a3e 304 break;
Helvis 1:2b5f79285a3e 305 case 4:
Helvis 1:2b5f79285a3e 306 moveFast();
Helvis 1:2b5f79285a3e 307 break;
Helvis 1:2b5f79285a3e 308 case 5:
Helvis 1:2b5f79285a3e 309 stop();
Helvis 1:2b5f79285a3e 310 break;
Helvis 1:2b5f79285a3e 311 }
Helvis 1:2b5f79285a3e 312 }
Helvis 1:2b5f79285a3e 313
Helvis 1:2b5f79285a3e 314 int Motion::finish() {
Helvis 1:2b5f79285a3e 315
Helvis 1:2b5f79285a3e 316 return line;
Helvis 1:2b5f79285a3e 317 }