Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@20:20573f55a5fd, 2018-05-11 (annotated)

Committer:
luethale
Date:
Fri May 11 13:12:28 2018 +0000
Revision:
20:20573f55a5fd
Parent:
19:6cd6cc5c8b4c
Child:
21:41997651337a
Clean version without trash

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