Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@28:b7ce1e3bf08b, 2018-05-17 (annotated)

Committer:
Helvis
Date:
Thu May 17 16:39:03 2018 +0000
Revision:
28:b7ce1e3bf08b
Parent:
27:f111ba194412
Child:
29:0419f4873807
Child:
30:bdb8c92434a0
v2.2c;

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