Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@25:8854037f6336, 2018-05-15 (annotated)

Committer:
Helvis
Date:
Tue May 15 15:29:02 2018 +0000
Revision:
25:8854037f6336
Parent:
24:11c5fb5280eb
Child:
26:f964408401fa
v2.1b

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 19:6cd6cc5c8b4c 7 const float Motion::LEFT_MID_VAL = 40.73f; //44.73
Helvis 19:6cd6cc5c8b4c 8 const float Motion::RIGHT_MID_VAL = 43.03f; //47.03
Helvis 6:4868f789c223 9 const float Motion::KP = 2.5;
Helvis 5:e2c0a4388d85 10 const float Motion::KD = 0.0f;
Helvis 6:4868f789c223 11 const int Motion::MOVE_DIST = 1605;
Helvis 15:0ae61d3d199f 12 const float Motion::MOVE_SPEED = 50.0f;
Helvis 13:845e49f20426 13 const float Motion::SCAN_SPEED = 50.0f;
Helvis 13:845e49f20426 14 const float Motion::ROTATE_SPEED = 80.0f;
Helvis 22:91526c8d15ba 15 const float Motion::ACCEL_CONST = 3.5f; //2.212f
Helvis 1:2b5f79285a3e 16
Helvis 1:2b5f79285a3e 17
Helvis 1:2b5f79285a3e 18 Motion::Motion(Controller& controller, EncoderCounter& counterLeft,
Helvis 1:2b5f79285a3e 19 EncoderCounter& counterRight, IRSensor& irSensorL,
Helvis 1:2b5f79285a3e 20 IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
Helvis 1:2b5f79285a3e 21 DigitalOut& enableMotorDriver) :
Helvis 1:2b5f79285a3e 22 controller(controller), counterLeft(counterLeft),
Helvis 1:2b5f79285a3e 23 counterRight(counterRight), irSensorL(irSensorL),
Helvis 1:2b5f79285a3e 24 irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
Helvis 1:2b5f79285a3e 25 enableMotorDriver(enableMotorDriver) {}
Helvis 1:2b5f79285a3e 26
Helvis 1:2b5f79285a3e 27 Motion::~Motion() {}
Helvis 1:2b5f79285a3e 28
Helvis 1:2b5f79285a3e 29 /**
Helvis 1:2b5f79285a3e 30 * Eine Feldstrecke druchführen
Helvis 1:2b5f79285a3e 31 */
Helvis 1:2b5f79285a3e 32 void Motion::move() {
Helvis 1:2b5f79285a3e 33
Helvis 1:2b5f79285a3e 34 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 35 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 36 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 37 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 38
Helvis 16:c5b864804632 39 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 16:c5b864804632 40
Helvis 13:845e49f20426 41 t.start();
Helvis 13:845e49f20426 42
Helvis 6:4868f789c223 43 while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
Helvis 1:2b5f79285a3e 44
Helvis 1:2b5f79285a3e 45 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 46 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 47 distanceC = irSensorC.readC();
Helvis 12:75d0291a9785 48 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 49 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 50
Helvis 1:2b5f79285a3e 51 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 16:c5b864804632 52
Helvis 16:c5b864804632 53 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 16:c5b864804632 54
Helvis 15:0ae61d3d199f 55 accel(MOVE_SPEED);
Helvis 12:75d0291a9785 56 control();
Helvis 5:e2c0a4388d85 57
Helvis 5:e2c0a4388d85 58 if ((distanceC) < 40.0f) {
Helvis 5:e2c0a4388d85 59 countsLOld = countsL;
Helvis 5:e2c0a4388d85 60 countsROld = countsR;
Helvis 19:6cd6cc5c8b4c 61
Helvis 5:e2c0a4388d85 62 while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
Helvis 5:e2c0a4388d85 63 countsL = counterLeft.read();
Helvis 19:6cd6cc5c8b4c 64 countsR = counterRight.read();
Helvis 19:6cd6cc5c8b4c 65 distanceC = irSensorC.readC();
Helvis 19:6cd6cc5c8b4c 66 if (distanceC > 40.0f) {
Helvis 19:6cd6cc5c8b4c 67 stop();
Helvis 19:6cd6cc5c8b4c 68 break;
Helvis 19:6cd6cc5c8b4c 69 }
Helvis 5:e2c0a4388d85 70 }
Helvis 5:e2c0a4388d85 71 stop();
Helvis 5:e2c0a4388d85 72 break;
Helvis 19:6cd6cc5c8b4c 73
Helvis 19:6cd6cc5c8b4c 74 }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
Helvis 5:e2c0a4388d85 75 countsLOld += 500;
Helvis 19:6cd6cc5c8b4c 76 countsROld += 500;
Helvis 19:6cd6cc5c8b4c 77
Helvis 23:accd07ca2da7 78 }/*else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) {
Helvis 19:6cd6cc5c8b4c 79 countsLOld = counterLeft.read();
Helvis 19:6cd6cc5c8b4c 80 countsROld = counterRight.read();
Helvis 19:6cd6cc5c8b4c 81
Helvis 19:6cd6cc5c8b4c 82 while ((countsL - countsLOld) < MOVE_DIST*0.5f + 816.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 816.0f) {
Helvis 12:75d0291a9785 83 countsL = counterLeft.read();
Helvis 12:75d0291a9785 84 countsR = counterRight.read();
Helvis 19:6cd6cc5c8b4c 85 accel(MOVE_SPEED);
Helvis 19:6cd6cc5c8b4c 86 control();
Helvis 12:75d0291a9785 87 }
Helvis 12:75d0291a9785 88 stop();
Helvis 12:75d0291a9785 89 break;
Helvis 23:accd07ca2da7 90 }*/
Helvis 23:accd07ca2da7 91
Helvis 23:accd07ca2da7 92 }
Helvis 23:accd07ca2da7 93
Helvis 23:accd07ca2da7 94 t.stop();
Helvis 23:accd07ca2da7 95 t.reset();
Helvis 23:accd07ca2da7 96 lastMove = false;
Helvis 23:accd07ca2da7 97 }
Helvis 23:accd07ca2da7 98
Helvis 23:accd07ca2da7 99 /**
Helvis 23:accd07ca2da7 100 * Eine Feldstrecke druchführen
Helvis 23:accd07ca2da7 101 */
Helvis 23:accd07ca2da7 102 void Motion::moveHalf() {
Helvis 23:accd07ca2da7 103
Helvis 23:accd07ca2da7 104 countsLOld = counterLeft.read();
Helvis 23:accd07ca2da7 105 countsROld = counterRight.read();
Helvis 23:accd07ca2da7 106 countsL = counterLeft.read();
Helvis 23:accd07ca2da7 107 countsR = counterRight.read();
Helvis 23:accd07ca2da7 108
Helvis 23:accd07ca2da7 109 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 23:accd07ca2da7 110
Helvis 23:accd07ca2da7 111 t.start();
Helvis 23:accd07ca2da7 112
Helvis 23:accd07ca2da7 113 while ((countsL - countsLOld) < 824 || (countsR - countsROld) > -824) {
Helvis 23:accd07ca2da7 114
Helvis 23:accd07ca2da7 115 countsL = counterLeft.read();
Helvis 23:accd07ca2da7 116 countsR = counterRight.read();
Helvis 23:accd07ca2da7 117 distanceC = irSensorC.readC();
Helvis 23:accd07ca2da7 118 distanceL = irSensorL.readL();
Helvis 23:accd07ca2da7 119 distanceR = irSensorR.readR();
Helvis 23:accd07ca2da7 120
Helvis 23:accd07ca2da7 121 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 23:accd07ca2da7 122
Helvis 23:accd07ca2da7 123 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 23:accd07ca2da7 124
Helvis 23:accd07ca2da7 125 accel(MOVE_SPEED);
Helvis 23:accd07ca2da7 126 control();
Helvis 23:accd07ca2da7 127
Helvis 23:accd07ca2da7 128 if (distanceC < 101.0f && lastMove == false) {
Helvis 23:accd07ca2da7 129 stop();
Helvis 23:accd07ca2da7 130 break;
Helvis 24:11c5fb5280eb 131 }else if (distanceC < 40.0f && lastMove == true) {
Helvis 23:accd07ca2da7 132 stop();
Helvis 23:accd07ca2da7 133 break;
Helvis 19:6cd6cc5c8b4c 134 }
Helvis 1:2b5f79285a3e 135
Helvis 1:2b5f79285a3e 136 }
Helvis 13:845e49f20426 137
Helvis 13:845e49f20426 138 t.stop();
Helvis 13:845e49f20426 139 t.reset();
Helvis 19:6cd6cc5c8b4c 140 lastMove = false;
Helvis 1:2b5f79285a3e 141 }
Helvis 19:6cd6cc5c8b4c 142
Helvis 1:2b5f79285a3e 143 /**
Helvis 1:2b5f79285a3e 144 * Eine Feldstrecke mit überprüfung der Ziellinie fahren
Helvis 1:2b5f79285a3e 145 */
Helvis 1:2b5f79285a3e 146 void Motion::scanMove() {
Helvis 1:2b5f79285a3e 147
Helvis 16:c5b864804632 148 acceleration = false;
Helvis 17:8a8758bfe3c5 149 deceleration = false;
Helvis 22:91526c8d15ba 150 longMove = false;
Helvis 16:c5b864804632 151
Helvis 1:2b5f79285a3e 152 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 153 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 154 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 155 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 156
Helvis 13:845e49f20426 157 t.start();
Helvis 5:e2c0a4388d85 158
Helvis 6:4868f789c223 159 while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
Helvis 1:2b5f79285a3e 160
Helvis 1:2b5f79285a3e 161 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 162 countsR = counterRight.read();
Helvis 3:076dd7ec7eb4 163 distanceC = irSensorC.readC();
Helvis 12:75d0291a9785 164 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 165 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 166
Helvis 1:2b5f79285a3e 167 if (enableMotorDriver == 0) {
Helvis 1:2b5f79285a3e 168 enableMotorDriver = 1;
Helvis 1:2b5f79285a3e 169 }
Helvis 1:2b5f79285a3e 170
Helvis 19:6cd6cc5c8b4c 171 if (lineSensor.read() > 0.85f) {
Helvis 1:2b5f79285a3e 172 line = 1;
Helvis 1:2b5f79285a3e 173 }
Helvis 1:2b5f79285a3e 174
Helvis 13:845e49f20426 175
Helvis 15:0ae61d3d199f 176 accel(SCAN_SPEED);
Helvis 13:845e49f20426 177 control();
Helvis 13:845e49f20426 178
Helvis 5:e2c0a4388d85 179 if ((distanceC) < 40.0f) {
Helvis 5:e2c0a4388d85 180 countsLOld = countsL;
Helvis 5:e2c0a4388d85 181 countsROld = countsR;
Helvis 5:e2c0a4388d85 182 while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
Helvis 5:e2c0a4388d85 183 countsL = counterLeft.read();
Helvis 5:e2c0a4388d85 184 countsR = counterRight.read();
Helvis 19:6cd6cc5c8b4c 185 if (distanceC > 40.0f) {
Helvis 19:6cd6cc5c8b4c 186 stop();
Helvis 19:6cd6cc5c8b4c 187 break;
Helvis 19:6cd6cc5c8b4c 188 }
Helvis 5:e2c0a4388d85 189 }
Helvis 23:accd07ca2da7 190
Helvis 3:076dd7ec7eb4 191 stop();
Helvis 23:accd07ca2da7 192 break;
Helvis 23:accd07ca2da7 193
Helvis 6:4868f789c223 194 }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
Helvis 5:e2c0a4388d85 195 countsLOld += 500;
Helvis 5:e2c0a4388d85 196 countsROld += 500;
Helvis 23:accd07ca2da7 197 }
Helvis 23:accd07ca2da7 198 }
Helvis 23:accd07ca2da7 199
Helvis 13:845e49f20426 200 t.stop();
Helvis 13:845e49f20426 201 t.reset();
Helvis 19:6cd6cc5c8b4c 202 lastMove = false;
Helvis 1:2b5f79285a3e 203 }
Helvis 1:2b5f79285a3e 204 /**
Helvis 1:2b5f79285a3e 205 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 206 */
Helvis 1:2b5f79285a3e 207 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 208
Helvis 1:2b5f79285a3e 209 stop();
Helvis 1:2b5f79285a3e 210 controller.counterReset();
Helvis 1:2b5f79285a3e 211 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 212 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 213 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 214 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 215
Helvis 12:75d0291a9785 216 controller.setDesiredSpeedLeft(-ROTATE_SPEED);
Helvis 12:75d0291a9785 217 controller.setDesiredSpeedRight(-ROTATE_SPEED);
Helvis 1:2b5f79285a3e 218
Helvis 13:845e49f20426 219 while ((countsL - countsLOld) > -870 || (countsR - countsROld) > -870) {
Helvis 1:2b5f79285a3e 220
Helvis 12:75d0291a9785 221 //accel();
Helvis 12:75d0291a9785 222
Helvis 1:2b5f79285a3e 223 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 224 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 225
Helvis 1:2b5f79285a3e 226 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 12:75d0291a9785 227
Helvis 1:2b5f79285a3e 228 }
Helvis 1:2b5f79285a3e 229
Helvis 1:2b5f79285a3e 230 stop();
Helvis 1:2b5f79285a3e 231 }
Helvis 1:2b5f79285a3e 232
Helvis 1:2b5f79285a3e 233 /**
Helvis 1:2b5f79285a3e 234 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 235 */
Helvis 1:2b5f79285a3e 236 void Motion::rotateR() {
Helvis 1:2b5f79285a3e 237
Helvis 1:2b5f79285a3e 238 stop();
Helvis 1:2b5f79285a3e 239 controller.counterReset();
Helvis 1:2b5f79285a3e 240 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 241 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 242 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 243 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 244
Helvis 12:75d0291a9785 245 controller.setDesiredSpeedLeft(ROTATE_SPEED);
Helvis 12:75d0291a9785 246 controller.setDesiredSpeedRight(ROTATE_SPEED);
Helvis 1:2b5f79285a3e 247
Helvis 13:845e49f20426 248 while ((countsL - countsLOld) < 870 || (countsR - countsROld) < 870) {
Helvis 1:2b5f79285a3e 249
Helvis 12:75d0291a9785 250 //accel();
Helvis 12:75d0291a9785 251
Helvis 1:2b5f79285a3e 252 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 253 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 254
Helvis 1:2b5f79285a3e 255 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 12:75d0291a9785 256
Helvis 1:2b5f79285a3e 257 }
Helvis 1:2b5f79285a3e 258
Helvis 1:2b5f79285a3e 259 stop();
Helvis 1:2b5f79285a3e 260 }
Helvis 2:f898adf2d817 261 /**
Helvis 2:f898adf2d817 262 * Links abbiegen
Helvis 2:f898adf2d817 263 */
Helvis 3:076dd7ec7eb4 264 void Motion::turnL() {
Helvis 2:f898adf2d817 265
Helvis 2:f898adf2d817 266 controller.counterReset();
Helvis 2:f898adf2d817 267 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 268 countsROld = counterRight.read();
Helvis 2:f898adf2d817 269 countsL = counterLeft.read();
Helvis 2:f898adf2d817 270 countsR = counterRight.read();
Helvis 2:f898adf2d817 271
Helvis 2:f898adf2d817 272 controller.setDesiredSpeedLeft(17.0f);
Helvis 2:f898adf2d817 273 controller.setDesiredSpeedRight(-83.0f);
Helvis 2:f898adf2d817 274
Helvis 2:f898adf2d817 275 while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
Helvis 2:f898adf2d817 276
Helvis 2:f898adf2d817 277 countsL = counterLeft.read();
Helvis 2:f898adf2d817 278 countsR = counterRight.read();
Helvis 2:f898adf2d817 279
Helvis 2:f898adf2d817 280 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 281 }
Helvis 2:f898adf2d817 282 }
Helvis 2:f898adf2d817 283 /**
Helvis 2:f898adf2d817 284 * Rechts abbiegen
Helvis 2:f898adf2d817 285 */
Helvis 3:076dd7ec7eb4 286 void Motion::turnR() {
Helvis 2:f898adf2d817 287
Helvis 2:f898adf2d817 288 controller.counterReset();
Helvis 2:f898adf2d817 289 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 290 countsROld = counterRight.read();
Helvis 2:f898adf2d817 291 countsL = counterLeft.read();
Helvis 2:f898adf2d817 292 countsR = counterRight.read();
Helvis 2:f898adf2d817 293
Helvis 2:f898adf2d817 294 controller.setDesiredSpeedLeft(83.0f);
Helvis 2:f898adf2d817 295 controller.setDesiredSpeedRight(-17.0f);
Helvis 2:f898adf2d817 296
Helvis 2:f898adf2d817 297 while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
Helvis 2:f898adf2d817 298
Helvis 2:f898adf2d817 299 countsL = counterLeft.read();
Helvis 2:f898adf2d817 300 countsR = counterRight.read();
Helvis 2:f898adf2d817 301
Helvis 2:f898adf2d817 302 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 3:076dd7ec7eb4 303 }
Helvis 2:f898adf2d817 304 }
Helvis 1:2b5f79285a3e 305 /**
Helvis 1:2b5f79285a3e 306 * Motor Stop
Helvis 1:2b5f79285a3e 307 */
Helvis 1:2b5f79285a3e 308 void Motion::stop() {
Helvis 1:2b5f79285a3e 309
Helvis 1:2b5f79285a3e 310 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 311 controller.setDesiredSpeedRight(0.0f);
Helvis 16:c5b864804632 312 actSpeed = 0.0f;
Helvis 1:2b5f79285a3e 313
Helvis 1:2b5f79285a3e 314 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 315 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 316
Helvis 1:2b5f79285a3e 317 waitStop = 0;
Helvis 1:2b5f79285a3e 318 while( waitStop < ticks) {
Helvis 16:c5b864804632 319 controller.setDesiredSpeedLeft(0.0f);
Helvis 16:c5b864804632 320 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 321 waitStop+= 1;
Helvis 1:2b5f79285a3e 322 }
Helvis 1:2b5f79285a3e 323 }
Helvis 1:2b5f79285a3e 324 /**
Helvis 1:2b5f79285a3e 325 * 180° Rotation
Helvis 1:2b5f79285a3e 326 */
Helvis 1:2b5f79285a3e 327 void Motion::rotate180() {
Helvis 13:845e49f20426 328 //1723
Helvis 1:2b5f79285a3e 329 stop();
Helvis 1:2b5f79285a3e 330 controller.counterReset();
Helvis 1:2b5f79285a3e 331 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 332 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 333 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 334 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 335
Helvis 13:845e49f20426 336 //controller.setDesiredSpeedLeft(-ROTATE_SPEED);
Helvis 13:845e49f20426 337 //controller.setDesiredSpeedRight(-ROTATE_SPEED);
Helvis 13:845e49f20426 338
Helvis 13:845e49f20426 339 t.start();
Helvis 1:2b5f79285a3e 340
Helvis 13:845e49f20426 341 while ((countsL - countsLOld) > -900 || (countsR - countsROld) > -900) {
Helvis 1:2b5f79285a3e 342
Helvis 13:845e49f20426 343 actSpeed = 3.5f * t.read()*60.0f;
Helvis 13:845e49f20426 344
Helvis 13:845e49f20426 345 controller.setDesiredSpeedLeft(-actSpeed);
Helvis 13:845e49f20426 346 controller.setDesiredSpeedRight(-actSpeed);
Helvis 12:75d0291a9785 347
Helvis 1:2b5f79285a3e 348 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 349 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 350
Helvis 1:2b5f79285a3e 351 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 352 }
Helvis 1:2b5f79285a3e 353
Helvis 13:845e49f20426 354 t.reset();
Helvis 13:845e49f20426 355
Helvis 13:845e49f20426 356 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 13:845e49f20426 357
Helvis 13:845e49f20426 358 while ((countsL - countsLOld) > -1720 || (countsR - countsROld) > -1720) {
Helvis 13:845e49f20426 359
Helvis 13:845e49f20426 360 actSpeed = avgSpeed + (-3.5f * t.read()*60.0f);
Helvis 13:845e49f20426 361
Helvis 13:845e49f20426 362 controller.setDesiredSpeedLeft(-actSpeed);
Helvis 13:845e49f20426 363 controller.setDesiredSpeedRight(-actSpeed);
Helvis 13:845e49f20426 364
Helvis 13:845e49f20426 365 countsL = counterLeft.read();
Helvis 13:845e49f20426 366 countsR = counterRight.read();
Helvis 13:845e49f20426 367
Helvis 13:845e49f20426 368 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 13:845e49f20426 369 }
Helvis 13:845e49f20426 370 t.stop();
Helvis 13:845e49f20426 371 t.reset();
Helvis 1:2b5f79285a3e 372 stop();
Helvis 1:2b5f79285a3e 373 }
Helvis 1:2b5f79285a3e 374
Helvis 1:2b5f79285a3e 375
Helvis 1:2b5f79285a3e 376 void Motion::control() {
Helvis 1:2b5f79285a3e 377
Helvis 24:11c5fb5280eb 378 float wallLeft = 47.0f; //48.73
Helvis 24:11c5fb5280eb 379 float wallRight = 44.0f; //51.03
Helvis 1:2b5f79285a3e 380
Helvis 12:75d0291a9785 381 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 382 distanceR = irSensorR.readR();
Helvis 12:75d0291a9785 383
Helvis 6:4868f789c223 384
Helvis 12:75d0291a9785 385 if (distanceL < wallLeft && distanceR < wallRight) {
Helvis 12:75d0291a9785 386
Helvis 24:11c5fb5280eb 387 errorP = distanceL - distanceR - 3.0f;
Helvis 1:2b5f79285a3e 388
Helvis 24:11c5fb5280eb 389 }/* if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 12:75d0291a9785 390
Helvis 12:75d0291a9785 391 errorP = distanceL - LEFT_MID_VAL;
Helvis 24:11c5fb5280eb 392
Helvis 12:75d0291a9785 393 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 12:75d0291a9785 394
Helvis 12:75d0291a9785 395 errorP = RIGHT_MID_VAL - distanceR;
Helvis 12:75d0291a9785 396
Helvis 24:11c5fb5280eb 397 }*/else if (distanceL < wallLeft && distanceR > wallRight) {
Helvis 24:11c5fb5280eb 398
Helvis 24:11c5fb5280eb 399 errorP = distanceL - wallLeft;
Helvis 24:11c5fb5280eb 400
Helvis 24:11c5fb5280eb 401 }else if (distanceL < wallLeft+15.0f && distanceL > wallLeft && distanceR > wallRight) {
Helvis 24:11c5fb5280eb 402
Helvis 24:11c5fb5280eb 403 errorP = distanceL - wallLeft;
Helvis 24:11c5fb5280eb 404
Helvis 24:11c5fb5280eb 405 }else if (distanceL > wallLeft && distanceR < wallRight) {
Helvis 24:11c5fb5280eb 406
Helvis 24:11c5fb5280eb 407 errorP = wallRight - distanceR;
Helvis 24:11c5fb5280eb 408
Helvis 24:11c5fb5280eb 409 }else if (distanceR < wallRight+15.0f && distanceL > wallLeft && distanceR > wallRight) {
Helvis 24:11c5fb5280eb 410
Helvis 24:11c5fb5280eb 411 errorP = wallRight - distanceR;
Helvis 24:11c5fb5280eb 412
Helvis 12:75d0291a9785 413 }else{
Helvis 12:75d0291a9785 414
Helvis 12:75d0291a9785 415 errorP = 0;
Helvis 12:75d0291a9785 416 errorD = 0;
Helvis 12:75d0291a9785 417 }
Helvis 12:75d0291a9785 418
Helvis 12:75d0291a9785 419 errorD = errorP - oldErrorP;
Helvis 12:75d0291a9785 420
Helvis 12:75d0291a9785 421 oldErrorP = errorP;
Helvis 12:75d0291a9785 422
Helvis 25:8854037f6336 423 if (abs(errorP) > 1.0f && abs(errorP) < 80.0f) {
Helvis 12:75d0291a9785 424 totalError = KP*errorP + KD*errorD;
Helvis 21:41997651337a 425 }/*else{
Helvis 12:75d0291a9785 426 totalError = 0;
Helvis 21:41997651337a 427 }*/
Helvis 12:75d0291a9785 428
Helvis 13:845e49f20426 429 controller.setDesiredSpeedLeft(actSpeed - totalError);
Helvis 13:845e49f20426 430 controller.setDesiredSpeedRight(-actSpeed - totalError);
Helvis 1:2b5f79285a3e 431 }
Helvis 1:2b5f79285a3e 432
Helvis 18:3309329d5f42 433 void Motion::runTask(int path[],int task, bool reverse, int junction) {
Helvis 1:2b5f79285a3e 434
Helvis 12:75d0291a9785 435 switch(path[task]) {
Helvis 12:75d0291a9785 436
Helvis 12:75d0291a9785 437 case 1:
Helvis 23:accd07ca2da7 438
Helvis 23:accd07ca2da7 439 //Acceleration
Helvis 18:3309329d5f42 440 if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) {
Helvis 22:91526c8d15ba 441
Helvis 17:8a8758bfe3c5 442 acceleration = true;
Helvis 21:41997651337a 443 longMove = true;
Helvis 17:8a8758bfe3c5 444 deceleration = false;
Helvis 22:91526c8d15ba 445
Helvis 18:3309329d5f42 446 }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0)) {
Helvis 22:91526c8d15ba 447
Helvis 12:75d0291a9785 448 acceleration = true;
Helvis 21:41997651337a 449 longMove = true;
Helvis 15:0ae61d3d199f 450 deceleration = false;
Helvis 22:91526c8d15ba 451
Helvis 12:75d0291a9785 452 }else{
Helvis 17:8a8758bfe3c5 453 acceleration = false;
Helvis 15:0ae61d3d199f 454 deceleration = false;
Helvis 17:8a8758bfe3c5 455 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 12:75d0291a9785 456 }
Helvis 23:accd07ca2da7 457
Helvis 23:accd07ca2da7 458 //Deceleration
Helvis 18:3309329d5f42 459 if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) {
Helvis 22:91526c8d15ba 460
Helvis 17:8a8758bfe3c5 461 deceleration = true;
Helvis 17:8a8758bfe3c5 462 acceleration = false;
Helvis 19:6cd6cc5c8b4c 463 lastMove = true;
Helvis 21:41997651337a 464 longMove = false;
Helvis 22:91526c8d15ba 465
Helvis 17:8a8758bfe3c5 466 }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
Helvis 22:91526c8d15ba 467
Helvis 12:75d0291a9785 468 deceleration = true;
Helvis 15:0ae61d3d199f 469 acceleration = false;
Helvis 19:6cd6cc5c8b4c 470 lastMove = true;
Helvis 21:41997651337a 471 longMove = false;
Helvis 22:91526c8d15ba 472
Helvis 12:75d0291a9785 473 }else{
Helvis 12:75d0291a9785 474 deceleration = false;
Helvis 12:75d0291a9785 475 }
Helvis 12:75d0291a9785 476
Helvis 17:8a8758bfe3c5 477 //printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration);
Helvis 17:8a8758bfe3c5 478 //printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed);
Helvis 17:8a8758bfe3c5 479
Helvis 12:75d0291a9785 480 move();
Helvis 12:75d0291a9785 481 break;
Helvis 12:75d0291a9785 482 case 2:
Helvis 12:75d0291a9785 483 rotateL();
Helvis 12:75d0291a9785 484 break;
Helvis 12:75d0291a9785 485 case 3:
Helvis 12:75d0291a9785 486 rotateR();
Helvis 12:75d0291a9785 487 break;
Helvis 12:75d0291a9785 488 case 4:
Helvis 23:accd07ca2da7 489
Helvis 23:accd07ca2da7 490 if (reverse == false && path[task] == 4 && path[task+1] == 1) {
Helvis 23:accd07ca2da7 491
Helvis 23:accd07ca2da7 492 acceleration = true;
Helvis 23:accd07ca2da7 493 longMove = true;
Helvis 23:accd07ca2da7 494 deceleration = false;
Helvis 23:accd07ca2da7 495
Helvis 23:accd07ca2da7 496 }else{
Helvis 23:accd07ca2da7 497 acceleration = false;
Helvis 23:accd07ca2da7 498 deceleration = false;
Helvis 23:accd07ca2da7 499 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 23:accd07ca2da7 500 }
Helvis 23:accd07ca2da7 501
Helvis 23:accd07ca2da7 502 if (reverse == false && path[task-1] == 1 && path[task] == 4) {
Helvis 23:accd07ca2da7 503
Helvis 23:accd07ca2da7 504 deceleration = true;
Helvis 23:accd07ca2da7 505 acceleration = false;
Helvis 23:accd07ca2da7 506 lastMove = true;
Helvis 23:accd07ca2da7 507 longMove = false;
Helvis 23:accd07ca2da7 508
Helvis 23:accd07ca2da7 509 }else{
Helvis 23:accd07ca2da7 510 deceleration = false;
Helvis 23:accd07ca2da7 511 }
Helvis 23:accd07ca2da7 512
Helvis 23:accd07ca2da7 513 if (reverse == false && path[task+1] == 0) {
Helvis 23:accd07ca2da7 514
Helvis 23:accd07ca2da7 515 lastMove = true;
Helvis 23:accd07ca2da7 516
Helvis 23:accd07ca2da7 517 }
Helvis 23:accd07ca2da7 518
Helvis 23:accd07ca2da7 519
Helvis 23:accd07ca2da7 520 moveHalf();
Helvis 23:accd07ca2da7 521 break;
Helvis 23:accd07ca2da7 522 case 5:
Helvis 23:accd07ca2da7 523 turnL();
Helvis 23:accd07ca2da7 524 break;
Helvis 23:accd07ca2da7 525 case 6:
Helvis 23:accd07ca2da7 526 turnR();
Helvis 23:accd07ca2da7 527 break;
Helvis 23:accd07ca2da7 528 case 7:
Helvis 23:accd07ca2da7 529 break;
Helvis 12:75d0291a9785 530 }
Helvis 1:2b5f79285a3e 531 }
Helvis 1:2b5f79285a3e 532
Helvis 1:2b5f79285a3e 533 int Motion::finish() {
Helvis 12:75d0291a9785 534
Helvis 12:75d0291a9785 535 return line;
Helvis 1:2b5f79285a3e 536 }
Helvis 11:2960fc540616 537
Helvis 12:75d0291a9785 538
Helvis 13:845e49f20426 539 void Motion::accel(float targetSpeed) {
Helvis 13:845e49f20426 540
Helvis 13:845e49f20426 541
Helvis 12:75d0291a9785 542 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 12:75d0291a9785 543
Helvis 19:6cd6cc5c8b4c 544 if ( avgSpeed < targetSpeed && deceleration == false && acceleration == false) {
Helvis 12:75d0291a9785 545
Helvis 16:c5b864804632 546 actSpeed = ACCEL_CONST * t.read()*60.0f;
Helvis 19:6cd6cc5c8b4c 547
Helvis 21:41997651337a 548 }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
Helvis 21:41997651337a 549
Helvis 25:8854037f6336 550 actSpeed = 51.0f;
Helvis 21:41997651337a 551
Helvis 22:91526c8d15ba 552 }else if (avgSpeed < targetSpeed*2.8f && acceleration == true) {
Helvis 12:75d0291a9785 553
Helvis 22:91526c8d15ba 554 actSpeed = ACCEL_CONST * t.read()*60.0f;
Helvis 16:c5b864804632 555
Helvis 25:8854037f6336 556 }else if (avgSpeed > targetSpeed+5.0f && deceleration == true) {
Helvis 11:2960fc540616 557
Helvis 22:91526c8d15ba 558 actSpeed = targetSpeed*2.8f - ACCEL_CONST * t.read()*60.0f;
Helvis 24:11c5fb5280eb 559
Helvis 24:11c5fb5280eb 560 }else if (avgSpeed < targetSpeed && deceleration == true) {
Helvis 24:11c5fb5280eb 561
Helvis 25:8854037f6336 562 actSpeed = 51.0f;
Helvis 24:11c5fb5280eb 563
Helvis 24:11c5fb5280eb 564 }
Helvis 12:75d0291a9785 565 }
Helvis 12:75d0291a9785 566