Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@27:f111ba194412, 2018-05-15 (annotated)

Committer:
luethale
Date:
Tue May 15 18:34:39 2018 +0000
Revision:
27:f111ba194412
Parent:
26:f964408401fa
Child:
28:b7ce1e3bf08b
v2.2b

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