Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@12:75d0291a9785, 2018-05-02 (annotated)

Committer:
Helvis
Date:
Wed May 02 14:58:48 2018 +0000
Revision:
12:75d0291a9785
Parent:
11:2960fc540616
Child:
13:845e49f20426
v0.9.6b: funktioniert!

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