Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@16:c5b864804632, 2018-05-07 (annotated)

Committer:
Helvis
Date:
Mon May 07 14:03:36 2018 +0000
Revision:
16:c5b864804632
Parent:
15:0ae61d3d199f
Child:
17:8a8758bfe3c5
blah 2;

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 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 16:c5b864804632 141
Helvis 1:2b5f79285a3e 142 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 143 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 144 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 145 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 146
Helvis 13:845e49f20426 147 speedLeft = SCAN_SPEED;
Helvis 13:845e49f20426 148 speedRight = -SCAN_SPEED;
Helvis 1:2b5f79285a3e 149
Helvis 15:0ae61d3d199f 150 //controller.setDesiredSpeedLeft(actSpeed);
Helvis 15:0ae61d3d199f 151 //controller.setDesiredSpeedRight(-actSpeed);
Helvis 1:2b5f79285a3e 152
Helvis 5:e2c0a4388d85 153 distanceC = irSensorC.readC();
Helvis 13:845e49f20426 154 t.start();
Helvis 5:e2c0a4388d85 155
Helvis 6:4868f789c223 156 while ((countsL - countsLOld) < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
Helvis 1:2b5f79285a3e 157
Helvis 1:2b5f79285a3e 158 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 159 countsR = counterRight.read();
Helvis 3:076dd7ec7eb4 160 distanceC = irSensorC.readC();
Helvis 12:75d0291a9785 161 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 162 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 163
Helvis 1:2b5f79285a3e 164 if (enableMotorDriver == 0) {
Helvis 1:2b5f79285a3e 165 enableMotorDriver = 1;
Helvis 1:2b5f79285a3e 166 }
Helvis 1:2b5f79285a3e 167
Helvis 3:076dd7ec7eb4 168 if (lineSensor.read() == 1.0f) {
Helvis 1:2b5f79285a3e 169 line = 1;
Helvis 1:2b5f79285a3e 170 }
Helvis 1:2b5f79285a3e 171
Helvis 13:845e49f20426 172
Helvis 15:0ae61d3d199f 173 accel(SCAN_SPEED);
Helvis 13:845e49f20426 174 control();
Helvis 13:845e49f20426 175
Helvis 13:845e49f20426 176 //controller.setDesiredSpeedLeft(actSpeed);
Helvis 13:845e49f20426 177 //controller.setDesiredSpeedRight(-actSpeed);
Helvis 1:2b5f79285a3e 178
Helvis 5:e2c0a4388d85 179 if ((distanceC) < 40.0f) {
Helvis 5:e2c0a4388d85 180 countsLOld = countsL;
Helvis 5:e2c0a4388d85 181 countsROld = countsR;
Helvis 5:e2c0a4388d85 182 while ((countsL - countsLOld) < 70 || (countsR - countsROld) > -70) {
Helvis 5:e2c0a4388d85 183 countsL = counterLeft.read();
Helvis 5:e2c0a4388d85 184 countsR = counterRight.read();
Helvis 5:e2c0a4388d85 185 }
Helvis 3:076dd7ec7eb4 186 stop();
Helvis 3:076dd7ec7eb4 187 break;
Helvis 6:4868f789c223 188 }else if ( ((countsL - countsLOld) >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
Helvis 5:e2c0a4388d85 189 countsLOld += 500;
Helvis 5:e2c0a4388d85 190 countsROld += 500;
Helvis 12:75d0291a9785 191 }/*else if ( distanceL > 80.0f || distanceR > 80.0f ) {
Helvis 12:75d0291a9785 192 countsLOld = countsL;
Helvis 12:75d0291a9785 193 countsROld = countsR;
Helvis 12:75d0291a9785 194 while ((countsL - countsLOld) < MOVE_DIST*0.5f + 330.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 330.0f) {
Helvis 12:75d0291a9785 195 countsL = counterLeft.read();
Helvis 12:75d0291a9785 196 countsR = counterRight.read();
Helvis 12:75d0291a9785 197 }
Helvis 12:75d0291a9785 198 stop();
Helvis 12:75d0291a9785 199 break;
Helvis 12:75d0291a9785 200 }*/
Helvis 5:e2c0a4388d85 201 }
Helvis 13:845e49f20426 202 t.stop();
Helvis 13:845e49f20426 203 t.reset();
Helvis 1:2b5f79285a3e 204 }
Helvis 1:2b5f79285a3e 205 /**
Helvis 1:2b5f79285a3e 206 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 207 */
Helvis 1:2b5f79285a3e 208 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 209
Helvis 1:2b5f79285a3e 210 stop();
Helvis 1:2b5f79285a3e 211 controller.counterReset();
Helvis 1:2b5f79285a3e 212 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 213 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 214 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 215 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 216
Helvis 12:75d0291a9785 217 controller.setDesiredSpeedLeft(-ROTATE_SPEED);
Helvis 12:75d0291a9785 218 controller.setDesiredSpeedRight(-ROTATE_SPEED);
Helvis 1:2b5f79285a3e 219
Helvis 13:845e49f20426 220 while ((countsL - countsLOld) > -870 || (countsR - countsROld) > -870) {
Helvis 1:2b5f79285a3e 221
Helvis 12:75d0291a9785 222 //accel();
Helvis 12:75d0291a9785 223
Helvis 1:2b5f79285a3e 224 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 225 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 226
Helvis 1:2b5f79285a3e 227 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 12:75d0291a9785 228
Helvis 1:2b5f79285a3e 229 }
Helvis 1:2b5f79285a3e 230
Helvis 1:2b5f79285a3e 231 stop();
Helvis 1:2b5f79285a3e 232 }
Helvis 1:2b5f79285a3e 233
Helvis 1:2b5f79285a3e 234 /**
Helvis 1:2b5f79285a3e 235 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 236 */
Helvis 1:2b5f79285a3e 237 void Motion::rotateR() {
Helvis 1:2b5f79285a3e 238
Helvis 1:2b5f79285a3e 239 stop();
Helvis 1:2b5f79285a3e 240 controller.counterReset();
Helvis 1:2b5f79285a3e 241 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 242 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 243 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 244 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 245
Helvis 12:75d0291a9785 246 controller.setDesiredSpeedLeft(ROTATE_SPEED);
Helvis 12:75d0291a9785 247 controller.setDesiredSpeedRight(ROTATE_SPEED);
Helvis 1:2b5f79285a3e 248
Helvis 13:845e49f20426 249 while ((countsL - countsLOld) < 870 || (countsR - countsROld) < 870) {
Helvis 1:2b5f79285a3e 250
Helvis 12:75d0291a9785 251 //accel();
Helvis 12:75d0291a9785 252
Helvis 1:2b5f79285a3e 253 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 254 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 255
Helvis 1:2b5f79285a3e 256 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 12:75d0291a9785 257
Helvis 1:2b5f79285a3e 258 }
Helvis 1:2b5f79285a3e 259
Helvis 1:2b5f79285a3e 260 stop();
Helvis 1:2b5f79285a3e 261 }
Helvis 2:f898adf2d817 262 /**
Helvis 2:f898adf2d817 263 * Links abbiegen
Helvis 2:f898adf2d817 264 */
Helvis 3:076dd7ec7eb4 265 void Motion::turnL() {
Helvis 2:f898adf2d817 266
Helvis 2:f898adf2d817 267 controller.counterReset();
Helvis 2:f898adf2d817 268 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 269 countsROld = counterRight.read();
Helvis 2:f898adf2d817 270 countsL = counterLeft.read();
Helvis 2:f898adf2d817 271 countsR = counterRight.read();
Helvis 2:f898adf2d817 272
Helvis 2:f898adf2d817 273 controller.setDesiredSpeedLeft(17.0f);
Helvis 2:f898adf2d817 274 controller.setDesiredSpeedRight(-83.0f);
Helvis 2:f898adf2d817 275
Helvis 2:f898adf2d817 276 while ((countsL - countsLOld) < 440 || (countsR - countsROld) > -2148) {
Helvis 2:f898adf2d817 277
Helvis 2:f898adf2d817 278 countsL = counterLeft.read();
Helvis 2:f898adf2d817 279 countsR = counterRight.read();
Helvis 2:f898adf2d817 280
Helvis 2:f898adf2d817 281 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 2:f898adf2d817 282 }
Helvis 2:f898adf2d817 283 }
Helvis 2:f898adf2d817 284 /**
Helvis 2:f898adf2d817 285 * Rechts abbiegen
Helvis 2:f898adf2d817 286 */
Helvis 3:076dd7ec7eb4 287 void Motion::turnR() {
Helvis 2:f898adf2d817 288
Helvis 2:f898adf2d817 289 controller.counterReset();
Helvis 2:f898adf2d817 290 countsLOld = counterLeft.read();
Helvis 2:f898adf2d817 291 countsROld = counterRight.read();
Helvis 2:f898adf2d817 292 countsL = counterLeft.read();
Helvis 2:f898adf2d817 293 countsR = counterRight.read();
Helvis 2:f898adf2d817 294
Helvis 2:f898adf2d817 295 controller.setDesiredSpeedLeft(83.0f);
Helvis 2:f898adf2d817 296 controller.setDesiredSpeedRight(-17.0f);
Helvis 2:f898adf2d817 297
Helvis 2:f898adf2d817 298 while ((countsL - countsLOld) < 2148 || (countsR - countsROld) > -440) {
Helvis 2:f898adf2d817 299
Helvis 2:f898adf2d817 300 countsL = counterLeft.read();
Helvis 2:f898adf2d817 301 countsR = counterRight.read();
Helvis 2:f898adf2d817 302
Helvis 2:f898adf2d817 303 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 3:076dd7ec7eb4 304 }
Helvis 2:f898adf2d817 305 }
Helvis 1:2b5f79285a3e 306 /**
Helvis 1:2b5f79285a3e 307 * Motor Stop
Helvis 1:2b5f79285a3e 308 */
Helvis 1:2b5f79285a3e 309 void Motion::stop() {
Helvis 1:2b5f79285a3e 310
Helvis 1:2b5f79285a3e 311 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 312 controller.setDesiredSpeedRight(0.0f);
Helvis 16:c5b864804632 313 actSpeed = 0.0f;
Helvis 1:2b5f79285a3e 314
Helvis 1:2b5f79285a3e 315 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 316 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 317
Helvis 1:2b5f79285a3e 318 waitStop = 0;
Helvis 1:2b5f79285a3e 319 while( waitStop < ticks) {
Helvis 16:c5b864804632 320 controller.setDesiredSpeedLeft(0.0f);
Helvis 16:c5b864804632 321 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 322 waitStop+= 1;
Helvis 1:2b5f79285a3e 323 }
Helvis 1:2b5f79285a3e 324 }
Helvis 1:2b5f79285a3e 325 /**
Helvis 1:2b5f79285a3e 326 * 180° Rotation
Helvis 1:2b5f79285a3e 327 */
Helvis 1:2b5f79285a3e 328 void Motion::rotate180() {
Helvis 13:845e49f20426 329 //1723
Helvis 1:2b5f79285a3e 330 stop();
Helvis 1:2b5f79285a3e 331 controller.counterReset();
Helvis 1:2b5f79285a3e 332 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 333 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 334 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 335 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 336
Helvis 13:845e49f20426 337 //controller.setDesiredSpeedLeft(-ROTATE_SPEED);
Helvis 13:845e49f20426 338 //controller.setDesiredSpeedRight(-ROTATE_SPEED);
Helvis 13:845e49f20426 339
Helvis 13:845e49f20426 340 t.start();
Helvis 1:2b5f79285a3e 341
Helvis 13:845e49f20426 342 while ((countsL - countsLOld) > -900 || (countsR - countsROld) > -900) {
Helvis 1:2b5f79285a3e 343
Helvis 13:845e49f20426 344 actSpeed = 3.5f * t.read()*60.0f;
Helvis 13:845e49f20426 345
Helvis 13:845e49f20426 346 controller.setDesiredSpeedLeft(-actSpeed);
Helvis 13:845e49f20426 347 controller.setDesiredSpeedRight(-actSpeed);
Helvis 12:75d0291a9785 348
Helvis 1:2b5f79285a3e 349 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 350 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 351
Helvis 1:2b5f79285a3e 352 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 353 }
Helvis 1:2b5f79285a3e 354
Helvis 13:845e49f20426 355 t.reset();
Helvis 13:845e49f20426 356
Helvis 13:845e49f20426 357 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 13:845e49f20426 358
Helvis 13:845e49f20426 359 while ((countsL - countsLOld) > -1720 || (countsR - countsROld) > -1720) {
Helvis 13:845e49f20426 360
Helvis 13:845e49f20426 361 actSpeed = avgSpeed + (-3.5f * t.read()*60.0f);
Helvis 13:845e49f20426 362
Helvis 13:845e49f20426 363 controller.setDesiredSpeedLeft(-actSpeed);
Helvis 13:845e49f20426 364 controller.setDesiredSpeedRight(-actSpeed);
Helvis 13:845e49f20426 365
Helvis 13:845e49f20426 366 countsL = counterLeft.read();
Helvis 13:845e49f20426 367 countsR = counterRight.read();
Helvis 13:845e49f20426 368
Helvis 13:845e49f20426 369 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 13:845e49f20426 370 }
Helvis 13:845e49f20426 371 t.stop();
Helvis 13:845e49f20426 372 t.reset();
Helvis 1:2b5f79285a3e 373 stop();
Helvis 1:2b5f79285a3e 374 }
Helvis 1:2b5f79285a3e 375
Helvis 1:2b5f79285a3e 376
Helvis 1:2b5f79285a3e 377 void Motion::control() {
Helvis 1:2b5f79285a3e 378
Helvis 12:75d0291a9785 379 float wallLeft = 48.73f;
Helvis 12:75d0291a9785 380 float wallRight = 51.03f;
Helvis 1:2b5f79285a3e 381
Helvis 12:75d0291a9785 382 distanceL = irSensorL.readL();
Helvis 12:75d0291a9785 383 distanceR = irSensorR.readR();
Helvis 12:75d0291a9785 384
Helvis 6:4868f789c223 385
Helvis 12:75d0291a9785 386 if (distanceL < wallLeft && distanceR < wallRight) {
Helvis 12:75d0291a9785 387
Helvis 12:75d0291a9785 388 errorP = distanceL - distanceR + 2.30f;
Helvis 1:2b5f79285a3e 389
Helvis 12:75d0291a9785 390 }else if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 12:75d0291a9785 391
Helvis 12:75d0291a9785 392 errorP = distanceL - LEFT_MID_VAL;
Helvis 12:75d0291a9785 393
Helvis 12:75d0291a9785 394 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 12:75d0291a9785 395
Helvis 12:75d0291a9785 396 errorP = RIGHT_MID_VAL - distanceR;
Helvis 12:75d0291a9785 397
Helvis 12:75d0291a9785 398 }else{
Helvis 12:75d0291a9785 399
Helvis 12:75d0291a9785 400 errorP = 0;
Helvis 12:75d0291a9785 401 errorD = 0;
Helvis 12:75d0291a9785 402 }
Helvis 12:75d0291a9785 403
Helvis 12:75d0291a9785 404 errorD = errorP - oldErrorP;
Helvis 12:75d0291a9785 405
Helvis 12:75d0291a9785 406 oldErrorP = errorP;
Helvis 12:75d0291a9785 407
Helvis 12:75d0291a9785 408 if (abs(errorP) < 80.0f) {
Helvis 12:75d0291a9785 409 totalError = KP*errorP + KD*errorD;
Helvis 12:75d0291a9785 410 }else{
Helvis 12:75d0291a9785 411 totalError = 0;
Helvis 12:75d0291a9785 412 }
Helvis 12:75d0291a9785 413
Helvis 13:845e49f20426 414 controller.setDesiredSpeedLeft(actSpeed - totalError);
Helvis 13:845e49f20426 415 controller.setDesiredSpeedRight(-actSpeed - totalError);
Helvis 1:2b5f79285a3e 416 }
Helvis 1:2b5f79285a3e 417
Helvis 12:75d0291a9785 418 void Motion::runTask(int path[],int task, bool reverse) {
Helvis 1:2b5f79285a3e 419
Helvis 12:75d0291a9785 420 switch(path[task]) {
Helvis 12:75d0291a9785 421
Helvis 12:75d0291a9785 422 case 1:
Helvis 16:c5b864804632 423 if (reverse == true && path[task-1] == path[task] && path[task+1] != path[task]) {
Helvis 12:75d0291a9785 424 acceleration = true;
Helvis 15:0ae61d3d199f 425 deceleration = false;
Helvis 12:75d0291a9785 426 }else{
Helvis 12:75d0291a9785 427 acceleration = false;
Helvis 12:75d0291a9785 428 }
Helvis 12:75d0291a9785 429
Helvis 16:c5b864804632 430 if (reverse == false && path[task+1] == path[task] && path[task-1] != path[task]) {
Helvis 15:0ae61d3d199f 431 acceleration = true;
Helvis 15:0ae61d3d199f 432 deceleration = false;
Helvis 12:75d0291a9785 433 }else{
Helvis 12:75d0291a9785 434 acceleration = false;
Helvis 12:75d0291a9785 435 }
Helvis 12:75d0291a9785 436
Helvis 12:75d0291a9785 437 if (reverse == true && path[task-1] != path[task]) {
Helvis 12:75d0291a9785 438 deceleration = true;
Helvis 15:0ae61d3d199f 439 acceleration = false;
Helvis 12:75d0291a9785 440 }else{
Helvis 12:75d0291a9785 441 deceleration = false;
Helvis 12:75d0291a9785 442 }
Helvis 12:75d0291a9785 443
Helvis 12:75d0291a9785 444 if (reverse == false && path[task+1] != path[task]) {
Helvis 12:75d0291a9785 445 deceleration = true;
Helvis 15:0ae61d3d199f 446 acceleration = false;
Helvis 12:75d0291a9785 447 }else{
Helvis 12:75d0291a9785 448 deceleration = false;
Helvis 12:75d0291a9785 449 }
Helvis 12:75d0291a9785 450 move();
Helvis 12:75d0291a9785 451 break;
Helvis 12:75d0291a9785 452 case 2:
Helvis 12:75d0291a9785 453 rotateL();
Helvis 12:75d0291a9785 454 break;
Helvis 12:75d0291a9785 455 case 3:
Helvis 12:75d0291a9785 456 rotateR();
Helvis 12:75d0291a9785 457 break;
Helvis 12:75d0291a9785 458 case 4:
Helvis 12:75d0291a9785 459 moveFast();
Helvis 12:75d0291a9785 460 break;
Helvis 12:75d0291a9785 461 case 5:
Helvis 12:75d0291a9785 462 stop();
Helvis 12:75d0291a9785 463 break;
Helvis 12:75d0291a9785 464 }
Helvis 1:2b5f79285a3e 465 }
Helvis 1:2b5f79285a3e 466
Helvis 1:2b5f79285a3e 467 int Motion::finish() {
Helvis 12:75d0291a9785 468
Helvis 12:75d0291a9785 469 return line;
Helvis 1:2b5f79285a3e 470 }
Helvis 11:2960fc540616 471
Helvis 12:75d0291a9785 472
Helvis 13:845e49f20426 473 void Motion::accel(float targetSpeed) {
Helvis 13:845e49f20426 474
Helvis 13:845e49f20426 475
Helvis 12:75d0291a9785 476
Helvis 12:75d0291a9785 477 avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
Helvis 12:75d0291a9785 478 avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
Helvis 12:75d0291a9785 479
Helvis 15:0ae61d3d199f 480 if ( avgSpeed < targetSpeed) {
Helvis 12:75d0291a9785 481
Helvis 16:c5b864804632 482 actSpeed = ACCEL_CONST * t.read()*60.0f;
Helvis 16:c5b864804632 483
Helvis 16:c5b864804632 484 }else if (avgSpeed < targetSpeed*2.6f && acceleration == true) {
Helvis 12:75d0291a9785 485
Helvis 16:c5b864804632 486 actSpeed = 2.5f * t.read()*60.0f;
Helvis 16:c5b864804632 487
Helvis 16:c5b864804632 488 }else if ( avgSpeed > targetSpeed && deceleration == true) {
Helvis 11:2960fc540616 489
Helvis 15:0ae61d3d199f 490 actSpeed = targetSpeed*2.6f - ACCEL_CONST * t.read()*60.0f;
Helvis 12:75d0291a9785 491 }
Helvis 12:75d0291a9785 492
Helvis 12:75d0291a9785 493
Helvis 12:75d0291a9785 494 }
Helvis 12:75d0291a9785 495