Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@22:91526c8d15ba, 2018-05-15 (annotated)

Committer:
Helvis
Date:
Tue May 15 11:19:15 2018 +0000
Revision:
22:91526c8d15ba
Parent:
21:41997651337a
Child:
23:accd07ca2da7
v1.1b

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