Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@15:0ae61d3d199f, 2018-05-04 (annotated)

Committer:
Helvis
Date:
Fri May 04 20:36:38 2018 +0000
Revision:
15:0ae61d3d199f
Parent:
14:2926932e26df
Child:
16:c5b864804632
blahblah

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