Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@14:2926932e26df, 2018-05-04 (annotated)

Committer:
luethale
Date:
Fri May 04 19:56:05 2018 +0000
Revision:
14:2926932e26df
Parent:
13:845e49f20426
Child:
15:0ae61d3d199f
?pis anders KABUTT

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