Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Committer:
sahilmgandhi
Date:
Sun May 21 13:04:21 2017 +0000
Revision:
28:8126a4d620e8
Parent:
27:02ce1040f331
Child:
29:ec2c5a69acd6
Final commit before CAMM ... it is not looking so good with floodfill and detecting empty spots in the maze

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sahilmgandhi 0:a03c771ab78e 1 #include "mbed.h"
sahilmgandhi 26:d20f1adac2d3 2
kyleliangus 6:3d68fedd6fd9 3 #include "irpair.h"
kyleliangus 4:b5b7836ca2b0 4 #include "main.h"
kyleliangus 4:b5b7836ca2b0 5 #include "motor.h"
sahilmgandhi 26:d20f1adac2d3 6
sahilmgandhi 26:d20f1adac2d3 7 #include <stdlib.h>
sahilmgandhi 26:d20f1adac2d3 8 #include <stack> // std::stack
sahilmgandhi 26:d20f1adac2d3 9 #include <utility> // std::pair, std::make_pair
christine222 25:f827a8b7880e 10
sahilmgandhi 1:8a4b2f573923 11 #include "ITG3200.h"
sahilmgandhi 14:9e7bb03ddccb 12 #include "stm32f4xx.h"
sahilmgandhi 7:6f5cb6377bd4 13 #include "QEI.h"
sahilmgandhi 26:d20f1adac2d3 14
vanshg 11:8fc2b703086b 15 /* Constants for when HIGH_PWM_VOLTAGE = 0.2
kyleliangus 9:1d8e4da058cd 16 #define IP_CONSTANT 6
kyleliangus 9:1d8e4da058cd 17 #define II_CONSTANT 0
kyleliangus 9:1d8e4da058cd 18 #define ID_CONSTANT 1
vanshg 11:8fc2b703086b 19 */
sahilmgandhi 26:d20f1adac2d3 20
vanshg 11:8fc2b703086b 21 // Constants for when HIGH_PWM_VOLTAGE = 0.1
christine222 21:9a6cb07bdcb6 22 // #define IP_CONSTANT 8.85
christine222 21:9a6cb07bdcb6 23 // #define II_CONSTANT 0.005
christine222 21:9a6cb07bdcb6 24 // #define ID_CONSTANT 3.15
sahilmgandhi 28:8126a4d620e8 25 #define IP_CONSTANT 8.15
sahilmgandhi 28:8126a4d620e8 26 #define II_CONSTANT 0.06
sahilmgandhi 28:8126a4d620e8 27 #define ID_CONSTANT 7.5
sahilmgandhi 26:d20f1adac2d3 28
sahilmgandhi 28:8126a4d620e8 29 const int desiredCount180 = 2700;
christine222 25:f827a8b7880e 30 const int desiredCountR = 1575;
christine222 25:f827a8b7880e 31 const int desiredCountL = 1650;
sahilmgandhi 26:d20f1adac2d3 32
sahilmgandhi 16:d9252437bd92 33 const int oneCellCount = 5400;
sahilmgandhi 28:8126a4d620e8 34 const int oneCellCountMomentum = 4450;//4800; // one cell count is actually approximately 5400, but this value is considering momentum!
sahilmgandhi 26:d20f1adac2d3 35
sahilmgandhi 17:f713758f6238 36 float receiverOneReading = 0.0;
sahilmgandhi 17:f713758f6238 37 float receiverTwoReading = 0.0;
sahilmgandhi 17:f713758f6238 38 float receiverThreeReading = 0.0;
sahilmgandhi 17:f713758f6238 39 float receiverFourReading = 0.0;
sahilmgandhi 17:f713758f6238 40
sahilmgandhi 26:d20f1adac2d3 41 float ir1base = 0.0;
sahilmgandhi 26:d20f1adac2d3 42 float ir2base = 0.0;
sahilmgandhi 26:d20f1adac2d3 43
sahilmgandhi 26:d20f1adac2d3 44 float ir3base = 0.0;
sahilmgandhi 26:d20f1adac2d3 45
sahilmgandhi 26:d20f1adac2d3 46 float ir4base = 0.0;
christine222 23:690b0ca34ee9 47
sahilmgandhi 28:8126a4d620e8 48 float initAverageL = 8.25;
sahilmgandhi 28:8126a4d620e8 49 float averageDivL = 27.8; //blue
christine222 25:f827a8b7880e 50 float initAverageR = 8.75; //4.5
sahilmgandhi 28:8126a4d620e8 51 float averageDivR = 28.5; //red
sahilmgandhi 28:8126a4d620e8 52 float averageDivUpper = 0.5;
christine222 25:f827a8b7880e 53
sahilmgandhi 26:d20f1adac2d3 54 //IRSAvg= >: 0.143701, 0.128285
sahilmgandhi 26:d20f1adac2d3 55
sahilmgandhi 26:d20f1adac2d3 56 //facing wall ir2 and ir3
sahilmgandhi 26:d20f1adac2d3 57 //0.144562, 0.113971 in maze
sahilmgandhi 26:d20f1adac2d3 58
sahilmgandhi 26:d20f1adac2d3 59 // normal hall ir2 and ir3
sahilmgandhi 26:d20f1adac2d3 60 //0.013665, 0.010889 in maze
sahilmgandhi 26:d20f1adac2d3 61
sahilmgandhi 26:d20f1adac2d3 62 //0.014462, 0.009138
sahilmgandhi 26:d20f1adac2d3 63 // 0.013888, 0.010530
sahilmgandhi 26:d20f1adac2d3 64
sahilmgandhi 26:d20f1adac2d3 65
sahilmgandhi 26:d20f1adac2d3 66 //no walls ir2 and ir3
sahilmgandhi 26:d20f1adac2d3 67 //0.003265, 0.002904 in maze9
sahilmgandhi 26:d20f1adac2d3 68
sahilmgandhi 26:d20f1adac2d3 69 //0.003162, 0.003123
sahilmgandhi 26:d20f1adac2d3 70 //0.003795,
sahilmgandhi 26:d20f1adac2d3 71
sahilmgandhi 26:d20f1adac2d3 72 //0.005297, 0.007064
sahilmgandhi 26:d20f1adac2d3 73
sahilmgandhi 26:d20f1adac2d3 74 float noWallR = 0.007;
sahilmgandhi 26:d20f1adac2d3 75 float noWallL = 0.007;
sahilmgandhi 26:d20f1adac2d3 76
sahilmgandhi 16:d9252437bd92 77 void pidOnEncoders();
sahilmgandhi 26:d20f1adac2d3 78
sahilmgandhi 26:d20f1adac2d3 79
sahilmgandhi 14:9e7bb03ddccb 80 void turnLeft()
sahilmgandhi 14:9e7bb03ddccb 81 {
christine222 23:690b0ca34ee9 82 double speed0 = 0.11;
christine222 23:690b0ca34ee9 83 double speed1 = -0.13;
sahilmgandhi 26:d20f1adac2d3 84
christine222 13:2032db00f168 85 int counter = 0;
sahilmgandhi 14:9e7bb03ddccb 86 int initial0 = encoder0.getPulses();
sahilmgandhi 14:9e7bb03ddccb 87 int initial1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 88
sahilmgandhi 14:9e7bb03ddccb 89 int desiredCount0 = initial0 - desiredCountL;
sahilmgandhi 14:9e7bb03ddccb 90 int desiredCount1 = initial1 + desiredCountL;
sahilmgandhi 26:d20f1adac2d3 91
sahilmgandhi 14:9e7bb03ddccb 92 int count0 = initial0;
sahilmgandhi 14:9e7bb03ddccb 93 int count1 = initial1;
sahilmgandhi 26:d20f1adac2d3 94
christine222 13:2032db00f168 95 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 96 double error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 97
sahilmgandhi 14:9e7bb03ddccb 98 while(1) {
sahilmgandhi 26:d20f1adac2d3 99
sahilmgandhi 14:9e7bb03ddccb 100 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
christine222 13:2032db00f168 101 count0 = encoder0.getPulses();
christine222 13:2032db00f168 102 count1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 103
sahilmgandhi 14:9e7bb03ddccb 104 error0 = count0 - desiredCount0;
sahilmgandhi 14:9e7bb03ddccb 105 error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 106
christine222 13:2032db00f168 107 right_motor.move(speed0);
christine222 13:2032db00f168 108 left_motor.move(speed1);
christine222 13:2032db00f168 109 counter = 0;
sahilmgandhi 14:9e7bb03ddccb 110 } else {
christine222 13:2032db00f168 111 counter++;
christine222 13:2032db00f168 112 right_motor.brake();
christine222 13:2032db00f168 113 left_motor.brake();
christine222 13:2032db00f168 114 }
sahilmgandhi 26:d20f1adac2d3 115
sahilmgandhi 14:9e7bb03ddccb 116 if (counter > 60) {
christine222 13:2032db00f168 117 break;
christine222 13:2032db00f168 118 }
sahilmgandhi 14:9e7bb03ddccb 119 }
sahilmgandhi 26:d20f1adac2d3 120
christine222 13:2032db00f168 121 right_motor.brake();
christine222 13:2032db00f168 122 left_motor.brake();
sahilmgandhi 26:d20f1adac2d3 123 turnFlag = 0; // zeroing out the flags!
sahilmgandhi 26:d20f1adac2d3 124 currDir -= 1;
christine222 13:2032db00f168 125 }
sahilmgandhi 26:d20f1adac2d3 126
sahilmgandhi 26:d20f1adac2d3 127 void turnRight()
sahilmgandhi 14:9e7bb03ddccb 128 {
sahilmgandhi 26:d20f1adac2d3 129 double speed0 = -0.11;
sahilmgandhi 26:d20f1adac2d3 130 double speed1 = 0.13;
sahilmgandhi 26:d20f1adac2d3 131
christine222 13:2032db00f168 132 int counter = 0;
sahilmgandhi 14:9e7bb03ddccb 133 int initial0 = encoder0.getPulses();
sahilmgandhi 14:9e7bb03ddccb 134 int initial1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 135
sahilmgandhi 26:d20f1adac2d3 136 int desiredCount0 = initial0 + desiredCountR;
sahilmgandhi 26:d20f1adac2d3 137 int desiredCount1 = initial1 - desiredCountR;
sahilmgandhi 26:d20f1adac2d3 138
sahilmgandhi 14:9e7bb03ddccb 139 int count0 = initial0;
sahilmgandhi 14:9e7bb03ddccb 140 int count1 = initial1;
sahilmgandhi 26:d20f1adac2d3 141
christine222 13:2032db00f168 142 double error0 = count0 - desiredCount0;
christine222 13:2032db00f168 143 double error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 144
sahilmgandhi 14:9e7bb03ddccb 145 while(1) {
sahilmgandhi 26:d20f1adac2d3 146
sahilmgandhi 14:9e7bb03ddccb 147 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
christine222 13:2032db00f168 148 count0 = encoder0.getPulses();
christine222 13:2032db00f168 149 count1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 150
sahilmgandhi 14:9e7bb03ddccb 151 error0 = count0 - desiredCount0;
sahilmgandhi 14:9e7bb03ddccb 152 error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 153
christine222 13:2032db00f168 154 right_motor.move(speed0);
christine222 13:2032db00f168 155 left_motor.move(speed1);
christine222 13:2032db00f168 156 counter = 0;
sahilmgandhi 14:9e7bb03ddccb 157 } else {
christine222 13:2032db00f168 158 counter++;
christine222 13:2032db00f168 159 right_motor.brake();
christine222 13:2032db00f168 160 left_motor.brake();
christine222 13:2032db00f168 161 }
sahilmgandhi 26:d20f1adac2d3 162
sahilmgandhi 26:d20f1adac2d3 163 if (counter > 60) {
sahilmgandhi 26:d20f1adac2d3 164 break;
sahilmgandhi 26:d20f1adac2d3 165 }
sahilmgandhi 26:d20f1adac2d3 166 }
sahilmgandhi 26:d20f1adac2d3 167
sahilmgandhi 26:d20f1adac2d3 168 right_motor.brake();
sahilmgandhi 26:d20f1adac2d3 169 left_motor.brake();
sahilmgandhi 26:d20f1adac2d3 170 turnFlag = 0;
sahilmgandhi 26:d20f1adac2d3 171 currDir += 1;
sahilmgandhi 26:d20f1adac2d3 172 }
sahilmgandhi 26:d20f1adac2d3 173
sahilmgandhi 26:d20f1adac2d3 174 void turnLeft180()
sahilmgandhi 26:d20f1adac2d3 175 {
sahilmgandhi 26:d20f1adac2d3 176 double speed0 = 0.15;
sahilmgandhi 26:d20f1adac2d3 177 double speed1 = -0.15;
sahilmgandhi 26:d20f1adac2d3 178
sahilmgandhi 26:d20f1adac2d3 179 int counter = 0;
sahilmgandhi 26:d20f1adac2d3 180 int initial0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 181 int initial1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 182
sahilmgandhi 26:d20f1adac2d3 183 int desiredCount0 = initial0 - desiredCountL*2;
sahilmgandhi 26:d20f1adac2d3 184 int desiredCount1 = initial1 + desiredCountL*2;
sahilmgandhi 26:d20f1adac2d3 185
sahilmgandhi 26:d20f1adac2d3 186 int count0 = initial0;
sahilmgandhi 26:d20f1adac2d3 187 int count1 = initial1;
sahilmgandhi 26:d20f1adac2d3 188
sahilmgandhi 26:d20f1adac2d3 189 double error0 = count0 - desiredCount0;
sahilmgandhi 26:d20f1adac2d3 190 double error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 191
sahilmgandhi 26:d20f1adac2d3 192
sahilmgandhi 26:d20f1adac2d3 193 while(1) {
sahilmgandhi 26:d20f1adac2d3 194
sahilmgandhi 26:d20f1adac2d3 195 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
sahilmgandhi 26:d20f1adac2d3 196 count0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 197 count1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 198
sahilmgandhi 26:d20f1adac2d3 199 error0 = count0 - desiredCount0;
sahilmgandhi 26:d20f1adac2d3 200 error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 201
sahilmgandhi 26:d20f1adac2d3 202 right_motor.move(speed0);
sahilmgandhi 26:d20f1adac2d3 203 left_motor.move(speed1);
sahilmgandhi 26:d20f1adac2d3 204 counter = 0;
sahilmgandhi 26:d20f1adac2d3 205 } else {
sahilmgandhi 26:d20f1adac2d3 206 counter++;
sahilmgandhi 26:d20f1adac2d3 207 right_motor.brake();
sahilmgandhi 26:d20f1adac2d3 208 left_motor.brake();
sahilmgandhi 26:d20f1adac2d3 209 }
sahilmgandhi 26:d20f1adac2d3 210
sahilmgandhi 26:d20f1adac2d3 211 if (counter > 60) {
sahilmgandhi 26:d20f1adac2d3 212 break;
sahilmgandhi 26:d20f1adac2d3 213 }
sahilmgandhi 26:d20f1adac2d3 214 }
sahilmgandhi 26:d20f1adac2d3 215
sahilmgandhi 26:d20f1adac2d3 216 right_motor.brake();
sahilmgandhi 26:d20f1adac2d3 217 left_motor.brake();
sahilmgandhi 26:d20f1adac2d3 218 currDir -= 2;
sahilmgandhi 26:d20f1adac2d3 219 }
sahilmgandhi 26:d20f1adac2d3 220
sahilmgandhi 26:d20f1adac2d3 221 void turnRight180()
sahilmgandhi 26:d20f1adac2d3 222 {
sahilmgandhi 26:d20f1adac2d3 223 double speed0 = -0.16;
sahilmgandhi 26:d20f1adac2d3 224 double speed1 = 0.16;
sahilmgandhi 26:d20f1adac2d3 225
sahilmgandhi 26:d20f1adac2d3 226 int counter = 0;
sahilmgandhi 26:d20f1adac2d3 227 int initial0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 228 int initial1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 229
sahilmgandhi 26:d20f1adac2d3 230 int desiredCount0 = initial0 + desiredCount180;
sahilmgandhi 26:d20f1adac2d3 231 int desiredCount1 = initial1 - desiredCount180;
sahilmgandhi 26:d20f1adac2d3 232
sahilmgandhi 26:d20f1adac2d3 233 int count0 = initial0;
sahilmgandhi 26:d20f1adac2d3 234 int count1 = initial1;
sahilmgandhi 26:d20f1adac2d3 235
sahilmgandhi 26:d20f1adac2d3 236 double error0 = count0 - desiredCount0;
sahilmgandhi 26:d20f1adac2d3 237 double error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 238
sahilmgandhi 26:d20f1adac2d3 239
sahilmgandhi 26:d20f1adac2d3 240 while(1) {
sahilmgandhi 26:d20f1adac2d3 241
sahilmgandhi 26:d20f1adac2d3 242 if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
sahilmgandhi 26:d20f1adac2d3 243 count0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 244 count1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 245
sahilmgandhi 26:d20f1adac2d3 246 error0 = count0 - desiredCount0;
sahilmgandhi 26:d20f1adac2d3 247 error1 = count1 - desiredCount1;
sahilmgandhi 26:d20f1adac2d3 248
sahilmgandhi 26:d20f1adac2d3 249 right_motor.move(speed0);
sahilmgandhi 26:d20f1adac2d3 250 left_motor.move(speed1);
sahilmgandhi 26:d20f1adac2d3 251 counter = 0;
sahilmgandhi 26:d20f1adac2d3 252 } else {
sahilmgandhi 26:d20f1adac2d3 253 counter++;
sahilmgandhi 26:d20f1adac2d3 254 right_motor.brake();
sahilmgandhi 26:d20f1adac2d3 255 left_motor.brake();
sahilmgandhi 26:d20f1adac2d3 256 }
sahilmgandhi 26:d20f1adac2d3 257
sahilmgandhi 14:9e7bb03ddccb 258 if (counter > 60) {
sahilmgandhi 14:9e7bb03ddccb 259 break;
sahilmgandhi 14:9e7bb03ddccb 260 }
sahilmgandhi 14:9e7bb03ddccb 261 }
sahilmgandhi 14:9e7bb03ddccb 262 right_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 263 left_motor.brake();
sahilmgandhi 20:82836745332e 264 currDir += 2;
sahilmgandhi 14:9e7bb03ddccb 265 }
sahilmgandhi 26:d20f1adac2d3 266
sahilmgandhi 17:f713758f6238 267 void moveForwardCellEncoder(double cellNum){
sahilmgandhi 17:f713758f6238 268 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
sahilmgandhi 17:f713758f6238 269 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
sahilmgandhi 26:d20f1adac2d3 270
sahilmgandhi 20:82836745332e 271 left_motor.forward(0.125);
sahilmgandhi 20:82836745332e 272 right_motor.forward(0.095);
sahilmgandhi 19:7b66a518b6f8 273 wait_ms(1);
sahilmgandhi 16:d9252437bd92 274 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
sahilmgandhi 20:82836745332e 275 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 20:82836745332e 276 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 20:82836745332e 277 // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
sahilmgandhi 26:d20f1adac2d3 278 if (receiverThreeReading < ir3base){
sahilmgandhi 20:82836745332e 279 // redLed.write(1);
sahilmgandhi 20:82836745332e 280 // blueLed.write(0);
sahilmgandhi 19:7b66a518b6f8 281 turnFlag |= RIGHT_FLAG;
sahilmgandhi 20:82836745332e 282 }
sahilmgandhi 26:d20f1adac2d3 283 else if (receiverTwoReading < ir2base){
sahilmgandhi 20:82836745332e 284 // redLed.write(0);
sahilmgandhi 20:82836745332e 285 // blueLed.write(1);
sahilmgandhi 17:f713758f6238 286 turnFlag |= LEFT_FLAG;
sahilmgandhi 20:82836745332e 287 }
sahilmgandhi 16:d9252437bd92 288 pidOnEncoders();
sahilmgandhi 16:d9252437bd92 289 }
sahilmgandhi 26:d20f1adac2d3 290
sahilmgandhi 16:d9252437bd92 291 left_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 292 right_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 293 }
sahilmgandhi 26:d20f1adac2d3 294
sahilmgandhi 26:d20f1adac2d3 295
sahilmgandhi 26:d20f1adac2d3 296 void moveForwardEncoder(double num){
sahilmgandhi 26:d20f1adac2d3 297
christine222 21:9a6cb07bdcb6 298 int count0;
christine222 21:9a6cb07bdcb6 299 int count1;
christine222 21:9a6cb07bdcb6 300 count0 = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 301 count1 = encoder1.getPulses();
christine222 21:9a6cb07bdcb6 302 int initial1 = count1;
christine222 21:9a6cb07bdcb6 303 int initial0 = count0;
christine222 21:9a6cb07bdcb6 304 int diff = count0 - count1;
christine222 21:9a6cb07bdcb6 305 double kp = 0.00015;
christine222 21:9a6cb07bdcb6 306 double kd = 0.00019;
christine222 21:9a6cb07bdcb6 307 int prev = 0;
christine222 25:f827a8b7880e 308
christine222 23:690b0ca34ee9 309 double speed0 = 0.10;
christine222 23:690b0ca34ee9 310 double speed1 = 0.12;
christine222 21:9a6cb07bdcb6 311 right_motor.move(speed0);
christine222 21:9a6cb07bdcb6 312 left_motor.move(speed1);
sahilmgandhi 26:d20f1adac2d3 313
sahilmgandhi 26:d20f1adac2d3 314
sahilmgandhi 26:d20f1adac2d3 315 while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){
christine222 23:690b0ca34ee9 316 //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){
christine222 21:9a6cb07bdcb6 317 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
christine222 21:9a6cb07bdcb6 318
christine222 21:9a6cb07bdcb6 319 count0 = encoder0.getPulses() - initial0;
christine222 21:9a6cb07bdcb6 320 count1 = encoder1.getPulses() - initial1;
christine222 21:9a6cb07bdcb6 321 int x = count0 - count1;
christine222 21:9a6cb07bdcb6 322 //double d = kp * x + kd * ( x - prev );
christine222 21:9a6cb07bdcb6 323 double kppart = kp * x;
christine222 21:9a6cb07bdcb6 324 double kdpart = kd * (x-prev);
christine222 21:9a6cb07bdcb6 325 double d = kppart + kdpart;
christine222 21:9a6cb07bdcb6 326
christine222 21:9a6cb07bdcb6 327 //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
christine222 21:9a6cb07bdcb6 328 if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
christine222 21:9a6cb07bdcb6 329 {
christine222 21:9a6cb07bdcb6 330 left_motor.move( speed1-0.8*d );
christine222 21:9a6cb07bdcb6 331 right_motor.move( speed0+d );
christine222 21:9a6cb07bdcb6 332 }
christine222 21:9a6cb07bdcb6 333 else if( x > diff + 40 )
christine222 21:9a6cb07bdcb6 334 {
christine222 21:9a6cb07bdcb6 335 left_motor.move( speed1-0.8*d );
christine222 21:9a6cb07bdcb6 336 right_motor.move( speed0+d );
christine222 21:9a6cb07bdcb6 337 }
christine222 21:9a6cb07bdcb6 338 // else
christine222 21:9a6cb07bdcb6 339 // {
christine222 21:9a6cb07bdcb6 340 // left_motor.brake();
christine222 21:9a6cb07bdcb6 341 // right_motor.brake();
christine222 21:9a6cb07bdcb6 342 // }
christine222 21:9a6cb07bdcb6 343 prev = x;
christine222 21:9a6cb07bdcb6 344 }
sahilmgandhi 26:d20f1adac2d3 345
christine222 21:9a6cb07bdcb6 346 //pidOnEncoders();
christine222 23:690b0ca34ee9 347 //pidBrake();
christine222 23:690b0ca34ee9 348 right_motor.brake();
christine222 23:690b0ca34ee9 349 left_motor.brake();
christine222 23:690b0ca34ee9 350 return;
christine222 23:690b0ca34ee9 351 }
sahilmgandhi 26:d20f1adac2d3 352
sahilmgandhi 26:d20f1adac2d3 353
christine222 23:690b0ca34ee9 354 void moveForwardWallEncoder(){
sahilmgandhi 26:d20f1adac2d3 355 int count0;
christine222 23:690b0ca34ee9 356 int count1;
christine222 23:690b0ca34ee9 357 count0 = encoder0.getPulses();
christine222 23:690b0ca34ee9 358 count1 = encoder1.getPulses();
christine222 23:690b0ca34ee9 359 int initial1 = count1;
christine222 23:690b0ca34ee9 360 int initial0 = count0;
christine222 23:690b0ca34ee9 361 int diff = count0 - count1;
christine222 23:690b0ca34ee9 362 double kp = 0.00015;
christine222 23:690b0ca34ee9 363 double kd = 0.00019;
christine222 23:690b0ca34ee9 364 int prev = 0;
christine222 25:f827a8b7880e 365
christine222 25:f827a8b7880e 366 double speed0 = 0.11;
christine222 25:f827a8b7880e 367 double speed1 = 0.13;
christine222 23:690b0ca34ee9 368 right_motor.move(speed0);
christine222 23:690b0ca34ee9 369 left_motor.move(speed1);
christine222 23:690b0ca34ee9 370
christine222 25:f827a8b7880e 371 float ir1 = IRP_1.getSamples(50);
christine222 25:f827a8b7880e 372 float ir4 = IRP_4.getSamples(50);
christine222 25:f827a8b7880e 373
christine222 25:f827a8b7880e 374 if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4){
christine222 24:e7063765d6f0 375 return;
christine222 24:e7063765d6f0 376 }
christine222 25:f827a8b7880e 377
christine222 23:690b0ca34ee9 378 //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
christine222 25:f827a8b7880e 379 //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){
christine222 25:f827a8b7880e 380 while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ){
christine222 23:690b0ca34ee9 381 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
christine222 23:690b0ca34ee9 382
christine222 23:690b0ca34ee9 383 count0 = encoder0.getPulses() - initial0;
christine222 23:690b0ca34ee9 384 count1 = encoder1.getPulses() - initial1;
christine222 23:690b0ca34ee9 385 int x = count0 - count1;
christine222 23:690b0ca34ee9 386 //double d = kp * x + kd * ( x - prev );
christine222 23:690b0ca34ee9 387 double kppart = kp * x;
christine222 23:690b0ca34ee9 388 double kdpart = kd * (x-prev);
christine222 23:690b0ca34ee9 389 double d = kppart + kdpart;
christine222 23:690b0ca34ee9 390
christine222 23:690b0ca34ee9 391 //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
christine222 23:690b0ca34ee9 392 if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
christine222 23:690b0ca34ee9 393 {
christine222 23:690b0ca34ee9 394 left_motor.move( speed1-0.8*d );
christine222 23:690b0ca34ee9 395 right_motor.move( speed0+d );
christine222 23:690b0ca34ee9 396 }
christine222 23:690b0ca34ee9 397 else if( x > diff + 40 )
christine222 23:690b0ca34ee9 398 {
christine222 23:690b0ca34ee9 399 left_motor.move( speed1-0.8*d );
christine222 23:690b0ca34ee9 400 right_motor.move( speed0+d );
christine222 23:690b0ca34ee9 401 }
christine222 23:690b0ca34ee9 402 // else
christine222 23:690b0ca34ee9 403 // {
christine222 23:690b0ca34ee9 404 // left_motor.brake();
christine222 23:690b0ca34ee9 405 // right_motor.brake();
christine222 23:690b0ca34ee9 406 // }
christine222 23:690b0ca34ee9 407 prev = x;
christine222 25:f827a8b7880e 408 ir1 = IRP_1.getSamples(50);
christine222 25:f827a8b7880e 409 ir4 = IRP_4.getSamples(50);
christine222 23:690b0ca34ee9 410 }
christine222 25:f827a8b7880e 411
christine222 23:690b0ca34ee9 412 //pidOnEncoders();
christine222 23:690b0ca34ee9 413 //pidBrake();
christine222 23:690b0ca34ee9 414 right_motor.brake();
christine222 23:690b0ca34ee9 415 left_motor.brake();
christine222 21:9a6cb07bdcb6 416 return;
christine222 21:9a6cb07bdcb6 417 }
sahilmgandhi 26:d20f1adac2d3 418
sahilmgandhi 14:9e7bb03ddccb 419 void moveForwardUntilWallIr()
sahilmgandhi 14:9e7bb03ddccb 420 {
kyleliangus 9:1d8e4da058cd 421 double currentError = 0;
kyleliangus 9:1d8e4da058cd 422 double previousError = 0;
kyleliangus 9:1d8e4da058cd 423 double derivError = 0;
kyleliangus 9:1d8e4da058cd 424 double sumError = 0;
christine222 25:f827a8b7880e 425
vanshg 11:8fc2b703086b 426 double HIGH_PWM_VOLTAGE = 0.1;
christine222 25:f827a8b7880e 427
christine222 25:f827a8b7880e 428 double rightSpeed = 0.25;
christine222 25:f827a8b7880e 429 double leftSpeed = 0.23;
christine222 25:f827a8b7880e 430
kyleliangus 9:1d8e4da058cd 431 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 432 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 433
vanshg 11:8fc2b703086b 434 int count = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 435 while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered
christine222 21:9a6cb07bdcb6 436
christine222 21:9a6cb07bdcb6 437 if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
christine222 23:690b0ca34ee9 438 //moveForwardWallEncoder();
christine222 22:681190ff98f0 439 }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone
christine222 22:681190ff98f0 440 //moveForwardRightWall();
christine222 22:681190ff98f0 441 }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone
christine222 22:681190ff98f0 442 //moveForwardLeftWall();
christine222 21:9a6cb07bdcb6 443 }else{
christine222 21:9a6cb07bdcb6 444 // will move forward using encoders only
christine222 21:9a6cb07bdcb6 445 // if cell ahead doesn't have a wall on either one side or both sides
christine222 25:f827a8b7880e 446
christine222 21:9a6cb07bdcb6 447 int pulseCount = (encoder0.getPulses()-count) % 5600;
christine222 21:9a6cb07bdcb6 448 if (pulseCount > 5400 && pulseCount < 5800) {
christine222 21:9a6cb07bdcb6 449 blueLed.write(0);
christine222 21:9a6cb07bdcb6 450 } else {
christine222 21:9a6cb07bdcb6 451 blueLed.write(1);
christine222 21:9a6cb07bdcb6 452 }
christine222 21:9a6cb07bdcb6 453 sumError += currentError;
christine222 25:f827a8b7880e 454 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ;
christine222 21:9a6cb07bdcb6 455 derivError = currentError - previousError;
christine222 21:9a6cb07bdcb6 456 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 21:9a6cb07bdcb6 457 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
christine222 21:9a6cb07bdcb6 458 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 459 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 460 } else { // r is faster than L. speed up l, slow down r
christine222 21:9a6cb07bdcb6 461 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 462 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 463 }
christine222 25:f827a8b7880e 464
christine222 21:9a6cb07bdcb6 465 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 21:9a6cb07bdcb6 466 if (leftSpeed < 0) leftSpeed = 0;
christine222 21:9a6cb07bdcb6 467 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 21:9a6cb07bdcb6 468 if (rightSpeed < 0) rightSpeed = 0;
christine222 25:f827a8b7880e 469
christine222 21:9a6cb07bdcb6 470 right_motor.forward(rightSpeed);
christine222 21:9a6cb07bdcb6 471 left_motor.forward(leftSpeed);
christine222 25:f827a8b7880e 472
christine222 21:9a6cb07bdcb6 473 previousError = currentError;
christine222 25:f827a8b7880e 474
christine222 21:9a6cb07bdcb6 475 ir2 = IRP_2.getSamples( SAMPLE_NUM );
christine222 21:9a6cb07bdcb6 476 ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 477
kyleliangus 9:1d8e4da058cd 478 }
christine222 25:f827a8b7880e 479
christine222 21:9a6cb07bdcb6 480 //backward();
christine222 22:681190ff98f0 481 //wait_ms(40);
christine222 21:9a6cb07bdcb6 482 //brake();
christine222 25:f827a8b7880e 483
christine222 21:9a6cb07bdcb6 484 left_motor.brake();
christine222 21:9a6cb07bdcb6 485 right_motor.brake();
kyleliangus 9:1d8e4da058cd 486 }
vanshg 10:810d1849da9d 487 }
sahilmgandhi 26:d20f1adac2d3 488
sahilmgandhi 14:9e7bb03ddccb 489 void printDipFlag()
sahilmgandhi 14:9e7bb03ddccb 490 {
vanshg 11:8fc2b703086b 491 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 492 }
sahilmgandhi 26:d20f1adac2d3 493
sahilmgandhi 14:9e7bb03ddccb 494 void enableButton1()
sahilmgandhi 14:9e7bb03ddccb 495 {
vanshg 10:810d1849da9d 496 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 497 printDipFlag();
vanshg 10:810d1849da9d 498 }
sahilmgandhi 14:9e7bb03ddccb 499 void enableButton2()
sahilmgandhi 14:9e7bb03ddccb 500 {
vanshg 10:810d1849da9d 501 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 502 printDipFlag();
vanshg 10:810d1849da9d 503 }
sahilmgandhi 14:9e7bb03ddccb 504 void enableButton3()
sahilmgandhi 14:9e7bb03ddccb 505 {
vanshg 10:810d1849da9d 506 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 507 printDipFlag();
vanshg 10:810d1849da9d 508 }
sahilmgandhi 14:9e7bb03ddccb 509 void enableButton4()
sahilmgandhi 14:9e7bb03ddccb 510 {
vanshg 10:810d1849da9d 511 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 512 printDipFlag();
vanshg 10:810d1849da9d 513 }
sahilmgandhi 14:9e7bb03ddccb 514 void disableButton1()
sahilmgandhi 14:9e7bb03ddccb 515 {
vanshg 10:810d1849da9d 516 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 517 printDipFlag();
vanshg 10:810d1849da9d 518 }
sahilmgandhi 14:9e7bb03ddccb 519 void disableButton2()
sahilmgandhi 14:9e7bb03ddccb 520 {
vanshg 10:810d1849da9d 521 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 522 printDipFlag();
vanshg 10:810d1849da9d 523 }
sahilmgandhi 14:9e7bb03ddccb 524 void disableButton3()
sahilmgandhi 14:9e7bb03ddccb 525 {
vanshg 10:810d1849da9d 526 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 527 printDipFlag();
vanshg 10:810d1849da9d 528 }
sahilmgandhi 14:9e7bb03ddccb 529 void disableButton4()
sahilmgandhi 14:9e7bb03ddccb 530 {
vanshg 10:810d1849da9d 531 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 532 printDipFlag();
kyleliangus 9:1d8e4da058cd 533 }
sahilmgandhi 26:d20f1adac2d3 534
kyleliangus 15:b80555a4a8b9 535 void pidOnEncoders()
kyleliangus 15:b80555a4a8b9 536 {
kyleliangus 15:b80555a4a8b9 537 int count0;
kyleliangus 15:b80555a4a8b9 538 int count1;
kyleliangus 15:b80555a4a8b9 539 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 540 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 541 int diff = count0 - count1;
sahilmgandhi 28:8126a4d620e8 542 double kp = 0.00013;
sahilmgandhi 28:8126a4d620e8 543 double kd = 0.00016;
kyleliangus 15:b80555a4a8b9 544 int prev = 0;
sahilmgandhi 26:d20f1adac2d3 545
sahilmgandhi 16:d9252437bd92 546 int counter = 0;
kyleliangus 15:b80555a4a8b9 547 while(1)
kyleliangus 15:b80555a4a8b9 548 {
kyleliangus 15:b80555a4a8b9 549 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 550 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 551 int x = count0 - count1;
kyleliangus 15:b80555a4a8b9 552 //double d = kp * x + kd * ( x - prev );
kyleliangus 15:b80555a4a8b9 553 double kppart = kp * x;
kyleliangus 15:b80555a4a8b9 554 double kdpart = kd * (x-prev);
kyleliangus 15:b80555a4a8b9 555 double d = kppart + kdpart;
kyleliangus 15:b80555a4a8b9 556
kyleliangus 15:b80555a4a8b9 557 //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
sahilmgandhi 28:8126a4d620e8 558 if( x < diff - 60 ) // count1 is bigger, right wheel pushed forward
kyleliangus 15:b80555a4a8b9 559 {
kyleliangus 15:b80555a4a8b9 560 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 561 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 562 }
sahilmgandhi 28:8126a4d620e8 563 else if( x > diff + 60 )
kyleliangus 15:b80555a4a8b9 564 {
kyleliangus 15:b80555a4a8b9 565 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 566 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 567 }
sahilmgandhi 16:d9252437bd92 568 // else
sahilmgandhi 16:d9252437bd92 569 // {
sahilmgandhi 16:d9252437bd92 570 // left_motor.brake();
sahilmgandhi 16:d9252437bd92 571 // right_motor.brake();
sahilmgandhi 16:d9252437bd92 572 // }
kyleliangus 15:b80555a4a8b9 573 prev = x;
sahilmgandhi 16:d9252437bd92 574 counter++;
sahilmgandhi 28:8126a4d620e8 575 if (counter == 10)
sahilmgandhi 16:d9252437bd92 576 break;
kyleliangus 15:b80555a4a8b9 577 }
kyleliangus 15:b80555a4a8b9 578 }
sahilmgandhi 26:d20f1adac2d3 579
christine222 22:681190ff98f0 580 void nCellEncoderAndIR(double cellCount){
sahilmgandhi 19:7b66a518b6f8 581 double currentError = 0;
sahilmgandhi 19:7b66a518b6f8 582 double previousError = 0;
sahilmgandhi 19:7b66a518b6f8 583 double derivError = 0;
sahilmgandhi 19:7b66a518b6f8 584 double sumError = 0;
christine222 25:f827a8b7880e 585
sahilmgandhi 28:8126a4d620e8 586 double HIGH_PWM_VOLTAGE = 0.12;
sahilmgandhi 28:8126a4d620e8 587 double rightSpeed = 0.12;
sahilmgandhi 28:8126a4d620e8 588 double leftSpeed = 0.12;
christine222 25:f827a8b7880e 589
christine222 25:f827a8b7880e 590 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 591 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 592
sahilmgandhi 28:8126a4d620e8 593 left_motor.forward(0.12);
sahilmgandhi 28:8126a4d620e8 594 right_motor.forward(0.10);
christine222 25:f827a8b7880e 595
sahilmgandhi 19:7b66a518b6f8 596 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 597 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 598
christine222 25:f827a8b7880e 599 // float previr2 = ir2;
christine222 25:f827a8b7880e 600 // float previr3 = ir3;
christine222 25:f827a8b7880e 601
christine222 23:690b0ca34ee9 602 int state = 0;
christine222 23:690b0ca34ee9 603
sahilmgandhi 26:d20f1adac2d3 604 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
sahilmgandhi 28:8126a4d620e8 605 receiverTwoReading = IRP_2.getSamples(50);
sahilmgandhi 28:8126a4d620e8 606 receiverThreeReading = IRP_3.getSamples(50);
sahilmgandhi 28:8126a4d620e8 607 receiverOneReading = IRP_1.getSamples(50);
sahilmgandhi 28:8126a4d620e8 608 receiverFourReading = IRP_4.getSamples(50);
christine222 25:f827a8b7880e 609
sahilmgandhi 27:02ce1040f331 610 if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
sahilmgandhi 26:d20f1adac2d3 611 if (currDir % 4 == 0){
sahilmgandhi 27:02ce1040f331 612 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 613 }
sahilmgandhi 26:d20f1adac2d3 614 else if (currDir % 4 == 1){
sahilmgandhi 27:02ce1040f331 615 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 616 }
sahilmgandhi 26:d20f1adac2d3 617 else if (currDir % 4 == 2){
sahilmgandhi 27:02ce1040f331 618 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 619 }
sahilmgandhi 26:d20f1adac2d3 620 else if (currDir % 4 == 3){
sahilmgandhi 27:02ce1040f331 621 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 622 }
sahilmgandhi 26:d20f1adac2d3 623 break;
sahilmgandhi 26:d20f1adac2d3 624 }
christine222 25:f827a8b7880e 625
sahilmgandhi 28:8126a4d620e8 626 if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))){
christine222 25:f827a8b7880e 627 // both sides gone
sahilmgandhi 26:d20f1adac2d3 628 int prev0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 629 int prev1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 630 int diff0 = desiredCount0 - prev0;
sahilmgandhi 26:d20f1adac2d3 631 int diff1 = desiredCount1 - prev1;
sahilmgandhi 26:d20f1adac2d3 632 int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
sahilmgandhi 26:d20f1adac2d3 633 moveForwardEncoder(valToPass);
sahilmgandhi 26:d20f1adac2d3 634 }
sahilmgandhi 28:8126a4d620e8 635 else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR){// right wall gone
sahilmgandhi 27:02ce1040f331 636 // if (currDir % 4 == 0){
sahilmgandhi 27:02ce1040f331 637 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 27:02ce1040f331 638 // }
sahilmgandhi 27:02ce1040f331 639 // else if (currDir % 4 == 1){
sahilmgandhi 27:02ce1040f331 640 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 27:02ce1040f331 641 // }
sahilmgandhi 27:02ce1040f331 642 // else if (currDir % 4 == 2){
sahilmgandhi 27:02ce1040f331 643 // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 27:02ce1040f331 644 // }
sahilmgandhi 27:02ce1040f331 645 // else if (currDir % 4 == 3){
sahilmgandhi 27:02ce1040f331 646 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 27:02ce1040f331 647 // }
christine222 25:f827a8b7880e 648 // RED RED RED RED RED
christine222 25:f827a8b7880e 649 state = 1;
christine222 25:f827a8b7880e 650 redLed.write(0);
christine222 25:f827a8b7880e 651 greenLed.write(1);
christine222 25:f827a8b7880e 652 blueLed.write(1);
sahilmgandhi 28:8126a4d620e8 653 }else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL){// left wall gone
sahilmgandhi 27:02ce1040f331 654 // if (currDir % 4 == 0){
sahilmgandhi 27:02ce1040f331 655 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 27:02ce1040f331 656 // }
sahilmgandhi 27:02ce1040f331 657 // else if (currDir % 4 == 1){
sahilmgandhi 27:02ce1040f331 658 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 27:02ce1040f331 659 // }
sahilmgandhi 27:02ce1040f331 660 // else if (currDir % 4 == 2){
sahilmgandhi 27:02ce1040f331 661 // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 27:02ce1040f331 662 // }
sahilmgandhi 27:02ce1040f331 663 // else if (currDir % 4 == 3){
sahilmgandhi 27:02ce1040f331 664 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 27:02ce1040f331 665 // }
christine222 25:f827a8b7880e 666 // BLUE BLUE BLUE BLUE
christine222 25:f827a8b7880e 667 state = 2;
christine222 25:f827a8b7880e 668 redLed.write(1);
christine222 25:f827a8b7880e 669 greenLed.write(1);
christine222 25:f827a8b7880e 670 blueLed.write(0);
sahilmgandhi 27:02ce1040f331 671 }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
sahilmgandhi 28:8126a4d620e8 672 // if (currDir % 4 == 0){
sahilmgandhi 28:8126a4d620e8 673 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 28:8126a4d620e8 674 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 28:8126a4d620e8 675 // }
sahilmgandhi 28:8126a4d620e8 676 // else if (currDir % 4 == 1){
sahilmgandhi 28:8126a4d620e8 677 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 28:8126a4d620e8 678 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 28:8126a4d620e8 679 // }
sahilmgandhi 28:8126a4d620e8 680 // else if (currDir % 4 == 2){
sahilmgandhi 28:8126a4d620e8 681 // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 28:8126a4d620e8 682 // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 28:8126a4d620e8 683 // }
sahilmgandhi 28:8126a4d620e8 684 // else if (currDir % 4 == 3){
sahilmgandhi 28:8126a4d620e8 685 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 28:8126a4d620e8 686 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 28:8126a4d620e8 687 // }
christine222 25:f827a8b7880e 688 // both walls there
christine222 25:f827a8b7880e 689 state = 0;
christine222 25:f827a8b7880e 690 redLed.write(1);
christine222 25:f827a8b7880e 691 greenLed.write(0);
christine222 25:f827a8b7880e 692 blueLed.write(1);
christine222 25:f827a8b7880e 693 }
christine222 25:f827a8b7880e 694
christine222 25:f827a8b7880e 695 switch(state){
christine222 25:f827a8b7880e 696 case(0):{ // both walls there
christine222 25:f827a8b7880e 697 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 698 break;
christine222 25:f827a8b7880e 699 }
christine222 25:f827a8b7880e 700 case(1):{// RED RED RED RED RED
christine222 25:f827a8b7880e 701 currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
christine222 25:f827a8b7880e 702 break;
christine222 25:f827a8b7880e 703 }
christine222 25:f827a8b7880e 704 case(2):{// blue
christine222 25:f827a8b7880e 705 currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 706 break;
christine222 25:f827a8b7880e 707 }
christine222 25:f827a8b7880e 708 default:{
christine222 25:f827a8b7880e 709 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 710 break;
christine222 25:f827a8b7880e 711 }
christine222 25:f827a8b7880e 712 }
christine222 25:f827a8b7880e 713
christine222 25:f827a8b7880e 714 sumError += currentError;
christine222 25:f827a8b7880e 715 derivError = currentError - previousError;
christine222 25:f827a8b7880e 716 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 25:f827a8b7880e 717 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
christine222 25:f827a8b7880e 718 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 719 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 720 } else { // r is faster than L. speed up l, slow down r
christine222 25:f827a8b7880e 721 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 722 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 723 }
christine222 25:f827a8b7880e 724 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 25:f827a8b7880e 725 if (leftSpeed < 0) leftSpeed = 0;
christine222 25:f827a8b7880e 726 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 25:f827a8b7880e 727 if (rightSpeed < 0) rightSpeed = 0;
sahilmgandhi 26:d20f1adac2d3 728
christine222 25:f827a8b7880e 729 right_motor.forward(rightSpeed);
christine222 25:f827a8b7880e 730 left_motor.forward(leftSpeed);
christine222 25:f827a8b7880e 731 pidOnEncoders();
sahilmgandhi 26:d20f1adac2d3 732
christine222 25:f827a8b7880e 733 previousError = currentError;
sahilmgandhi 26:d20f1adac2d3 734 }
sahilmgandhi 28:8126a4d620e8 735 if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1){
sahilmgandhi 28:8126a4d620e8 736 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 737 mouseY += 1;
sahilmgandhi 28:8126a4d620e8 738 }
sahilmgandhi 28:8126a4d620e8 739 else if (currDir % 4 == 1){
sahilmgandhi 28:8126a4d620e8 740 mouseX + 1;
sahilmgandhi 28:8126a4d620e8 741 }
sahilmgandhi 28:8126a4d620e8 742 else if (currDir % 4 == 2){
sahilmgandhi 28:8126a4d620e8 743 mouseY -= 1;
sahilmgandhi 28:8126a4d620e8 744 }
sahilmgandhi 28:8126a4d620e8 745 else if (currDir % 4 == 3){
sahilmgandhi 28:8126a4d620e8 746 mouseX -= 1;
sahilmgandhi 28:8126a4d620e8 747 }
sahilmgandhi 26:d20f1adac2d3 748 }
sahilmgandhi 26:d20f1adac2d3 749
sahilmgandhi 19:7b66a518b6f8 750 left_motor.brake();
sahilmgandhi 19:7b66a518b6f8 751 right_motor.brake();
sahilmgandhi 17:f713758f6238 752 }
christine222 25:f827a8b7880e 753
sahilmgandhi 26:d20f1adac2d3 754 bool isWallInFront(int x, int y){
sahilmgandhi 27:02ce1040f331 755 return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
sahilmgandhi 26:d20f1adac2d3 756 }
sahilmgandhi 26:d20f1adac2d3 757 bool isWallInBack(int x, int y){
sahilmgandhi 27:02ce1040f331 758 return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
sahilmgandhi 26:d20f1adac2d3 759 }
sahilmgandhi 26:d20f1adac2d3 760 bool isWallOnRight(int x, int y){
sahilmgandhi 27:02ce1040f331 761 return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
sahilmgandhi 26:d20f1adac2d3 762 }
sahilmgandhi 26:d20f1adac2d3 763 bool isWallOnLeft(int x, int y){
sahilmgandhi 27:02ce1040f331 764 return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
sahilmgandhi 26:d20f1adac2d3 765 }
sahilmgandhi 26:d20f1adac2d3 766
sahilmgandhi 26:d20f1adac2d3 767 int chooseNextMovement(){
sahilmgandhi 26:d20f1adac2d3 768 int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
sahilmgandhi 26:d20f1adac2d3 769 if (goingToCenter && (currentDistance == 0)){
sahilmgandhi 26:d20f1adac2d3 770 goingToCenter = false;
sahilmgandhi 26:d20f1adac2d3 771 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 772 }
sahilmgandhi 26:d20f1adac2d3 773 else if (!goingToCenter && (currentDistance == 0)){
sahilmgandhi 26:d20f1adac2d3 774 goingToCenter == true;
sahilmgandhi 26:d20f1adac2d3 775 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 776 }
sahilmgandhi 26:d20f1adac2d3 777
sahilmgandhi 26:d20f1adac2d3 778 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 779 int currX = 0;
sahilmgandhi 26:d20f1adac2d3 780 int currY = 0;
sahilmgandhi 26:d20f1adac2d3 781 int currDist = 0;
sahilmgandhi 26:d20f1adac2d3 782 int dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 783 if (!justTurned){
sahilmgandhi 26:d20f1adac2d3 784 cellsToVisit.push(make_pair(mouseX, mouseY));
sahilmgandhi 26:d20f1adac2d3 785 while (!cellsToVisit.empty()) {
sahilmgandhi 26:d20f1adac2d3 786 pair<int, int> curr = cellsToVisit.top();
sahilmgandhi 26:d20f1adac2d3 787 cellsToVisit.pop();
sahilmgandhi 26:d20f1adac2d3 788 currX = curr.first;
sahilmgandhi 26:d20f1adac2d3 789 currY = curr.second;
sahilmgandhi 26:d20f1adac2d3 790 currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
sahilmgandhi 26:d20f1adac2d3 791 int minDist = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 792
sahilmgandhi 26:d20f1adac2d3 793 if (hasVisited(currX, currY)) { // then we want to actually see where the walls are, else we treat it as if there are no walls!
sahilmgandhi 26:d20f1adac2d3 794 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 795 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 796 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 797 }
sahilmgandhi 26:d20f1adac2d3 798 }
sahilmgandhi 27:02ce1040f331 799 if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 800 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 801 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 802 }
sahilmgandhi 26:d20f1adac2d3 803 }
sahilmgandhi 27:02ce1040f331 804 if (currY + 1 < MAZE_LEN && !isWallInFront( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 805 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 806 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 807 }
sahilmgandhi 26:d20f1adac2d3 808 }
sahilmgandhi 27:02ce1040f331 809 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 810 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 811 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 812 }
sahilmgandhi 26:d20f1adac2d3 813 }
sahilmgandhi 26:d20f1adac2d3 814 } else {
sahilmgandhi 26:d20f1adac2d3 815 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 816 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 817 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 818 }
sahilmgandhi 26:d20f1adac2d3 819 }
sahilmgandhi 26:d20f1adac2d3 820 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 821 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 822 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 823 }
sahilmgandhi 26:d20f1adac2d3 824 }
sahilmgandhi 26:d20f1adac2d3 825 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 826 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 827 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 828 }
sahilmgandhi 26:d20f1adac2d3 829 }
sahilmgandhi 26:d20f1adac2d3 830 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 831 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 832 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 833 }
sahilmgandhi 26:d20f1adac2d3 834 }
sahilmgandhi 26:d20f1adac2d3 835 }
sahilmgandhi 26:d20f1adac2d3 836
sahilmgandhi 26:d20f1adac2d3 837 if (minDist == INT_MAX)
sahilmgandhi 26:d20f1adac2d3 838 continue;
sahilmgandhi 26:d20f1adac2d3 839 if (currDist > minDist)
sahilmgandhi 26:d20f1adac2d3 840 continue;
sahilmgandhi 26:d20f1adac2d3 841 if (currDist <= minDist) {
sahilmgandhi 26:d20f1adac2d3 842 manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
sahilmgandhi 26:d20f1adac2d3 843 }
sahilmgandhi 26:d20f1adac2d3 844 if (hasVisited(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 845 if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 846 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 847 }
sahilmgandhi 26:d20f1adac2d3 848 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 849 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 850 }
sahilmgandhi 26:d20f1adac2d3 851 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 852 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 853 }
sahilmgandhi 26:d20f1adac2d3 854 if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 855 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 856 }
sahilmgandhi 26:d20f1adac2d3 857 } else {
sahilmgandhi 26:d20f1adac2d3 858 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 859 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 860 }
sahilmgandhi 26:d20f1adac2d3 861 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 862 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 863 }
sahilmgandhi 26:d20f1adac2d3 864 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 865 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 866 }
sahilmgandhi 26:d20f1adac2d3 867 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 868 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 869 }
sahilmgandhi 26:d20f1adac2d3 870 }
sahilmgandhi 26:d20f1adac2d3 871 }
sahilmgandhi 26:d20f1adac2d3 872
sahilmgandhi 26:d20f1adac2d3 873 int minDistance = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 874 if (currDir % 4 == 0) {
sahilmgandhi 26:d20f1adac2d3 875 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 876 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 877 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 878 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 879 }
sahilmgandhi 26:d20f1adac2d3 880 }
sahilmgandhi 26:d20f1adac2d3 881 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 882 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 883 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 884 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 885 }
sahilmgandhi 26:d20f1adac2d3 886 }
sahilmgandhi 26:d20f1adac2d3 887 if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 888 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 889 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 890 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 891 }
sahilmgandhi 26:d20f1adac2d3 892 }
sahilmgandhi 26:d20f1adac2d3 893 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 894 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 895 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 896 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 897 }
sahilmgandhi 26:d20f1adac2d3 898 }
sahilmgandhi 26:d20f1adac2d3 899 } else if (currDir % 4 == 2) {
sahilmgandhi 26:d20f1adac2d3 900 if (mouseX - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 901 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 902 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 903 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 904 }
sahilmgandhi 26:d20f1adac2d3 905 }
sahilmgandhi 26:d20f1adac2d3 906 if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 907 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 908 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 909 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 910 }
sahilmgandhi 26:d20f1adac2d3 911 }
sahilmgandhi 26:d20f1adac2d3 912 if (mouseY - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 913 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 914 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 915 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 916 }
sahilmgandhi 26:d20f1adac2d3 917 }
sahilmgandhi 26:d20f1adac2d3 918 if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 919 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 920 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 921 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 922 }
sahilmgandhi 26:d20f1adac2d3 923 }
sahilmgandhi 26:d20f1adac2d3 924 } else if (currDir % 4 == 1) {
sahilmgandhi 26:d20f1adac2d3 925 if (mouseY - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 926 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 927 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 928 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 929 }
sahilmgandhi 26:d20f1adac2d3 930 }
sahilmgandhi 26:d20f1adac2d3 931 if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 932 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 933 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 934 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 935 }
sahilmgandhi 26:d20f1adac2d3 936 }
sahilmgandhi 26:d20f1adac2d3 937 if (mouseX + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 938 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 939 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 940 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 941 }
sahilmgandhi 26:d20f1adac2d3 942 }
sahilmgandhi 26:d20f1adac2d3 943 if (mouseX - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 944 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 945 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 946 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 947 }
sahilmgandhi 26:d20f1adac2d3 948 }
sahilmgandhi 26:d20f1adac2d3 949 } else if (currDir % 4 == 3) {
sahilmgandhi 26:d20f1adac2d3 950 if (mouseY + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 951 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 952 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 953 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 954 }
sahilmgandhi 26:d20f1adac2d3 955 }
sahilmgandhi 26:d20f1adac2d3 956 if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 957 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 958 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 959 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 960 }
sahilmgandhi 26:d20f1adac2d3 961 }
sahilmgandhi 26:d20f1adac2d3 962 if (mouseX - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 963 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 964 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 965 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 966 }
sahilmgandhi 26:d20f1adac2d3 967 }
sahilmgandhi 26:d20f1adac2d3 968 if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 969 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 970 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 971 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 972 }
sahilmgandhi 26:d20f1adac2d3 973 }
sahilmgandhi 26:d20f1adac2d3 974 }
sahilmgandhi 26:d20f1adac2d3 975 }
sahilmgandhi 26:d20f1adac2d3 976 else{
sahilmgandhi 26:d20f1adac2d3 977 justTurned = false;
sahilmgandhi 26:d20f1adac2d3 978 dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 979 }
sahilmgandhi 26:d20f1adac2d3 980
sahilmgandhi 26:d20f1adac2d3 981 return dirToGo;
sahilmgandhi 26:d20f1adac2d3 982 }
sahilmgandhi 26:d20f1adac2d3 983
sahilmgandhi 26:d20f1adac2d3 984 bool hasVisited(int x, int y){
sahilmgandhi 26:d20f1adac2d3 985 return visitedCells[MAZE_LEN - 1 - y][x];
sahilmgandhi 26:d20f1adac2d3 986 }
sahilmgandhi 26:d20f1adac2d3 987
sahilmgandhi 26:d20f1adac2d3 988 void changeManhattanDistance(bool headCenter){
sahilmgandhi 26:d20f1adac2d3 989 if (headCenter){
sahilmgandhi 26:d20f1adac2d3 990 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 991 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 992 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(7 - i);
sahilmgandhi 26:d20f1adac2d3 993 }
sahilmgandhi 26:d20f1adac2d3 994 }
sahilmgandhi 26:d20f1adac2d3 995 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 996 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 997 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(8 - i);
sahilmgandhi 26:d20f1adac2d3 998 }
sahilmgandhi 26:d20f1adac2d3 999 }
sahilmgandhi 26:d20f1adac2d3 1000 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1001 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1002 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(7 - i);
sahilmgandhi 26:d20f1adac2d3 1003 }
sahilmgandhi 26:d20f1adac2d3 1004 }
sahilmgandhi 26:d20f1adac2d3 1005 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1006 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1007 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(8 - i);
sahilmgandhi 26:d20f1adac2d3 1008 }
sahilmgandhi 26:d20f1adac2d3 1009 }
sahilmgandhi 26:d20f1adac2d3 1010 }
sahilmgandhi 26:d20f1adac2d3 1011 else {
sahilmgandhi 26:d20f1adac2d3 1012 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1013 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1014 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1015 }
sahilmgandhi 26:d20f1adac2d3 1016 }
sahilmgandhi 26:d20f1adac2d3 1017 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1018 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1019 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1020 }
sahilmgandhi 26:d20f1adac2d3 1021 }
sahilmgandhi 26:d20f1adac2d3 1022 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1023 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1024 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1025 }
sahilmgandhi 26:d20f1adac2d3 1026 }
sahilmgandhi 26:d20f1adac2d3 1027 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1028 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1029 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1030 }
sahilmgandhi 26:d20f1adac2d3 1031 }
sahilmgandhi 26:d20f1adac2d3 1032 }
sahilmgandhi 26:d20f1adac2d3 1033 }
sahilmgandhi 26:d20f1adac2d3 1034
sahilmgandhi 0:a03c771ab78e 1035 int main()
sahilmgandhi 0:a03c771ab78e 1036 {
christine222 3:880f15be8c72 1037 //Set highest bandwidth.
christine222 23:690b0ca34ee9 1038 //gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 1039 serial.baud(9600);
christine222 23:690b0ca34ee9 1040 //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 26:d20f1adac2d3 1041
sahilmgandhi 1:8a4b2f573923 1042 wait (0.1);
sahilmgandhi 26:d20f1adac2d3 1043
sahilmgandhi 2:771db996cee0 1044 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 1045 greenLed.write(0);
sahilmgandhi 2:771db996cee0 1046 blueLed.write(1);
sahilmgandhi 26:d20f1adac2d3 1047
kyleliangus 9:1d8e4da058cd 1048 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 1049 //right_motor.forward(0.1);
sahilmgandhi 26:d20f1adac2d3 1050
kyleliangus 8:a0760acdc59e 1051 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 1052 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 1053 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 1054 // PB_3 is B of left
vanshg 11:8fc2b703086b 1055 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 1056 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 1057
vanshg 10:810d1849da9d 1058 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 1059 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 1060 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 1061 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 1062 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 1063 */
sahilmgandhi 26:d20f1adac2d3 1064
kyleliangus 12:5790e56a056f 1065 // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type
kyleliangus 12:5790e56a056f 1066 // of alternate function specified
kyleliangus 12:5790e56a056f 1067 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 1068 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 1069 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 1070 //
kyleliangus 12:5790e56a056f 1071 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 1072 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 1073 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 1074 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 1075 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 1076 // CR1 reenabales
kyleliangus 12:5790e56a056f 1077 // then read CNT reg of timer
sahilmgandhi 26:d20f1adac2d3 1078
sahilmgandhi 26:d20f1adac2d3 1079
vanshg 10:810d1849da9d 1080 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 1081 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 1082 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 1083 dipButton4.rise(&enableButton4);
sahilmgandhi 26:d20f1adac2d3 1084
vanshg 10:810d1849da9d 1085 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 1086 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 1087 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 1088 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 1089
sahilmgandhi 26:d20f1adac2d3 1090 // if(dipFlags == 0x1){
sahilmgandhi 26:d20f1adac2d3 1091 turnRight180();
sahilmgandhi 26:d20f1adac2d3 1092 wait_ms(1000);
sahilmgandhi 26:d20f1adac2d3 1093 // }else{
sahilmgandhi 26:d20f1adac2d3 1094 // turnRight();
sahilmgandhi 26:d20f1adac2d3 1095 // IRP_1.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 1096 // IRP_4.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 1097 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 1098 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 1099
sahilmgandhi 26:d20f1adac2d3 1100 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 1101 // turnLeft();
sahilmgandhi 26:d20f1adac2d3 1102 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 1103 // }
sahilmgandhi 26:d20f1adac2d3 1104
sahilmgandhi 26:d20f1adac2d3 1105
sahilmgandhi 26:d20f1adac2d3 1106 // init the wall, and mouse loc arrays:
sahilmgandhi 26:d20f1adac2d3 1107 wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
sahilmgandhi 26:d20f1adac2d3 1108 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 1109
sahilmgandhi 28:8126a4d620e8 1110 int prevEncoder0Count = 0;
sahilmgandhi 28:8126a4d620e8 1111 int prevEncoder1Count = 0;
sahilmgandhi 28:8126a4d620e8 1112 int currEncoder0Count = 0;
sahilmgandhi 28:8126a4d620e8 1113 int currEncoder1Count = 0;
sahilmgandhi 28:8126a4d620e8 1114
sahilmgandhi 28:8126a4d620e8 1115 bool overrideFloodFill = false;
christine222 25:f827a8b7880e 1116 //right_motor.forward( 0.2 );
christine222 25:f827a8b7880e 1117 //left_motor.forward( 0.2 );
sahilmgandhi 26:d20f1adac2d3 1118 //turnRight180();
sahilmgandhi 26:d20f1adac2d3 1119 //wait_ms(1500);
sahilmgandhi 26:d20f1adac2d3 1120 int nextMovement = 0;
sahilmgandhi 26:d20f1adac2d3 1121 while (1) {
sahilmgandhi 28:8126a4d620e8 1122 prevEncoder0Count = encoder0.getPulses();
sahilmgandhi 28:8126a4d620e8 1123 prevEncoder1Count = encoder1.getPulses();
sahilmgandhi 28:8126a4d620e8 1124
sahilmgandhi 28:8126a4d620e8 1125 if (!overrideFloodFill){
sahilmgandhi 28:8126a4d620e8 1126 nextMovement = chooseNextMovement();
sahilmgandhi 28:8126a4d620e8 1127 if (nextMovement == 0){
sahilmgandhi 28:8126a4d620e8 1128 nCellEncoderAndIR(1);
sahilmgandhi 28:8126a4d620e8 1129 }
sahilmgandhi 28:8126a4d620e8 1130 else if (nextMovement == 1){
sahilmgandhi 28:8126a4d620e8 1131 justTurned = true;
sahilmgandhi 28:8126a4d620e8 1132 turnRight();
sahilmgandhi 28:8126a4d620e8 1133 }
sahilmgandhi 28:8126a4d620e8 1134 else if (nextMovement == 2){
sahilmgandhi 28:8126a4d620e8 1135 justTurned = true;
sahilmgandhi 28:8126a4d620e8 1136 turnLeft();
sahilmgandhi 28:8126a4d620e8 1137 }
sahilmgandhi 28:8126a4d620e8 1138 else if (nextMovement == 3){
sahilmgandhi 28:8126a4d620e8 1139 nCellEncoderAndIR(1);
sahilmgandhi 28:8126a4d620e8 1140 }
sahilmgandhi 28:8126a4d620e8 1141 else if (nextMovement == 4){
sahilmgandhi 28:8126a4d620e8 1142 justTurned = true;
sahilmgandhi 28:8126a4d620e8 1143 turnRight180();
sahilmgandhi 28:8126a4d620e8 1144 }
sahilmgandhi 26:d20f1adac2d3 1145 }
sahilmgandhi 28:8126a4d620e8 1146 else{
sahilmgandhi 28:8126a4d620e8 1147 overrideFloodFill = false;
sahilmgandhi 28:8126a4d620e8 1148 if ((rand()%2 + 1) == 1){
sahilmgandhi 28:8126a4d620e8 1149 justTurned = true;
sahilmgandhi 28:8126a4d620e8 1150 turnLeft();
sahilmgandhi 28:8126a4d620e8 1151 }
sahilmgandhi 28:8126a4d620e8 1152 else{
sahilmgandhi 28:8126a4d620e8 1153 justTurned = true;
sahilmgandhi 28:8126a4d620e8 1154 turnRight();
sahilmgandhi 28:8126a4d620e8 1155 }
sahilmgandhi 26:d20f1adac2d3 1156 }
sahilmgandhi 28:8126a4d620e8 1157 currEncoder0Count = encoder0.getPulses();
sahilmgandhi 28:8126a4d620e8 1158 currEncoder1Count = encoder1.getPulses();
sahilmgandhi 28:8126a4d620e8 1159
sahilmgandhi 28:8126a4d620e8 1160 if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
sahilmgandhi 28:8126a4d620e8 1161 overrideFloodFill = true;
sahilmgandhi 26:d20f1adac2d3 1162 }
sahilmgandhi 28:8126a4d620e8 1163
sahilmgandhi 26:d20f1adac2d3 1164 wait_ms(1000); // reduce this once we fine tune this!
christine222 24:e7063765d6f0 1165
christine222 23:690b0ca34ee9 1166 //wait_ms(1500);
christine222 23:690b0ca34ee9 1167 //turnRight();
christine222 23:690b0ca34ee9 1168 //wait_ms(1500);
christine222 23:690b0ca34ee9 1169 //turnLeft();
sahilmgandhi 26:d20f1adac2d3 1170 // nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 1171 // wait_ms(500);
christine222 23:690b0ca34ee9 1172 // turnRight();
christine222 23:690b0ca34ee9 1173 // wait_ms(500);
christine222 23:690b0ca34ee9 1174 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 1175 // wait_ms(500);
christine222 23:690b0ca34ee9 1176 // turnRight();
christine222 23:690b0ca34ee9 1177 // wait_ms(500);
christine222 23:690b0ca34ee9 1178 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 1179 // wait_ms(500);
christine222 23:690b0ca34ee9 1180 // turnLeft();
christine222 23:690b0ca34ee9 1181 // wait_ms(500);
christine222 23:690b0ca34ee9 1182 // nCellEncoderAndIR(2);
christine222 23:690b0ca34ee9 1183 // wait_ms(500);
christine222 23:690b0ca34ee9 1184 // turnRight();
christine222 23:690b0ca34ee9 1185 // wait_ms(500);
christine222 23:690b0ca34ee9 1186 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 1187 // wait_ms(500);
christine222 23:690b0ca34ee9 1188 // turnRight();
christine222 23:690b0ca34ee9 1189 // wait_ms(500);
christine222 23:690b0ca34ee9 1190 // nCellEncoderAndIR(5);
christine222 23:690b0ca34ee9 1191 // break;
christine222 23:690b0ca34ee9 1192 // turnRight180();
sahilmgandhi 26:d20f1adac2d3 1193
christine222 23:690b0ca34ee9 1194 // int number = rand() % 4 + 1;
christine222 23:690b0ca34ee9 1195 // switch(number){
christine222 23:690b0ca34ee9 1196 // case(1):{//turn right
christine222 23:690b0ca34ee9 1197 // turnRight();
christine222 23:690b0ca34ee9 1198 // break;
christine222 23:690b0ca34ee9 1199 // }
christine222 23:690b0ca34ee9 1200 // case(2):{ // turn left
christine222 23:690b0ca34ee9 1201 // turnLeft();
christine222 23:690b0ca34ee9 1202 // break;
christine222 23:690b0ca34ee9 1203 // }
christine222 23:690b0ca34ee9 1204 // case(3):{// keep going
sahilmgandhi 26:d20f1adac2d3 1205
christine222 23:690b0ca34ee9 1206 // break;
christine222 23:690b0ca34ee9 1207 // }
christine222 23:690b0ca34ee9 1208 // case(4):{// turnaround
christine222 23:690b0ca34ee9 1209 // turnRight180();
christine222 23:690b0ca34ee9 1210 // break;
christine222 23:690b0ca34ee9 1211 // }
christine222 23:690b0ca34ee9 1212 // default:{// keep going
christine222 23:690b0ca34ee9 1213 // break;
christine222 23:690b0ca34ee9 1214 // }
christine222 23:690b0ca34ee9 1215 // }
sahilmgandhi 26:d20f1adac2d3 1216
christine222 23:690b0ca34ee9 1217 // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
christine222 23:690b0ca34ee9 1218 // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
sahilmgandhi 26:d20f1adac2d3 1219
christine222 23:690b0ca34ee9 1220 // float ir3 = IRP_2.getSamples(100)/initAverageL;
christine222 23:690b0ca34ee9 1221 // float ir2 = IRP_3.getSamples(100)/initAverageR;
christine222 22:681190ff98f0 1222
sahilmgandhi 26:d20f1adac2d3 1223
christine222 25:f827a8b7880e 1224
christine222 23:690b0ca34ee9 1225 /*
christine222 23:690b0ca34ee9 1226 counter2++;
christine222 23:690b0ca34ee9 1227 counter3++;
christine222 23:690b0ca34ee9 1228 ir2tot += IRP_2.getSamples(100);
christine222 23:690b0ca34ee9 1229 ir3tot += IRP_3.getSamples(100);
sahilmgandhi 26:d20f1adac2d3 1230
sahilmgandhi 26:d20f1adac2d3 1231
christine222 23:690b0ca34ee9 1232 ir2 = ir2tot/counter2;
christine222 23:690b0ca34ee9 1233 ir3 = ir3tot/counter3;
christine222 23:690b0ca34ee9 1234
sahilmgandhi 26:d20f1adac2d3 1235
christine222 23:690b0ca34ee9 1236 serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1237 */
christine222 23:690b0ca34ee9 1238 //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
christine222 23:690b0ca34ee9 1239 //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3);
christine222 23:690b0ca34ee9 1240 //break;
christine222 23:690b0ca34ee9 1241 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
christine222 23:690b0ca34ee9 1242 //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1243 //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
sahilmgandhi 26:d20f1adac2d3 1244
sahilmgandhi 26:d20f1adac2d3 1245
christine222 23:690b0ca34ee9 1246 ////////////////////////////////////////////////////////////////
sahilmgandhi 26:d20f1adac2d3 1247
christine222 22:681190ff98f0 1248 //nCellEncoderAndIR(3);
christine222 22:681190ff98f0 1249 //break;
sahilmgandhi 26:d20f1adac2d3 1250
christine222 23:690b0ca34ee9 1251 //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100));
christine222 25:f827a8b7880e 1252
christine222 21:9a6cb07bdcb6 1253 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
sahilmgandhi 26:d20f1adac2d3 1254
christine222 21:9a6cb07bdcb6 1255 //break;
christine222 21:9a6cb07bdcb6 1256 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1257 // wait(0.5);
christine222 21:9a6cb07bdcb6 1258 // handleTurns();
christine222 21:9a6cb07bdcb6 1259 // wait(0.5);
christine222 21:9a6cb07bdcb6 1260 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1261 // wait(0.5);
christine222 21:9a6cb07bdcb6 1262 // handleTurns();
christine222 21:9a6cb07bdcb6 1263 //break;
sahilmgandhi 16:d9252437bd92 1264 //pidOnEncoders();
sahilmgandhi 20:82836745332e 1265 // moveForwardUntilWallIr();
kyleliangus 8:a0760acdc59e 1266 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
kyleliangus 15:b80555a4a8b9 1267 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
sahilmgandhi 17:f713758f6238 1268 // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
christine222 21:9a6cb07bdcb6 1269 //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
sahilmgandhi 26:d20f1adac2d3 1270
christine222 3:880f15be8c72 1271 //reading = Rec_4.read();
christine222 3:880f15be8c72 1272 // serial.printf("reading: %f\n", reading);
christine222 3:880f15be8c72 1273 }
sahilmgandhi 26:d20f1adac2d3 1274 }