Arjun Boddu / CurrentMicrmouse

micrmouse

Dependencies:   AVEncoder mbed-src-AV

main.cpp@0:394261695392, 2016-03-18 (annotated)

Committer:
ajboddu
Date:
Fri Mar 18 07:42:11 2016 +0000
Revision:
0:394261695392
micromouse;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ajboddu 0:394261695392 1 //ADJUST VARIABLES SECTION
ajboddu 0:394261695392 2
ajboddu 0:394261695392 3 const int preRunLength = 1600;
ajboddu 0:394261695392 4 const int postRunLength = 0;
ajboddu 0:394261695392 5 const int turnLength = 850;
ajboddu 0:394261695392 6 const float leftAngleTurnThreshhold = -.045;
ajboddu 0:394261695392 7 const float rightAngleTurnThreshhold = -.06;
ajboddu 0:394261695392 8 const float baseSpeed = 0.25;
ajboddu 0:394261695392 9
ajboddu 0:394261695392 10 #include "PinDefinitions.h"
ajboddu 0:394261695392 11
ajboddu 0:394261695392 12 //PROTOTYPES
ajboddu 0:394261695392 13 void motorCorrections();
ajboddu 0:394261695392 14 float getLSreading();
ajboddu 0:394261695392 15 float getLAreading();
ajboddu 0:394261695392 16 float getRAreading();
ajboddu 0:394261695392 17 float getRSreading();
ajboddu 0:394261695392 18 float pidCorrection(int error);
ajboddu 0:394261695392 19 float proportion(int error);
ajboddu 0:394261695392 20 int derivative(int error);
ajboddu 0:394261695392 21 float integral(int error);
ajboddu 0:394261695392 22 void setRightPwm(float speed);
ajboddu 0:394261695392 23 void setLeftPwm(float speed);
ajboddu 0:394261695392 24 void postTurnRun();
ajboddu 0:394261695392 25 void preTurnRun();
ajboddu 0:394261695392 26
ajboddu 0:394261695392 27 Serial pc(PA_11, PA_12); //SERIAL TX/RX
ajboddu 0:394261695392 28 DigitalOut myled(LED1);
ajboddu 0:394261695392 29
ajboddu 0:394261695392 30 Ticker PIDcorrectionTicker;
ajboddu 0:394261695392 31 Ticker gyroOffsetCalc;
ajboddu 0:394261695392 32
ajboddu 0:394261695392 33 float baselineRS;
ajboddu 0:394261695392 34 float baselineLS;
ajboddu 0:394261695392 35 float baselineLA;
ajboddu 0:394261695392 36 float baselineRA;
ajboddu 0:394261695392 37
ajboddu 0:394261695392 38
ajboddu 0:394261695392 39 bool safeToRunSystick;
ajboddu 0:394261695392 40 int rightPulses;
ajboddu 0:394261695392 41 int leftPulses;
ajboddu 0:394261695392 42 float currentAngle = 0;
ajboddu 0:394261695392 43 float refreshTimeSeconds = 0.05; //How fast the systicker is going
ajboddu 0:394261695392 44 const int flashTime = 20; //The amount of time sensors are on before we take measurment (millisecond)
ajboddu 0:394261695392 45
ajboddu 0:394261695392 46 int prevError;
ajboddu 0:394261695392 47 int integrator = 0;
ajboddu 0:394261695392 48 int decayFactor = 1.1;
ajboddu 0:394261695392 49 float Kp = 0.001;
ajboddu 0:394261695392 50 //Kd =
ajboddu 0:394261695392 51 float Ki = 0.002;
ajboddu 0:394261695392 52
ajboddu 0:394261695392 53 //FOR THE SENSORS RUN WHEN DARK TO FIND AMBIENT LIGHT
ajboddu 0:394261695392 54 //HOW LONG DO I NEED TO TURN ON SENSORS TO GET A READING? 1MS? NO TIME AT ALL?
ajboddu 0:394261695392 55
ajboddu 0:394261695392 56
ajboddu 0:394261695392 57
ajboddu 0:394261695392 58 void motorCorrections() //Three things, I should probably break into three functions!!
ajboddu 0:394261695392 59 {
ajboddu 0:394261695392 60 if(safeToRunSystick) {
ajboddu 0:394261695392 61 int rightPulses = RENC.getPulses();
ajboddu 0:394261695392 62 int leftPulses = LENC.getPulses();
ajboddu 0:394261695392 63 //we want the difference which is the error
ajboddu 0:394261695392 64 int errorEncoder = rightPulses - leftPulses;
ajboddu 0:394261695392 65 float correction = pidCorrection(errorEncoder);
ajboddu 0:394261695392 66 if(correction > 0.5)
ajboddu 0:394261695392 67 correction = 0.5;
ajboddu 0:394261695392 68 else if(correction < -0.5)
ajboddu 0:394261695392 69 correction = -0.5;
ajboddu 0:394261695392 70 setRightPwm(baseSpeed-correction);
ajboddu 0:394261695392 71 setLeftPwm(baseSpeed+correction);
ajboddu 0:394261695392 72 //pc.printf("Error:%.6f \r\i", correction);
ajboddu 0:394261695392 73 //pc.printf("Gyro Raw: %.6f \r\n", currentAngle);
ajboddu 0:394261695392 74 RENC.reset();
ajboddu 0:394261695392 75 LENC.reset();
ajboddu 0:394261695392 76 myled = !myled;
ajboddu 0:394261695392 77 }
ajboddu 0:394261695392 78 }
ajboddu 0:394261695392 79
ajboddu 0:394261695392 80
ajboddu 0:394261695392 81 float getLSreading()
ajboddu 0:394261695392 82 {
ajboddu 0:394261695392 83 int ambient = IRSensorLS.read(); //Ambient Reading
ajboddu 0:394261695392 84 IRledLS.write(1);
ajboddu 0:394261695392 85 wait_ms(flashTime); //Maybe we can decrease this amount
ajboddu 0:394261695392 86 float sensorReading = IRSensorLS.read(); //Will get the reading
ajboddu 0:394261695392 87 IRledLS.write(0);
ajboddu 0:394261695392 88 return sensorReading - ambient; //Ideally we will get pure IR reading
ajboddu 0:394261695392 89
ajboddu 0:394261695392 90 }
ajboddu 0:394261695392 91
ajboddu 0:394261695392 92 float getLAreading()
ajboddu 0:394261695392 93 {
ajboddu 0:394261695392 94 int ambient = IRSensorLA.read(); //Ambient Reading
ajboddu 0:394261695392 95 IRledLA.write(1);
ajboddu 0:394261695392 96 wait_ms(flashTime); //Maybe we can decrease this amount
ajboddu 0:394261695392 97 float sensorReading = IRSensorLA.read(); //Will get the reading
ajboddu 0:394261695392 98 IRledLA.write(0);
ajboddu 0:394261695392 99 return sensorReading - ambient; //Ideally we will get pure IR reading
ajboddu 0:394261695392 100 }
ajboddu 0:394261695392 101
ajboddu 0:394261695392 102 float getRAreading()
ajboddu 0:394261695392 103 {
ajboddu 0:394261695392 104 int ambient = IRSensorRA.read(); //Ambient Reading
ajboddu 0:394261695392 105 IRledRA.write(1);
ajboddu 0:394261695392 106 wait_ms(flashTime); //Maybe we can decrease this amount
ajboddu 0:394261695392 107 float sensorReading = IRSensorRA.read(); //Will get the reading
ajboddu 0:394261695392 108 IRledRA.write(0);
ajboddu 0:394261695392 109 return sensorReading - ambient; //Ideally we will get pure IR reading
ajboddu 0:394261695392 110 }
ajboddu 0:394261695392 111
ajboddu 0:394261695392 112 float getRSreading()
ajboddu 0:394261695392 113 {
ajboddu 0:394261695392 114 int ambient = IRSensorRS.read(); //Ambient Reading
ajboddu 0:394261695392 115 IRledRS.write(1);
ajboddu 0:394261695392 116 wait_ms(flashTime); //Maybe we can decrease this amount
ajboddu 0:394261695392 117 float sensorReading = IRSensorRS.read(); //Will get the reading
ajboddu 0:394261695392 118 IRledRS.write(0);
ajboddu 0:394261695392 119 return sensorReading - ambient; //Ideally we will get pure IR reading
ajboddu 0:394261695392 120 }
ajboddu 0:394261695392 121 //The correction will be given in difference that power should be writen to the motors. We will chose baseline speed for both
ajboddu 0:394261695392 122 float pidCorrection(int error)
ajboddu 0:394261695392 123 {
ajboddu 0:394261695392 124 return proportion(error) + integral(error);
ajboddu 0:394261695392 125 // +derivative(error)
ajboddu 0:394261695392 126 }
ajboddu 0:394261695392 127
ajboddu 0:394261695392 128 float proportion(int error)
ajboddu 0:394261695392 129 {
ajboddu 0:394261695392 130 return Kp*error;
ajboddu 0:394261695392 131 }
ajboddu 0:394261695392 132 /*
ajboddu 0:394261695392 133 int derivative(int error){
ajboddu 0:394261695392 134 int dError = error - prevError;
ajboddu 0:394261695392 135 int dt = timer.read_us();
ajboddu 0:394261695392 136 timer.reset();
ajboddu 0:394261695392 137 prevError = error;
ajboddu 0:394261695392 138 int correctionD = -(Kd*dError)/dt;
ajboddu 0:394261695392 139 return correctionD;
ajboddu 0:394261695392 140 }
ajboddu 0:394261695392 141 */
ajboddu 0:394261695392 142 float integral(int error)
ajboddu 0:394261695392 143 {
ajboddu 0:394261695392 144 integrator +=error;
ajboddu 0:394261695392 145 integrator /= decayFactor;
ajboddu 0:394261695392 146 return Ki*integrator;
ajboddu 0:394261695392 147 }
ajboddu 0:394261695392 148
ajboddu 0:394261695392 149
ajboddu 0:394261695392 150 void setRightPwm(float speed)
ajboddu 0:394261695392 151 {
ajboddu 0:394261695392 152 if (speed == 0) {
ajboddu 0:394261695392 153 RMOTORA = 1.0;
ajboddu 0:394261695392 154 RMOTORB = 1.0;
ajboddu 0:394261695392 155 }
ajboddu 0:394261695392 156
ajboddu 0:394261695392 157 if (speed > 0) {
ajboddu 0:394261695392 158 RMOTORA = speed;
ajboddu 0:394261695392 159 RMOTORB = 0;
ajboddu 0:394261695392 160 } else {
ajboddu 0:394261695392 161 RMOTORA = 0;
ajboddu 0:394261695392 162 RMOTORB = -speed;
ajboddu 0:394261695392 163 }
ajboddu 0:394261695392 164 }
ajboddu 0:394261695392 165
ajboddu 0:394261695392 166 void setLeftPwm(float speed)
ajboddu 0:394261695392 167 {
ajboddu 0:394261695392 168 if (speed == 0) {
ajboddu 0:394261695392 169 LMOTORA = 1.0;
ajboddu 0:394261695392 170 LMOTORB = 1.0;
ajboddu 0:394261695392 171 }
ajboddu 0:394261695392 172
ajboddu 0:394261695392 173 if (speed > 0) {
ajboddu 0:394261695392 174 LMOTORA = speed;
ajboddu 0:394261695392 175 LMOTORB = 0;
ajboddu 0:394261695392 176 } else {
ajboddu 0:394261695392 177 LMOTORA = 0;
ajboddu 0:394261695392 178 LMOTORB = -speed;
ajboddu 0:394261695392 179 }
ajboddu 0:394261695392 180 }
ajboddu 0:394261695392 181
ajboddu 0:394261695392 182 void setBaselines()
ajboddu 0:394261695392 183 {
ajboddu 0:394261695392 184 baselineLS = getLSreading();
ajboddu 0:394261695392 185 baselineRS = getRSreading();
ajboddu 0:394261695392 186 baselineLA = getLAreading();
ajboddu 0:394261695392 187 baselineRA = getRAreading();
ajboddu 0:394261695392 188 }
ajboddu 0:394261695392 189
ajboddu 0:394261695392 190
ajboddu 0:394261695392 191 void turnAround()
ajboddu 0:394261695392 192 {
ajboddu 0:394261695392 193 safeToRunSystick = false;
ajboddu 0:394261695392 194 RENC.reset();
ajboddu 0:394261695392 195 LENC.reset();
ajboddu 0:394261695392 196 setRightPwm(-.1);
ajboddu 0:394261695392 197 setLeftPwm(.1);
ajboddu 0:394261695392 198 myled = !myled;
ajboddu 0:394261695392 199 while(RENC.getPulses() < 1700 && LENC.getPulses() < 1700) {
ajboddu 0:394261695392 200 }
ajboddu 0:394261695392 201 pc.printf("WE MADE IT OUT \r\n");
ajboddu 0:394261695392 202 myled = 0;
ajboddu 0:394261695392 203 setRightPwm(0);
ajboddu 0:394261695392 204 setLeftPwm(0);
ajboddu 0:394261695392 205 wait(0.2);
ajboddu 0:394261695392 206 RENC.reset();
ajboddu 0:394261695392 207 LENC.reset();
ajboddu 0:394261695392 208 safeToRunSystick = true;
ajboddu 0:394261695392 209
ajboddu 0:394261695392 210 }
ajboddu 0:394261695392 211
ajboddu 0:394261695392 212 void turnLeft()
ajboddu 0:394261695392 213 {
ajboddu 0:394261695392 214 safeToRunSystick = false;
ajboddu 0:394261695392 215 preTurnRun();
ajboddu 0:394261695392 216 //Now should be in position to turn
ajboddu 0:394261695392 217 setRightPwm(.1);
ajboddu 0:394261695392 218 setLeftPwm(-.1);
ajboddu 0:394261695392 219 RENC.reset();
ajboddu 0:394261695392 220 LENC.reset();
ajboddu 0:394261695392 221 myled = !myled;
ajboddu 0:394261695392 222 while(RENC.getPulses() < turnLength && LENC.getPulses() < turnLength) {}
ajboddu 0:394261695392 223 setRightPwm(0);
ajboddu 0:394261695392 224 setLeftPwm(0);
ajboddu 0:394261695392 225 wait(0.2);
ajboddu 0:394261695392 226 safeToRunSystick = true;
ajboddu 0:394261695392 227 //At this point the car should be pivoted
ajboddu 0:394261695392 228 postTurnRun();
ajboddu 0:394261695392 229 }
ajboddu 0:394261695392 230
ajboddu 0:394261695392 231 void turnRight()
ajboddu 0:394261695392 232 {
ajboddu 0:394261695392 233 safeToRunSystick = false;
ajboddu 0:394261695392 234 preTurnRun();
ajboddu 0:394261695392 235 //Now should be in position to turn
ajboddu 0:394261695392 236 setRightPwm(-.1);
ajboddu 0:394261695392 237 setLeftPwm(+.1);
ajboddu 0:394261695392 238 RENC.reset();
ajboddu 0:394261695392 239 LENC.reset();
ajboddu 0:394261695392 240 myled = !myled;
ajboddu 0:394261695392 241 while(RENC.getPulses() < turnLength && LENC.getPulses() < turnLength) {}
ajboddu 0:394261695392 242 setRightPwm(0);
ajboddu 0:394261695392 243 setLeftPwm(0);
ajboddu 0:394261695392 244 wait(0.2);
ajboddu 0:394261695392 245 postTurnRun();
ajboddu 0:394261695392 246 safeToRunSystick = true;
ajboddu 0:394261695392 247 //At this point the car should be pivoted
ajboddu 0:394261695392 248 }
ajboddu 0:394261695392 249
ajboddu 0:394261695392 250
ajboddu 0:394261695392 251
ajboddu 0:394261695392 252 void preTurnRun(){ //this code runs before and after every turn to ensure that the car is positioned at the right spot when it turns
ajboddu 0:394261695392 253 RENC.reset();
ajboddu 0:394261695392 254 LENC.reset();
ajboddu 0:394261695392 255 int rightPulses = 0;
ajboddu 0:394261695392 256 int leftPulses = 0;
ajboddu 0:394261695392 257 //we want the difference which is the error
ajboddu 0:394261695392 258 while(RENC.getPulses() < preRunLength && LENC.getPulses() < preRunLength) {
ajboddu 0:394261695392 259 rightPulses = RENC.getPulses() - rightPulses;
ajboddu 0:394261695392 260 leftPulses = LENC.getPulses() - leftPulses;
ajboddu 0:394261695392 261 int errorEncoder = rightPulses - leftPulses;
ajboddu 0:394261695392 262 float correction = proportion(errorEncoder);
ajboddu 0:394261695392 263 if(correction > 0.5)
ajboddu 0:394261695392 264 correction = 0.5;
ajboddu 0:394261695392 265 else if(correction < -0.5)
ajboddu 0:394261695392 266 correction = -0.5;
ajboddu 0:394261695392 267 setRightPwm(baseSpeed-correction);
ajboddu 0:394261695392 268 setLeftPwm(baseSpeed+correction);
ajboddu 0:394261695392 269 }
ajboddu 0:394261695392 270 }
ajboddu 0:394261695392 271
ajboddu 0:394261695392 272 void postTurnRun(){ //this code runs before and after every turn to ensure that the car is positioned at the right spot when it turns
ajboddu 0:394261695392 273 RENC.reset();
ajboddu 0:394261695392 274 LENC.reset();
ajboddu 0:394261695392 275 int rightPulses = 0;
ajboddu 0:394261695392 276 int leftPulses = 0;
ajboddu 0:394261695392 277 //we want the difference which is the error
ajboddu 0:394261695392 278 while(RENC.getPulses() < postRunLength && LENC.getPulses() < postRunLength) {
ajboddu 0:394261695392 279 rightPulses = RENC.getPulses() - rightPulses;
ajboddu 0:394261695392 280 leftPulses = LENC.getPulses() - leftPulses;
ajboddu 0:394261695392 281 int errorEncoder = rightPulses - leftPulses;
ajboddu 0:394261695392 282 float correction = proportion(errorEncoder);
ajboddu 0:394261695392 283 if(correction > 0.5)
ajboddu 0:394261695392 284 correction = 0.5;
ajboddu 0:394261695392 285 else if(correction < -0.5)
ajboddu 0:394261695392 286 correction = -0.5;
ajboddu 0:394261695392 287 setRightPwm(baseSpeed-correction);
ajboddu 0:394261695392 288 setLeftPwm(baseSpeed+correction);
ajboddu 0:394261695392 289 }
ajboddu 0:394261695392 290 }
ajboddu 0:394261695392 291
ajboddu 0:394261695392 292 int main()
ajboddu 0:394261695392 293 {
ajboddu 0:394261695392 294 wait(0.2);
ajboddu 0:394261695392 295 myled =1;
ajboddu 0:394261695392 296 wait(0.5);
ajboddu 0:394261695392 297 myled =0;
ajboddu 0:394261695392 298 wait(0.2);
ajboddu 0:394261695392 299 setBaselines();
ajboddu 0:394261695392 300 safeToRunSystick = true;
ajboddu 0:394261695392 301 PIDcorrectionTicker.attach(&motorCorrections, refreshTimeSeconds);
ajboddu 0:394261695392 302 while(1) {
ajboddu 0:394261695392 303 //Turns if theres a wall infront of it
ajboddu 0:394261695392 304 myled = !myled;
ajboddu 0:394261695392 305 float leftSSensor = getLSreading();
ajboddu 0:394261695392 306 float rightSSensor = getRSreading();
ajboddu 0:394261695392 307 if((leftSSensor > 0.1 + baselineLS) || (rightSSensor > 0.1 + baselineRS)) {
ajboddu 0:394261695392 308 // turnAround();
ajboddu 0:394261695392 309 }
ajboddu 0:394261695392 310 float leftASensor = getLAreading();
ajboddu 0:394261695392 311 float rightASensor = getRAreading();
ajboddu 0:394261695392 312
ajboddu 0:394261695392 313 float leftAStrig = leftASensor- baselineLA;
ajboddu 0:394261695392 314 float rightAStrig = rightASensor-baselineRA;
ajboddu 0:394261695392 315
ajboddu 0:394261695392 316 if(rightAStrig < rightAngleTurnThreshhold)
ajboddu 0:394261695392 317 turnRight();
ajboddu 0:394261695392 318 else if(leftAStrig < leftAngleTurnThreshhold)
ajboddu 0:394261695392 319 turnLeft();
ajboddu 0:394261695392 320
ajboddu 0:394261695392 321
ajboddu 0:394261695392 322 //pc.printf("Left angle: %.6f \r\n", leftASensor);
ajboddu 0:394261695392 323 // pc.printf("Right angle: %.6f Left angle: %.6f \r\n", rightAStrig, leftAStrig);
ajboddu 0:394261695392 324 // pc.printf("Gyro accum: %.6f \r\n", accumGyro);
ajboddu 0:394261695392 325
ajboddu 0:394261695392 326
ajboddu 0:394261695392 327 }
ajboddu 0:394261695392 328 }