Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@31:2c54f8304ef5, 2018-05-22 (annotated)

Committer:
Helvis
Date:
Tue May 22 23:35:53 2018 +0000
Revision:
31:2c54f8304ef5
Parent:
30:bdb8c92434a0
Child:
32:e984b7959cb0
v2.4 R?ckwertsfahren Kreuzung;

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