Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

Motion.cpp@1:2b5f79285a3e, 2018-04-19 (annotated)

Committer:
Helvis
Date:
Thu Apr 19 06:19:43 2018 +0000
Revision:
1:2b5f79285a3e
Child:
2:f898adf2d817
Micromouse V0.9: Tresholds + Regler noch anpassen

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 1:2b5f79285a3e 7 const float Motion::LEFT_MID_VAL = 45.0f;
Helvis 1:2b5f79285a3e 8 const float Motion::RIGHT_MID_VAL = 45.0f;
Helvis 1:2b5f79285a3e 9 const float Motion::KP = 2.5f;
Helvis 1:2b5f79285a3e 10 const float Motion::KD = 0.00f;
Helvis 1:2b5f79285a3e 11
Helvis 1:2b5f79285a3e 12
Helvis 1:2b5f79285a3e 13 Motion::Motion(Controller& controller, EncoderCounter& counterLeft,
Helvis 1:2b5f79285a3e 14 EncoderCounter& counterRight, IRSensor& irSensorL,
Helvis 1:2b5f79285a3e 15 IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
Helvis 1:2b5f79285a3e 16 DigitalOut& enableMotorDriver) :
Helvis 1:2b5f79285a3e 17 controller(controller), counterLeft(counterLeft),
Helvis 1:2b5f79285a3e 18 counterRight(counterRight), irSensorL(irSensorL),
Helvis 1:2b5f79285a3e 19 irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
Helvis 1:2b5f79285a3e 20 enableMotorDriver(enableMotorDriver) {}
Helvis 1:2b5f79285a3e 21
Helvis 1:2b5f79285a3e 22 Motion::~Motion() {}
Helvis 1:2b5f79285a3e 23
Helvis 1:2b5f79285a3e 24 /**
Helvis 1:2b5f79285a3e 25 * Eine Feldstrecke druchführen
Helvis 1:2b5f79285a3e 26 */
Helvis 1:2b5f79285a3e 27 void Motion::move() {
Helvis 1:2b5f79285a3e 28
Helvis 1:2b5f79285a3e 29 controller.counterReset();
Helvis 1:2b5f79285a3e 30 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 31 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 32 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 33 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 34
Helvis 1:2b5f79285a3e 35 speedLeft = 50.0f;
Helvis 1:2b5f79285a3e 36 speedRight = -50.0f;
Helvis 1:2b5f79285a3e 37
Helvis 1:2b5f79285a3e 38 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 39 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 40
Helvis 1:2b5f79285a3e 41 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
Helvis 1:2b5f79285a3e 42
Helvis 1:2b5f79285a3e 43 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 44 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 45
Helvis 1:2b5f79285a3e 46 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 47
Helvis 1:2b5f79285a3e 48 control();
Helvis 1:2b5f79285a3e 49
Helvis 1:2b5f79285a3e 50 }
Helvis 1:2b5f79285a3e 51 }
Helvis 1:2b5f79285a3e 52 /**
Helvis 1:2b5f79285a3e 53 * Eine Feldstrecke mit höherer Geschwindigkeit fahren
Helvis 1:2b5f79285a3e 54 */
Helvis 1:2b5f79285a3e 55 void Motion::moveFast() {
Helvis 1:2b5f79285a3e 56
Helvis 1:2b5f79285a3e 57 controller.counterReset();
Helvis 1:2b5f79285a3e 58 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 59 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 60 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 61 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 62
Helvis 1:2b5f79285a3e 63 speedLeft = 100.0f;
Helvis 1:2b5f79285a3e 64 speedRight = -100.0f;
Helvis 1:2b5f79285a3e 65
Helvis 1:2b5f79285a3e 66 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 67 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 68
Helvis 1:2b5f79285a3e 69 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
Helvis 1:2b5f79285a3e 70
Helvis 1:2b5f79285a3e 71 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 72 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 73
Helvis 1:2b5f79285a3e 74 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 75
Helvis 1:2b5f79285a3e 76 control();
Helvis 1:2b5f79285a3e 77
Helvis 1:2b5f79285a3e 78 }
Helvis 1:2b5f79285a3e 79 }
Helvis 1:2b5f79285a3e 80 /**
Helvis 1:2b5f79285a3e 81 * Eine Feldstrecke mit überprüfung der Ziellinie fahren
Helvis 1:2b5f79285a3e 82 */
Helvis 1:2b5f79285a3e 83 void Motion::scanMove() {
Helvis 1:2b5f79285a3e 84
Helvis 1:2b5f79285a3e 85 controller.counterReset();
Helvis 1:2b5f79285a3e 86 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 87 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 88 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 89 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 90
Helvis 1:2b5f79285a3e 91 speedLeft = 50.0f;
Helvis 1:2b5f79285a3e 92 speedRight = -50.0f;
Helvis 1:2b5f79285a3e 93
Helvis 1:2b5f79285a3e 94 controller.setDesiredSpeedLeft(speedLeft);
Helvis 1:2b5f79285a3e 95 controller.setDesiredSpeedRight(speedRight);
Helvis 1:2b5f79285a3e 96
Helvis 1:2b5f79285a3e 97 while ((countsL - countsLOld) < 1647 || (countsR - countsROld) > -1647) {
Helvis 1:2b5f79285a3e 98
Helvis 1:2b5f79285a3e 99 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 100 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 101
Helvis 1:2b5f79285a3e 102 if (enableMotorDriver == 0) {
Helvis 1:2b5f79285a3e 103 enableMotorDriver = 1;
Helvis 1:2b5f79285a3e 104 }
Helvis 1:2b5f79285a3e 105
Helvis 1:2b5f79285a3e 106 if (lineSensor.read() > 0.9f) {
Helvis 1:2b5f79285a3e 107 line = 1;
Helvis 1:2b5f79285a3e 108 }else{
Helvis 1:2b5f79285a3e 109 line = 0;
Helvis 1:2b5f79285a3e 110 }
Helvis 1:2b5f79285a3e 111
Helvis 1:2b5f79285a3e 112 control();
Helvis 1:2b5f79285a3e 113
Helvis 1:2b5f79285a3e 114 }
Helvis 1:2b5f79285a3e 115
Helvis 1:2b5f79285a3e 116 }
Helvis 1:2b5f79285a3e 117 /**
Helvis 1:2b5f79285a3e 118 * 90° Rotation nach Links
Helvis 1:2b5f79285a3e 119 */
Helvis 1:2b5f79285a3e 120 void Motion::rotateL() {
Helvis 1:2b5f79285a3e 121
Helvis 1:2b5f79285a3e 122 stop();
Helvis 1:2b5f79285a3e 123 controller.counterReset();
Helvis 1:2b5f79285a3e 124 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 125 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 126 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 127 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 128
Helvis 1:2b5f79285a3e 129 controller.setDesiredSpeedLeft(-20.0f);
Helvis 1:2b5f79285a3e 130 controller.setDesiredSpeedRight(-20.0f);
Helvis 1:2b5f79285a3e 131
Helvis 1:2b5f79285a3e 132 while ((countsL - countsLOld) > -862 || (countsR - countsROld) > -862) {
Helvis 1:2b5f79285a3e 133
Helvis 1:2b5f79285a3e 134 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 135 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 136
Helvis 1:2b5f79285a3e 137 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 138 }
Helvis 1:2b5f79285a3e 139
Helvis 1:2b5f79285a3e 140 stop();
Helvis 1:2b5f79285a3e 141 }
Helvis 1:2b5f79285a3e 142
Helvis 1:2b5f79285a3e 143 /**
Helvis 1:2b5f79285a3e 144 * 90° Rotation nach Rechts
Helvis 1:2b5f79285a3e 145 */
Helvis 1:2b5f79285a3e 146 void Motion::rotateR() {
Helvis 1:2b5f79285a3e 147
Helvis 1:2b5f79285a3e 148 stop();
Helvis 1:2b5f79285a3e 149 controller.counterReset();
Helvis 1:2b5f79285a3e 150 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 151 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 152 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 153 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 154
Helvis 1:2b5f79285a3e 155 controller.setDesiredSpeedLeft(20.0f);
Helvis 1:2b5f79285a3e 156 controller.setDesiredSpeedRight(20.0f);
Helvis 1:2b5f79285a3e 157
Helvis 1:2b5f79285a3e 158 while ((countsL - countsLOld) < 862 || (countsR - countsROld) < 862) {
Helvis 1:2b5f79285a3e 159
Helvis 1:2b5f79285a3e 160 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 161 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 162
Helvis 1:2b5f79285a3e 163 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 164 }
Helvis 1:2b5f79285a3e 165
Helvis 1:2b5f79285a3e 166 stop();
Helvis 1:2b5f79285a3e 167 }
Helvis 1:2b5f79285a3e 168
Helvis 1:2b5f79285a3e 169 /**
Helvis 1:2b5f79285a3e 170 * Motor Stop
Helvis 1:2b5f79285a3e 171 */
Helvis 1:2b5f79285a3e 172 void Motion::stop() {
Helvis 1:2b5f79285a3e 173
Helvis 1:2b5f79285a3e 174 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:2b5f79285a3e 175 controller.setDesiredSpeedRight(0.0f);
Helvis 1:2b5f79285a3e 176
Helvis 1:2b5f79285a3e 177 float sL = controller.getSpeedLeft();
Helvis 1:2b5f79285a3e 178 float ticks = 0.08f*sL;
Helvis 1:2b5f79285a3e 179
Helvis 1:2b5f79285a3e 180 waitStop = 0;
Helvis 1:2b5f79285a3e 181 while( waitStop < ticks) {
Helvis 1:2b5f79285a3e 182 waitStop+= 1;
Helvis 1:2b5f79285a3e 183 }
Helvis 1:2b5f79285a3e 184 }
Helvis 1:2b5f79285a3e 185 /**
Helvis 1:2b5f79285a3e 186 * 180° Rotation
Helvis 1:2b5f79285a3e 187 */
Helvis 1:2b5f79285a3e 188 void Motion::rotate180() {
Helvis 1:2b5f79285a3e 189
Helvis 1:2b5f79285a3e 190 stop();
Helvis 1:2b5f79285a3e 191 controller.counterReset();
Helvis 1:2b5f79285a3e 192 countsLOld = counterLeft.read();
Helvis 1:2b5f79285a3e 193 countsROld = counterRight.read();
Helvis 1:2b5f79285a3e 194 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 195 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 196
Helvis 1:2b5f79285a3e 197 controller.setDesiredSpeedLeft(-50.0f);
Helvis 1:2b5f79285a3e 198 controller.setDesiredSpeedRight(-50.0f);
Helvis 1:2b5f79285a3e 199
Helvis 1:2b5f79285a3e 200 while ((countsL - countsLOld) > -1723 || (countsR - countsROld) > -1723) {
Helvis 1:2b5f79285a3e 201
Helvis 1:2b5f79285a3e 202 countsL = counterLeft.read();
Helvis 1:2b5f79285a3e 203 countsR = counterRight.read();
Helvis 1:2b5f79285a3e 204
Helvis 1:2b5f79285a3e 205 if (enableMotorDriver == 0) {enableMotorDriver = 1;}
Helvis 1:2b5f79285a3e 206 }
Helvis 1:2b5f79285a3e 207
Helvis 1:2b5f79285a3e 208 stop();
Helvis 1:2b5f79285a3e 209 }
Helvis 1:2b5f79285a3e 210
Helvis 1:2b5f79285a3e 211
Helvis 1:2b5f79285a3e 212 void Motion::control() {
Helvis 1:2b5f79285a3e 213
Helvis 1:2b5f79285a3e 214 //float wallLeft = 70.0f;
Helvis 1:2b5f79285a3e 215 //float wallRight = 70.0f;
Helvis 1:2b5f79285a3e 216
Helvis 1:2b5f79285a3e 217 distanceL = irSensorL.readL();
Helvis 1:2b5f79285a3e 218 distanceR = irSensorR.readR();
Helvis 1:2b5f79285a3e 219
Helvis 1:2b5f79285a3e 220 if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
Helvis 1:2b5f79285a3e 221
Helvis 1:2b5f79285a3e 222 errorP = LEFT_MID_VAL - distanceL;
Helvis 1:2b5f79285a3e 223
Helvis 1:2b5f79285a3e 224 }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
Helvis 1:2b5f79285a3e 225
Helvis 1:2b5f79285a3e 226 errorP = distanceR - RIGHT_MID_VAL;
Helvis 1:2b5f79285a3e 227
Helvis 1:2b5f79285a3e 228 }else{
Helvis 1:2b5f79285a3e 229
Helvis 1:2b5f79285a3e 230 errorP = 0;
Helvis 1:2b5f79285a3e 231 }
Helvis 1:2b5f79285a3e 232
Helvis 1:2b5f79285a3e 233 errorD = errorP - oldErrorP;
Helvis 1:2b5f79285a3e 234
Helvis 1:2b5f79285a3e 235 oldErrorP = errorP;
Helvis 1:2b5f79285a3e 236
Helvis 1:2b5f79285a3e 237 totalError = KP*errorP + KD*errorD;
Helvis 1:2b5f79285a3e 238
Helvis 1:2b5f79285a3e 239 controller.setDesiredSpeedLeft(speedLeft + totalError);
Helvis 1:2b5f79285a3e 240 controller.setDesiredSpeedRight(speedRight + totalError);
Helvis 1:2b5f79285a3e 241 }
Helvis 1:2b5f79285a3e 242
Helvis 1:2b5f79285a3e 243 void Motion::runTask(int task) {
Helvis 1:2b5f79285a3e 244
Helvis 1:2b5f79285a3e 245 switch(task) {
Helvis 1:2b5f79285a3e 246
Helvis 1:2b5f79285a3e 247 case 1:
Helvis 1:2b5f79285a3e 248 move();
Helvis 1:2b5f79285a3e 249 break;
Helvis 1:2b5f79285a3e 250 case 2:
Helvis 1:2b5f79285a3e 251 rotateL();
Helvis 1:2b5f79285a3e 252 break;
Helvis 1:2b5f79285a3e 253 case 3:
Helvis 1:2b5f79285a3e 254 rotateR();
Helvis 1:2b5f79285a3e 255 break;
Helvis 1:2b5f79285a3e 256 case 4:
Helvis 1:2b5f79285a3e 257 moveFast();
Helvis 1:2b5f79285a3e 258 break;
Helvis 1:2b5f79285a3e 259 case 5:
Helvis 1:2b5f79285a3e 260 stop();
Helvis 1:2b5f79285a3e 261 break;
Helvis 1:2b5f79285a3e 262 }
Helvis 1:2b5f79285a3e 263 }
Helvis 1:2b5f79285a3e 264
Helvis 1:2b5f79285a3e 265 int Motion::finish() {
Helvis 1:2b5f79285a3e 266
Helvis 1:2b5f79285a3e 267 return line;
Helvis 1:2b5f79285a3e 268 }