Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@30:bdb8c92434a0, 2018-05-22 (annotated)

Committer:
Helvis
Date:
Tue May 22 16:40:36 2018 +0000
Revision:
30:bdb8c92434a0
Parent:
28:b7ce1e3bf08b
Child:
31:2c54f8304ef5
v2.2awdadajdbakuhefkwuefgluaszfguzs

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