Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@17:8a8758bfe3c5, 2018-05-07 (annotated)

Committer:
Helvis
Date:
Mon May 07 18:28:34 2018 +0000
Revision:
17:8a8758bfe3c5
Parent:
16:c5b864804632
Child:
18:3309329d5f42
+ Beschleunigung nach Sackgasse; + Saubere Abbremsung nach Sackgasse; - komische Beschleunigung (vermutlich Regler);

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 6:4868f789c223 7 const float Motion::LEFT_MID_VAL = 41.73f; //44.73
Helvis 6:4868f789c223 8 const float Motion::RIGHT_MID_VAL = 44.03f; //47.03
Helvis 6:4868f789c223 9 const float Motion::KP = 2.5;
Helvis 5:e2c0a4388d85 10 const float Motion::KD = 0.0f;
Helvis 6:4868f789c223 11 const int Motion::MOVE_DIST = 1605;
Helvis 15:0ae61d3d199f 12 const float Motion::MOVE_SPEED = 50.0f;
Helvis 13:845e49f20426 13 const float Motion::SCAN_SPEED = 50.0f;
Helvis 13:845e49f20426 14 const float Motion::ROTATE_SPEED = 80.0f;
Helvis 17:8a8758bfe3c5 15 const float Motion::ACCEL_CONST = 2.5f; //2.212f
Helvis 1:2b5f79285a3e 16
Helvis 1:2b5f79285a3e 17
Helvis 1:2b5f79285a3e 18 Motion::Motion(Controller& controller, EncoderCounter& counterLeft,
Helvis 1:2b5f79285a3e 19 EncoderCounter& counterRight, IRSensor& irSensorL,
Helvis 1:2b5f79285a3e 20 IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
Helvis 1:2b5f79285a3e 21 DigitalOut& enableMotorDriver) :
Helvis 1:2b5f79285a3e 22 controller(controller), counterLeft(counterLeft),
Helvis 1:2b5f79285a3e 23 counterRight(counterRight), irSensorL(irSensorL),
Helvis 1:2b5f79285a3e 24 irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
Helvis 1:2b5f79285a3e 25 enableMotorDriver(enableMotorDriver) {}
Helvis 1:2b5f79285a3e 26
Helvis 1:2b5f79285a3e 27 Motion::~Motion() {}
Helvis 1:2b5f79285a3e 28
Helvis 1:2b5f79285a3e 29 /**
Helvis 1:2b5f79285a3e 30 * Eine Feldstrecke druchführen
Helvis 1:2b5f79285a3e 31 */
Helvis 1:2b5f79285a3e 32 void Motion::move() {
Helvis 1:2b5f79285a3e 33
Helvis 1:2b5f79285a3e 34 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 35 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 36 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 37 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 38
Helvis 11:2960fc540616 39 speedLeft = MOVE_SPEED;
Helvis 11:2960fc540616 40 speedRight = -MOVE_SPEED;
Helvis 1:2b5f79285a3e 41
Helvis 15:0ae61d3d199f 42 //controller.setDesiredSpeedLeft(actSpeed);
Helvis 15:0ae61d3d199f 43 //controller.setDesiredSpeedRight(-actSpeed);
Helvis 1:2b5f79285a3e 44
Helvis 16:c5b864804632 45 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 16:c5b864804632 46
Helvis 16:c5b864804632 47 /*int partytime;
Helvis 16:c5b864804632 48
Helvis 16:c5b864804632 49 if (avgSpeed == 0.0f) partytime = 1;
Helvis 16:c5b864804632 50 else partytime = 0;*/
Helvis 16:c5b864804632 51
Helvis 13:845e49f20426 52 t.start();
Helvis 13:845e49f20426 53
Helvis 6:4868f789c223 54 while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
Helvis 1:2b5f79285a3e 55
Helvis 1:2b5f79285a3e 56 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 57 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 58 distanceC = irSensorC.readC();
Helvis 12:75d0291a9785 59 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 60 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 61
Helvis 1:2b5f79285a3e 62 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 16:c5b864804632 63
Helvis 16:c5b864804632 64 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 16:c5b864804632 65
Helvis 16:c5b864804632 66 // if (partytime == 1)
Helvis 15:0ae61d3d199f 67 accel(MOVE_SPEED);
Helvis 12:75d0291a9785 68 control();
Helvis 12:75d0291a9785 69 /*
Helvis 13:845e49f20426 70 accel(MOVE_SPEED);
Helvis 12:75d0291a9785 71 controller.setDesiredSpeedLeft(actSpeed);
Helvis 12:75d0291a9785 72 controller.setDesiredSpeedRight(-actSpeed);
Helvis 12:75d0291a9785 73 */
Helvis 5:e2c0a4388d85 74
Helvis 5:e2c0a4388d85 75 if ((distanceC) < 40.0f) {
Helvis 5:e2c0a4388d85 76 countsLOld = countsL;
Helvis 5:e2c0a4388d85 77 countsROld = countsR;
Helvis 5:e2c0a4388d85 78 while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
Helvis 5:e2c0a4388d85 79 countsL = counterLeft.read();
Helvis 5:e2c0a4388d85 80 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 81 }
Helvis 5:e2c0a4388d85 82 stop();
Helvis 5:e2c0a4388d85 83 break;
Helvis 12:75d0291a9785 84 }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 130.0f) && (distanceC > 40.0f)) {
Helvis 5:e2c0a4388d85 85 countsLOld += 500;
Helvis 5:e2c0a4388d85 86 countsROld += 500;
Helvis 12:75d0291a9785 87 }/* else if ( distanceL < 80.0f || distanceR < 80.0f ) {
Helvis 12:75d0291a9785 88 countsLOld = countsL;
Helvis 12:75d0291a9785 89 countsROld = countsR;
Helvis 12:75d0291a9785 90 while ((countsL - countsLOld) < MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) {
Helvis 12:75d0291a9785 91 countsL = counterLeft.read();
Helvis 12:75d0291a9785 92 countsR = counterRight.read();
Helvis 12:75d0291a9785 93 }
Helvis 12:75d0291a9785 94 stop();
Helvis 12:75d0291a9785 95 break;
Helvis 12:75d0291a9785 96 }*/
Helvis 1:2b5f79285a3e 97
Helvis 1:2b5f79285a3e 98 }
Helvis 13:845e49f20426 99
Helvis 13:845e49f20426 100 t.stop();
Helvis 13:845e49f20426 101 t.reset();
Helvis 1:2b5f79285a3e 102 }
Helvis 1:2b5f79285a3e 103 /**
Helvis 1:2b5f79285a3e 104 * Eine Feldstrecke mit höherer Geschwindigkeit fahren
Helvis 1:2b5f79285a3e 105 */
Helvis 1:2b5f79285a3e 106 void Motion::moveFast() {
Helvis 1:2b5f79285a3e 107
Helvis 1:2b5f79285a3e 108 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 109 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 110 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 111 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 112
Helvis 1:2b5f79285a3e 113 speedLeft = 100.0f;
Helvis 1:2b5f79285a3e 114 speedRight = -100.0f;
Helvis 1:2b5f79285a3e 115
Helvis 1:2b5f79285a3e 116 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 117 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 118
Helvis 1:2b5f79285a3e 119 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
Helvis 1:2b5f79285a3e 120
Helvis 1:2b5f79285a3e 121 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 122 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 123 distanceC = irSensorC.readC();
Helvis 1:2b5f79285a3e 124
Helvis 1:2b5f79285a3e 125 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 126
Helvis 1:2b5f79285a3e 127 control();
Helvis 1:2b5f79285a3e 128
Helvis 5:e2c0a4388d85 129 if (distanceC < 40.0f) {
Helvis 5:e2c0a4388d85 130 stop();
Helvis 5:e2c0a4388d85 131 break;
Helvis 5:e2c0a4388d85 132 }
Helvis 1:2b5f79285a3e 133 }
Helvis 1:2b5f79285a3e 134 }
Helvis 1:2b5f79285a3e 135 /**
Helvis 1:2b5f79285a3e 136 * Eine Feldstrecke mit überprüfung der Ziellinie fahren
Helvis 1:2b5f79285a3e 137 */
Helvis 1:2b5f79285a3e 138 void Motion::scanMove() {
Helvis 1:2b5f79285a3e 139
Helvis 16:c5b864804632 140 acceleration = false;
Helvis 17:8a8758bfe3c5 141 deceleration = false;
Helvis 16:c5b864804632 142
Helvis 1:2b5f79285a3e 143 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 144 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 145 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 146 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 147
Helvis 13:845e49f20426 148 speedLeft = SCAN_SPEED;
Helvis 13:845e49f20426 149 speedRight = -SCAN_SPEED;
Helvis 1:2b5f79285a3e 150
Helvis 15:0ae61d3d199f 151 //controller.setDesiredSpeedLeft(actSpeed);
Helvis 15:0ae61d3d199f 152 //controller.setDesiredSpeedRight(-actSpeed);
Helvis 1:2b5f79285a3e 153
Helvis 5:e2c0a4388d85 154 distanceC = irSensorC.readC();
Helvis 13:845e49f20426 155 t.start();
Helvis 5:e2c0a4388d85 156
Helvis 6:4868f789c223 157 while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
Helvis 1:2b5f79285a3e 158
Helvis 1:2b5f79285a3e 159 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 160 countsR = counterRight.read();
Helvis 3:076dd7ec7eb4 161 distanceC = irSensorC.readC();
Helvis 12:75d0291a9785 162 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 163 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 164
Helvis 1:2b5f79285a3e 165 if (enableMotorDriver == 0) {
Helvis 1:2b5f79285a3e 166 enableMotorDriver = 1;
Helvis 1:2b5f79285a3e 167 }
Helvis 1:2b5f79285a3e 168
Helvis 3:076dd7ec7eb4 169 if (lineSensor.read() == 1.0f) {
Helvis 1:2b5f79285a3e 170 line = 1;
Helvis 1:2b5f79285a3e 171 }
Helvis 1:2b5f79285a3e 172
Helvis 13:845e49f20426 173
Helvis 15:0ae61d3d199f 174 accel(SCAN_SPEED);
Helvis 13:845e49f20426 175 control();
Helvis 13:845e49f20426 176
Helvis 13:845e49f20426 177 //controller.setDesiredSpeedLeft(actSpeed);
Helvis 13:845e49f20426 178 //controller.setDesiredSpeedRight(-actSpeed);
Helvis 1:2b5f79285a3e 179
Helvis 5:e2c0a4388d85 180 if ((distanceC) < 40.0f) {
Helvis 5:e2c0a4388d85 181 countsLOld = countsL;
Helvis 5:e2c0a4388d85 182 countsROld = countsR;
Helvis 5:e2c0a4388d85 183 while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
Helvis 5:e2c0a4388d85 184 countsL = counterLeft.read();
Helvis 5:e2c0a4388d85 185 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 186 }
Helvis 3:076dd7ec7eb4 187 stop();
Helvis 3:076dd7ec7eb4 188 break;
Helvis 6:4868f789c223 189 }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
Helvis 5:e2c0a4388d85 190 countsLOld += 500;
Helvis 5:e2c0a4388d85 191 countsROld += 500;
Helvis 12:75d0291a9785 192 }/*else if ( distanceL > 80.0f || distanceR > 80.0f ) {
Helvis 12:75d0291a9785 193 countsLOld = countsL;
Helvis 12:75d0291a9785 194 countsROld = countsR;
Helvis 12:75d0291a9785 195 while ((countsL - countsLOld) < MOVE_DIST*0.5f + 330.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 330.0f) {
Helvis 12:75d0291a9785 196 countsL = counterLeft.read();
Helvis 12:75d0291a9785 197 countsR = counterRight.read();
Helvis 12:75d0291a9785 198 }
Helvis 12:75d0291a9785 199 stop();
Helvis 12:75d0291a9785 200 break;
Helvis 12:75d0291a9785 201 }*/
Helvis 5:e2c0a4388d85 202 }
Helvis 13:845e49f20426 203 t.stop();
Helvis 13:845e49f20426 204 t.reset();
Helvis 1:2b5f79285a3e 205 }
Helvis 1:2b5f79285a3e 206 /**
Helvis 1:2b5f79285a3e 207 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 208 */
Helvis 1:2b5f79285a3e 209 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 210
Helvis 1:2b5f79285a3e 211 stop();
Helvis 1:2b5f79285a3e 212 controller.counterReset();
Helvis 1:2b5f79285a3e 213 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 214 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 215 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 216 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 217
Helvis 12:75d0291a9785 218 controller.setDesiredSpeedLeft(-ROTATE_SPEED);
Helvis 12:75d0291a9785 219 controller.setDesiredSpeedRight(-ROTATE_SPEED);
Helvis 1:2b5f79285a3e 220
Helvis 13:845e49f20426 221 while ((countsL - countsLOld) > -870 || (countsR - countsROld) > -870) {
Helvis 1:2b5f79285a3e 222
Helvis 12:75d0291a9785 223 //accel();
Helvis 12:75d0291a9785 224
Helvis 1:2b5f79285a3e 225 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 226 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 227
Helvis 1:2b5f79285a3e 228 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 12:75d0291a9785 229
Helvis 1:2b5f79285a3e 230 }
Helvis 1:2b5f79285a3e 231
Helvis 1:2b5f79285a3e 232 stop();
Helvis 1:2b5f79285a3e 233 }
Helvis 1:2b5f79285a3e 234
Helvis 1:2b5f79285a3e 235 /**
Helvis 1:2b5f79285a3e 236 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 237 */
Helvis 1:2b5f79285a3e 238 void Motion::rotateR() {
Helvis 1:2b5f79285a3e 239
Helvis 1:2b5f79285a3e 240 stop();
Helvis 1:2b5f79285a3e 241 controller.counterReset();
Helvis 1:2b5f79285a3e 242 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 243 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 244 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 245 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 246
Helvis 12:75d0291a9785 247 controller.setDesiredSpeedLeft(ROTATE_SPEED);
Helvis 12:75d0291a9785 248 controller.setDesiredSpeedRight(ROTATE_SPEED);
Helvis 1:2b5f79285a3e 249
Helvis 13:845e49f20426 250 while ((countsL - countsLOld) < 870 || (countsR - countsROld) < 870) {
Helvis 1:2b5f79285a3e 251
Helvis 12:75d0291a9785 252 //accel();
Helvis 12:75d0291a9785 253
Helvis 1:2b5f79285a3e 254 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 255 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 256
Helvis 1:2b5f79285a3e 257 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 12:75d0291a9785 258
Helvis 1:2b5f79285a3e 259 }
Helvis 1:2b5f79285a3e 260
Helvis 1:2b5f79285a3e 261 stop();
Helvis 1:2b5f79285a3e 262 }
Helvis 2:f898adf2d817 263 /**
Helvis 2:f898adf2d817 264 * Links abbiegen
Helvis 2:f898adf2d817 265 */
Helvis 3:076dd7ec7eb4 266 void Motion::turnL() {
Helvis 2:f898adf2d817 267
Helvis 2:f898adf2d817 268 controller.counterReset();
Helvis 2:f898adf2d817 269 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 270 countsROld = counterRight.read();
Helvis 2:f898adf2d817 271 countsL = counterLeft.read();
Helvis 2:f898adf2d817 272 countsR = counterRight.read();
Helvis 2:f898adf2d817 273
Helvis 2:f898adf2d817 274 controller.setDesiredSpeedLeft(17.0f);
Helvis 2:f898adf2d817 275 controller.setDesiredSpeedRight(-83.0f);
Helvis 2:f898adf2d817 276
Helvis 2:f898adf2d817 277 while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
Helvis 2:f898adf2d817 278
Helvis 2:f898adf2d817 279 countsL = counterLeft.read();
Helvis 2:f898adf2d817 280 countsR = counterRight.read();
Helvis 2:f898adf2d817 281
Helvis 2:f898adf2d817 282 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 283 }
Helvis 2:f898adf2d817 284 }
Helvis 2:f898adf2d817 285 /**
Helvis 2:f898adf2d817 286 * Rechts abbiegen
Helvis 2:f898adf2d817 287 */
Helvis 3:076dd7ec7eb4 288 void Motion::turnR() {
Helvis 2:f898adf2d817 289
Helvis 2:f898adf2d817 290 controller.counterReset();
Helvis 2:f898adf2d817 291 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 292 countsROld = counterRight.read();
Helvis 2:f898adf2d817 293 countsL = counterLeft.read();
Helvis 2:f898adf2d817 294 countsR = counterRight.read();
Helvis 2:f898adf2d817 295
Helvis 2:f898adf2d817 296 controller.setDesiredSpeedLeft(83.0f);
Helvis 2:f898adf2d817 297 controller.setDesiredSpeedRight(-17.0f);
Helvis 2:f898adf2d817 298
Helvis 2:f898adf2d817 299 while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
Helvis 2:f898adf2d817 300
Helvis 2:f898adf2d817 301 countsL = counterLeft.read();
Helvis 2:f898adf2d817 302 countsR = counterRight.read();
Helvis 2:f898adf2d817 303
Helvis 2:f898adf2d817 304 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 3:076dd7ec7eb4 305 }
Helvis 2:f898adf2d817 306 }
Helvis 1:2b5f79285a3e 307 /**
Helvis 1:2b5f79285a3e 308 * Motor Stop
Helvis 1:2b5f79285a3e 309 */
Helvis 1:2b5f79285a3e 310 void Motion::stop() {
Helvis 1:2b5f79285a3e 311
Helvis 1:2b5f79285a3e 312 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 313 controller.setDesiredSpeedRight(0.0f);
Helvis 16:c5b864804632 314 actSpeed = 0.0f;
Helvis 1:2b5f79285a3e 315
Helvis 1:2b5f79285a3e 316 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 317 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 318
Helvis 1:2b5f79285a3e 319 waitStop = 0;
Helvis 1:2b5f79285a3e 320 while( waitStop < ticks) {
Helvis 16:c5b864804632 321 controller.setDesiredSpeedLeft(0.0f);
Helvis 16:c5b864804632 322 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 323 waitStop+= 1;
Helvis 1:2b5f79285a3e 324 }
Helvis 1:2b5f79285a3e 325 }
Helvis 1:2b5f79285a3e 326 /**
Helvis 1:2b5f79285a3e 327 * 180° Rotation
Helvis 1:2b5f79285a3e 328 */
Helvis 1:2b5f79285a3e 329 void Motion::rotate180() {
Helvis 13:845e49f20426 330 //1723
Helvis 1:2b5f79285a3e 331 stop();
Helvis 1:2b5f79285a3e 332 controller.counterReset();
Helvis 1:2b5f79285a3e 333 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 334 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 335 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 336 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 337
Helvis 13:845e49f20426 338 //controller.setDesiredSpeedLeft(-ROTATE_SPEED);
Helvis 13:845e49f20426 339 //controller.setDesiredSpeedRight(-ROTATE_SPEED);
Helvis 13:845e49f20426 340
Helvis 13:845e49f20426 341 t.start();
Helvis 1:2b5f79285a3e 342
Helvis 13:845e49f20426 343 while ((countsL - countsLOld) > -900 || (countsR - countsROld) > -900) {
Helvis 1:2b5f79285a3e 344
Helvis 13:845e49f20426 345 actSpeed = 3.5f * t.read()*60.0f;
Helvis 13:845e49f20426 346
Helvis 13:845e49f20426 347 controller.setDesiredSpeedLeft(-actSpeed);
Helvis 13:845e49f20426 348 controller.setDesiredSpeedRight(-actSpeed);
Helvis 12:75d0291a9785 349
Helvis 1:2b5f79285a3e 350 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 351 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 352
Helvis 1:2b5f79285a3e 353 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 354 }
Helvis 1:2b5f79285a3e 355
Helvis 13:845e49f20426 356 t.reset();
Helvis 13:845e49f20426 357
Helvis 13:845e49f20426 358 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 13:845e49f20426 359
Helvis 13:845e49f20426 360 while ((countsL - countsLOld) > -1720 || (countsR - countsROld) > -1720) {
Helvis 13:845e49f20426 361
Helvis 13:845e49f20426 362 actSpeed = avgSpeed + (-3.5f * t.read()*60.0f);
Helvis 13:845e49f20426 363
Helvis 13:845e49f20426 364 controller.setDesiredSpeedLeft(-actSpeed);
Helvis 13:845e49f20426 365 controller.setDesiredSpeedRight(-actSpeed);
Helvis 13:845e49f20426 366
Helvis 13:845e49f20426 367 countsL = counterLeft.read();
Helvis 13:845e49f20426 368 countsR = counterRight.read();
Helvis 13:845e49f20426 369
Helvis 13:845e49f20426 370 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 13:845e49f20426 371 }
Helvis 13:845e49f20426 372 t.stop();
Helvis 13:845e49f20426 373 t.reset();
Helvis 1:2b5f79285a3e 374 stop();
Helvis 1:2b5f79285a3e 375 }
Helvis 1:2b5f79285a3e 376
Helvis 1:2b5f79285a3e 377
Helvis 1:2b5f79285a3e 378 void Motion::control() {
Helvis 1:2b5f79285a3e 379
Helvis 12:75d0291a9785 380 float wallLeft = 48.73f;
Helvis 12:75d0291a9785 381 float wallRight = 51.03f;
Helvis 1:2b5f79285a3e 382
Helvis 12:75d0291a9785 383 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 384 distanceR = irSensorR.readR();
Helvis 12:75d0291a9785 385
Helvis 6:4868f789c223 386
Helvis 12:75d0291a9785 387 if (distanceL < wallLeft && distanceR < wallRight) {
Helvis 12:75d0291a9785 388
Helvis 12:75d0291a9785 389 errorP = distanceL - distanceR + 2.30f;
Helvis 1:2b5f79285a3e 390
Helvis 12:75d0291a9785 391 }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 12:75d0291a9785 392
Helvis 12:75d0291a9785 393 errorP = distanceL - LEFT_MID_VAL;
Helvis 12:75d0291a9785 394
Helvis 12:75d0291a9785 395 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 12:75d0291a9785 396
Helvis 12:75d0291a9785 397 errorP = RIGHT_MID_VAL - distanceR;
Helvis 12:75d0291a9785 398
Helvis 12:75d0291a9785 399 }else{
Helvis 12:75d0291a9785 400
Helvis 12:75d0291a9785 401 errorP = 0;
Helvis 12:75d0291a9785 402 errorD = 0;
Helvis 12:75d0291a9785 403 }
Helvis 12:75d0291a9785 404
Helvis 12:75d0291a9785 405 errorD = errorP - oldErrorP;
Helvis 12:75d0291a9785 406
Helvis 12:75d0291a9785 407 oldErrorP = errorP;
Helvis 12:75d0291a9785 408
Helvis 12:75d0291a9785 409 if (abs(errorP) < 80.0f) {
Helvis 12:75d0291a9785 410 totalError = KP*errorP + KD*errorD;
Helvis 12:75d0291a9785 411 }else{
Helvis 12:75d0291a9785 412 totalError = 0;
Helvis 12:75d0291a9785 413 }
Helvis 12:75d0291a9785 414
Helvis 13:845e49f20426 415 controller.setDesiredSpeedLeft(actSpeed - totalError);
Helvis 13:845e49f20426 416 controller.setDesiredSpeedRight(-actSpeed - totalError);
Helvis 1:2b5f79285a3e 417 }
Helvis 1:2b5f79285a3e 418
Helvis 12:75d0291a9785 419 void Motion::runTask(int path[],int task, bool reverse) {
Helvis 1:2b5f79285a3e 420
Helvis 12:75d0291a9785 421 switch(path[task]) {
Helvis 12:75d0291a9785 422
Helvis 12:75d0291a9785 423 case 1:
Helvis 17:8a8758bfe3c5 424 if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) {
Helvis 17:8a8758bfe3c5 425 acceleration = true;
Helvis 17:8a8758bfe3c5 426 deceleration = false;
Helvis 17:8a8758bfe3c5 427 }else if (reverse == false && path[task+1] == path[task] && path[task-1] != path[task]) {
Helvis 12:75d0291a9785 428 acceleration = true;
Helvis 15:0ae61d3d199f 429 deceleration = false;
Helvis 12:75d0291a9785 430 }else{
Helvis 17:8a8758bfe3c5 431 acceleration = false;
Helvis 15:0ae61d3d199f 432 deceleration = false;
Helvis 17:8a8758bfe3c5 433 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 12:75d0291a9785 434 }
Helvis 12:75d0291a9785 435
Helvis 17:8a8758bfe3c5 436 if (reverse == true && path[task-1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
Helvis 17:8a8758bfe3c5 437 deceleration = true;
Helvis 17:8a8758bfe3c5 438 acceleration = false;
Helvis 17:8a8758bfe3c5 439 }else if (reverse == false && path[task+1] != path[task] && avgSpeed > 2.4f*MOVE_SPEED) {
Helvis 12:75d0291a9785 440 deceleration = true;
Helvis 15:0ae61d3d199f 441 acceleration = false;
Helvis 12:75d0291a9785 442 }else{
Helvis 12:75d0291a9785 443 deceleration = false;
Helvis 12:75d0291a9785 444 }
Helvis 12:75d0291a9785 445
Helvis 17:8a8758bfe3c5 446 //printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration);
Helvis 17:8a8758bfe3c5 447 //printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed);
Helvis 17:8a8758bfe3c5 448
Helvis 17:8a8758bfe3c5 449
Helvis 17:8a8758bfe3c5 450
Helvis 12:75d0291a9785 451 move();
Helvis 12:75d0291a9785 452 break;
Helvis 12:75d0291a9785 453 case 2:
Helvis 12:75d0291a9785 454 rotateL();
Helvis 12:75d0291a9785 455 break;
Helvis 12:75d0291a9785 456 case 3:
Helvis 12:75d0291a9785 457 rotateR();
Helvis 12:75d0291a9785 458 break;
Helvis 12:75d0291a9785 459 case 4:
Helvis 12:75d0291a9785 460 moveFast();
Helvis 12:75d0291a9785 461 break;
Helvis 12:75d0291a9785 462 case 5:
Helvis 12:75d0291a9785 463 stop();
Helvis 12:75d0291a9785 464 break;
Helvis 12:75d0291a9785 465 }
Helvis 1:2b5f79285a3e 466 }
Helvis 1:2b5f79285a3e 467
Helvis 1:2b5f79285a3e 468 int Motion::finish() {
Helvis 12:75d0291a9785 469
Helvis 12:75d0291a9785 470 return line;
Helvis 1:2b5f79285a3e 471 }
Helvis 11:2960fc540616 472
Helvis 12:75d0291a9785 473
Helvis 13:845e49f20426 474 void Motion::accel(float targetSpeed) {
Helvis 13:845e49f20426 475
Helvis 13:845e49f20426 476
Helvis 12:75d0291a9785 477
Helvis 12:75d0291a9785 478 avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
Helvis 12:75d0291a9785 479 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 12:75d0291a9785 480
Helvis 17:8a8758bfe3c5 481 if ( avgSpeed < targetSpeed && deceleration == false) {
Helvis 12:75d0291a9785 482
Helvis 16:c5b864804632 483 actSpeed = ACCEL_CONST * t.read()*60.0f;
Helvis 16:c5b864804632 484
Helvis 16:c5b864804632 485 }else if (avgSpeed < targetSpeed*2.6f && acceleration == true) {
Helvis 12:75d0291a9785 486
Helvis 16:c5b864804632 487 actSpeed = 2.5f * t.read()*60.0f;
Helvis 16:c5b864804632 488
Helvis 16:c5b864804632 489 }else if ( avgSpeed > targetSpeed && deceleration == true) {
Helvis 11:2960fc540616 490
Helvis 15:0ae61d3d199f 491 actSpeed = targetSpeed*2.6f - ACCEL_CONST * t.read()*60.0f;
Helvis 12:75d0291a9785 492 }
Helvis 12:75d0291a9785 493
Helvis 12:75d0291a9785 494
Helvis 12:75d0291a9785 495 }
Helvis 12:75d0291a9785 496