Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@29:0419f4873807, 2018-05-18 (annotated)

Committer:
Helvis
Date:
Fri May 18 09:30:00 2018 +0000
Revision:
29:0419f4873807
Parent:
28:b7ce1e3bf08b 
v2.2d;

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