Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@35:5a4e1a87b3da, 2018-06-30 (annotated)

Committer:
luethale
Date:
Sat Jun 30 09:49:06 2018 +0000
Revision:
35:5a4e1a87b3da
Parent:
34:0587c0943263
Child:
36:99f60052c746
Nacht_der_Technik

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