Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@2:f898adf2d817, 2018-04-19 (annotated)

Committer:
Helvis
Date:
Thu Apr 19 11:26:51 2018 +0000
Revision:
2:f898adf2d817
Parent:
1:2b5f79285a3e
Child:
3:076dd7ec7eb4
v0.9.1:  +Methoden rechts/links abbiegen

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 1:2b5f79285a3e 7 const float Motion::LEFT_MID_VAL = 45.0f;
Helvis 1:2b5f79285a3e 8 const float Motion::RIGHT_MID_VAL = 45.0f;
Helvis 1:2b5f79285a3e 9 const float Motion::KP = 2.5f;
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 1:2b5f79285a3e 101
Helvis 1:2b5f79285a3e 102 if (enableMotorDriver == 0) {
Helvis 1:2b5f79285a3e 103 enableMotorDriver = 1;
Helvis 1:2b5f79285a3e 104 }
Helvis 1:2b5f79285a3e 105
Helvis 1:2b5f79285a3e 106 if (lineSensor.read() > 0.9f) {
Helvis 1:2b5f79285a3e 107 line = 1;
Helvis 1:2b5f79285a3e 108 }else{
Helvis 1:2b5f79285a3e 109 line = 0;
Helvis 1:2b5f79285a3e 110 }
Helvis 1:2b5f79285a3e 111
Helvis 1:2b5f79285a3e 112 control();
Helvis 1:2b5f79285a3e 113
Helvis 1:2b5f79285a3e 114 }
Helvis 1:2b5f79285a3e 115
Helvis 1:2b5f79285a3e 116 }
Helvis 1:2b5f79285a3e 117 /**
Helvis 1:2b5f79285a3e 118 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 119 */
Helvis 1:2b5f79285a3e 120 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 121
Helvis 1:2b5f79285a3e 122 stop();
Helvis 1:2b5f79285a3e 123 controller.counterReset();
Helvis 1:2b5f79285a3e 124 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 125 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 126 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 127 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 128
Helvis 1:2b5f79285a3e 129 controller.setDesiredSpeedLeft(-20.0f);
Helvis 1:2b5f79285a3e 130 controller.setDesiredSpeedRight(-20.0f);
Helvis 1:2b5f79285a3e 131
Helvis 1:2b5f79285a3e 132 while ((countsL - countsLOld) > -862 || (countsR - countsROld) > -862) {
Helvis 1:2b5f79285a3e 133
Helvis 1:2b5f79285a3e 134 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 135 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 136
Helvis 1:2b5f79285a3e 137 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 138 }
Helvis 1:2b5f79285a3e 139
Helvis 1:2b5f79285a3e 140 stop();
Helvis 1:2b5f79285a3e 141 }
Helvis 1:2b5f79285a3e 142
Helvis 1:2b5f79285a3e 143 /**
Helvis 1:2b5f79285a3e 144 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 145 */
Helvis 1:2b5f79285a3e 146 void Motion::rotateR() {
Helvis 1:2b5f79285a3e 147
Helvis 1:2b5f79285a3e 148 stop();
Helvis 1:2b5f79285a3e 149 controller.counterReset();
Helvis 1:2b5f79285a3e 150 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 151 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 152 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 153 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 154
Helvis 1:2b5f79285a3e 155 controller.setDesiredSpeedLeft(20.0f);
Helvis 1:2b5f79285a3e 156 controller.setDesiredSpeedRight(20.0f);
Helvis 1:2b5f79285a3e 157
Helvis 1:2b5f79285a3e 158 while ((countsL - countsLOld) < 862 || (countsR - countsROld) < 862) {
Helvis 1:2b5f79285a3e 159
Helvis 1:2b5f79285a3e 160 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 161 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 162
Helvis 1:2b5f79285a3e 163 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 164 }
Helvis 1:2b5f79285a3e 165
Helvis 1:2b5f79285a3e 166 stop();
Helvis 1:2b5f79285a3e 167 }
Helvis 2:f898adf2d817 168 /**
Helvis 2:f898adf2d817 169 * Links abbiegen
Helvis 2:f898adf2d817 170 */
Helvis 2:f898adf2d817 171 void Motion::turnL() {
Helvis 2:f898adf2d817 172
Helvis 2:f898adf2d817 173
Helvis 2:f898adf2d817 174 controller.counterReset();
Helvis 2:f898adf2d817 175 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 176 countsROld = counterRight.read();
Helvis 2:f898adf2d817 177 countsL = counterLeft.read();
Helvis 2:f898adf2d817 178 countsR = counterRight.read();
Helvis 2:f898adf2d817 179
Helvis 2:f898adf2d817 180 controller.setDesiredSpeedLeft(50.0f);
Helvis 2:f898adf2d817 181 controller.setDesiredSpeedRight(-50.0f);
Helvis 2:f898adf2d817 182
Helvis 2:f898adf2d817 183 while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) {
Helvis 2:f898adf2d817 184
Helvis 2:f898adf2d817 185 countsL = counterLeft.read();
Helvis 2:f898adf2d817 186 countsR = counterRight.read();
Helvis 2:f898adf2d817 187
Helvis 2:f898adf2d817 188 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 189 }
Helvis 2:f898adf2d817 190
Helvis 2:f898adf2d817 191 controller.counterReset();
Helvis 2:f898adf2d817 192 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 193 countsROld = counterRight.read();
Helvis 2:f898adf2d817 194 countsL = counterLeft.read();
Helvis 2:f898adf2d817 195 countsR = counterRight.read();
Helvis 2:f898adf2d817 196
Helvis 2:f898adf2d817 197 controller.setDesiredSpeedLeft(17.0f);
Helvis 2:f898adf2d817 198 controller.setDesiredSpeedRight(-83.0f);
Helvis 2:f898adf2d817 199
Helvis 2:f898adf2d817 200 while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
Helvis 2:f898adf2d817 201
Helvis 2:f898adf2d817 202 countsL = counterLeft.read();
Helvis 2:f898adf2d817 203 countsR = counterRight.read();
Helvis 2:f898adf2d817 204
Helvis 2:f898adf2d817 205 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 206 }
Helvis 2:f898adf2d817 207
Helvis 2:f898adf2d817 208 controller.counterReset();
Helvis 2:f898adf2d817 209 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 210 countsROld = counterRight.read();
Helvis 2:f898adf2d817 211 countsL = counterLeft.read();
Helvis 2:f898adf2d817 212 countsR = counterRight.read();
Helvis 2:f898adf2d817 213
Helvis 2:f898adf2d817 214 controller.setDesiredSpeedLeft(50.0f);
Helvis 2:f898adf2d817 215 controller.setDesiredSpeedRight(-50.0f);
Helvis 2:f898adf2d817 216
Helvis 2:f898adf2d817 217 while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) {
Helvis 2:f898adf2d817 218
Helvis 2:f898adf2d817 219 countsL = counterLeft.read();
Helvis 2:f898adf2d817 220 countsR = counterRight.read();
Helvis 2:f898adf2d817 221
Helvis 2:f898adf2d817 222 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 223 }
Helvis 2:f898adf2d817 224 }
Helvis 2:f898adf2d817 225 /**
Helvis 2:f898adf2d817 226 * Rechts abbiegen
Helvis 2:f898adf2d817 227 */
Helvis 2:f898adf2d817 228 void Motion::turnR() {
Helvis 2:f898adf2d817 229
Helvis 2:f898adf2d817 230
Helvis 2:f898adf2d817 231 controller.counterReset();
Helvis 2:f898adf2d817 232 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 233 countsROld = counterRight.read();
Helvis 2:f898adf2d817 234 countsL = counterLeft.read();
Helvis 2:f898adf2d817 235 countsR = counterRight.read();
Helvis 2:f898adf2d817 236
Helvis 2:f898adf2d817 237 controller.setDesiredSpeedLeft(50.0f);
Helvis 2:f898adf2d817 238 controller.setDesiredSpeedRight(-50.0f);
Helvis 2:f898adf2d817 239
Helvis 2:f898adf2d817 240 while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) {
Helvis 2:f898adf2d817 241
Helvis 2:f898adf2d817 242 countsL = counterLeft.read();
Helvis 2:f898adf2d817 243 countsR = counterRight.read();
Helvis 2:f898adf2d817 244
Helvis 2:f898adf2d817 245 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 246 }
Helvis 2:f898adf2d817 247
Helvis 2:f898adf2d817 248 controller.counterReset();
Helvis 2:f898adf2d817 249 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 250 countsROld = counterRight.read();
Helvis 2:f898adf2d817 251 countsL = counterLeft.read();
Helvis 2:f898adf2d817 252 countsR = counterRight.read();
Helvis 2:f898adf2d817 253
Helvis 2:f898adf2d817 254 controller.setDesiredSpeedLeft(83.0f);
Helvis 2:f898adf2d817 255 controller.setDesiredSpeedRight(-17.0f);
Helvis 2:f898adf2d817 256
Helvis 2:f898adf2d817 257 while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
Helvis 2:f898adf2d817 258
Helvis 2:f898adf2d817 259 countsL = counterLeft.read();
Helvis 2:f898adf2d817 260 countsR = counterRight.read();
Helvis 2:f898adf2d817 261
Helvis 2:f898adf2d817 262 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 263 }
Helvis 2:f898adf2d817 264
Helvis 2:f898adf2d817 265 controller.counterReset();
Helvis 2:f898adf2d817 266 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 267 countsROld = counterRight.read();
Helvis 2:f898adf2d817 268 countsL = counterLeft.read();
Helvis 2:f898adf2d817 269 countsR = counterRight.read();
Helvis 2:f898adf2d817 270
Helvis 2:f898adf2d817 271 controller.setDesiredSpeedLeft(50.0f);
Helvis 2:f898adf2d817 272 controller.setDesiredSpeedRight(-50.0f);
Helvis 2:f898adf2d817 273
Helvis 2:f898adf2d817 274 while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) {
Helvis 2:f898adf2d817 275
Helvis 2:f898adf2d817 276 countsL = counterLeft.read();
Helvis 2:f898adf2d817 277 countsR = counterRight.read();
Helvis 2:f898adf2d817 278
Helvis 2:f898adf2d817 279 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 280 }
Helvis 2:f898adf2d817 281 }
Helvis 1:2b5f79285a3e 282 /**
Helvis 1:2b5f79285a3e 283 * Motor Stop
Helvis 1:2b5f79285a3e 284 */
Helvis 1:2b5f79285a3e 285 void Motion::stop() {
Helvis 1:2b5f79285a3e 286
Helvis 1:2b5f79285a3e 287 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 288 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 289
Helvis 1:2b5f79285a3e 290 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 291 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 292
Helvis 1:2b5f79285a3e 293 waitStop = 0;
Helvis 1:2b5f79285a3e 294 while( waitStop < ticks) {
Helvis 1:2b5f79285a3e 295 waitStop+= 1;
Helvis 1:2b5f79285a3e 296 }
Helvis 1:2b5f79285a3e 297 }
Helvis 1:2b5f79285a3e 298 /**
Helvis 1:2b5f79285a3e 299 * 180° Rotation
Helvis 1:2b5f79285a3e 300 */
Helvis 1:2b5f79285a3e 301 void Motion::rotate180() {
Helvis 1:2b5f79285a3e 302
Helvis 1:2b5f79285a3e 303 stop();
Helvis 1:2b5f79285a3e 304 controller.counterReset();
Helvis 1:2b5f79285a3e 305 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 306 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 307 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 308 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 309
Helvis 1:2b5f79285a3e 310 controller.setDesiredSpeedLeft(-50.0f);
Helvis 1:2b5f79285a3e 311 controller.setDesiredSpeedRight(-50.0f);
Helvis 1:2b5f79285a3e 312
Helvis 1:2b5f79285a3e 313 while ((countsL - countsLOld) > -1723 || (countsR - countsROld) > -1723) {
Helvis 1:2b5f79285a3e 314
Helvis 1:2b5f79285a3e 315 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 316 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 317
Helvis 1:2b5f79285a3e 318 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 319 }
Helvis 1:2b5f79285a3e 320
Helvis 1:2b5f79285a3e 321 stop();
Helvis 1:2b5f79285a3e 322 }
Helvis 1:2b5f79285a3e 323
Helvis 1:2b5f79285a3e 324
Helvis 1:2b5f79285a3e 325 void Motion::control() {
Helvis 1:2b5f79285a3e 326
Helvis 1:2b5f79285a3e 327 //float wallLeft = 70.0f;
Helvis 1:2b5f79285a3e 328 //float wallRight = 70.0f;
Helvis 1:2b5f79285a3e 329
Helvis 1:2b5f79285a3e 330 distanceL = irSensorL.readL();
Helvis 1:2b5f79285a3e 331 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 332
Helvis 1:2b5f79285a3e 333 if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 1:2b5f79285a3e 334
Helvis 1:2b5f79285a3e 335 errorP = LEFT_MID_VAL - distanceL;
Helvis 1:2b5f79285a3e 336
Helvis 1:2b5f79285a3e 337 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 1:2b5f79285a3e 338
Helvis 1:2b5f79285a3e 339 errorP = distanceR - RIGHT_MID_VAL;
Helvis 1:2b5f79285a3e 340
Helvis 1:2b5f79285a3e 341 }else{
Helvis 1:2b5f79285a3e 342
Helvis 1:2b5f79285a3e 343 errorP = 0;
Helvis 1:2b5f79285a3e 344 }
Helvis 1:2b5f79285a3e 345
Helvis 1:2b5f79285a3e 346 errorD = errorP - oldErrorP;
Helvis 1:2b5f79285a3e 347
Helvis 1:2b5f79285a3e 348 oldErrorP = errorP;
Helvis 1:2b5f79285a3e 349
Helvis 1:2b5f79285a3e 350 totalError = KP*errorP + KD*errorD;
Helvis 1:2b5f79285a3e 351
Helvis 1:2b5f79285a3e 352 controller.setDesiredSpeedLeft(speedLeft + totalError);
Helvis 1:2b5f79285a3e 353 controller.setDesiredSpeedRight(speedRight + totalError);
Helvis 1:2b5f79285a3e 354 }
Helvis 1:2b5f79285a3e 355
Helvis 1:2b5f79285a3e 356 void Motion::runTask(int task) {
Helvis 1:2b5f79285a3e 357
Helvis 1:2b5f79285a3e 358 switch(task) {
Helvis 1:2b5f79285a3e 359
Helvis 1:2b5f79285a3e 360 case 1:
Helvis 1:2b5f79285a3e 361 move();
Helvis 1:2b5f79285a3e 362 break;
Helvis 1:2b5f79285a3e 363 case 2:
Helvis 1:2b5f79285a3e 364 rotateL();
Helvis 1:2b5f79285a3e 365 break;
Helvis 1:2b5f79285a3e 366 case 3:
Helvis 1:2b5f79285a3e 367 rotateR();
Helvis 1:2b5f79285a3e 368 break;
Helvis 1:2b5f79285a3e 369 case 4:
Helvis 1:2b5f79285a3e 370 moveFast();
Helvis 1:2b5f79285a3e 371 break;
Helvis 1:2b5f79285a3e 372 case 5:
Helvis 1:2b5f79285a3e 373 stop();
Helvis 1:2b5f79285a3e 374 break;
Helvis 1:2b5f79285a3e 375 }
Helvis 1:2b5f79285a3e 376 }
Helvis 1:2b5f79285a3e 377
Helvis 1:2b5f79285a3e 378 int Motion::finish() {
Helvis 1:2b5f79285a3e 379
Helvis 1:2b5f79285a3e 380 return line;
Helvis 1:2b5f79285a3e 381 }