Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@13:845e49f20426, 2018-05-04 (annotated)

Committer:
Helvis
Date:
Fri May 04 19:10:36 2018 +0000
Revision:
13:845e49f20426
Parent:
12:75d0291a9785
Child:
14:2926932e26df
v0.9.7: Test Version von ISCHKAPUTT

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