MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@27:02ce1040f331, 2017-05-21 (annotated)

Committer:
sahilmgandhi
Date:
Sun May 21 10:30:19 2017 +0000
Revision:
27:02ce1040f331
Parent:
26:d20f1adac2d3
Child:
28:8126a4d620e8
Modifying floodfill

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
christine222 25:f827a8b7880e 25 #define IP_CONSTANT 8.5
christine222 21:9a6cb07bdcb6 26 #define II_CONSTANT 0.095
christine222 25:f827a8b7880e 27 #define ID_CONSTANT 6.85
sahilmgandhi 26:d20f1adac2d3 28
sahilmgandhi 26:d20f1adac2d3 29 const int desiredCount180 = 2800;
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 26:d20f1adac2d3 34 const int oneCellCountMomentum = 4700;//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
christine222 25:f827a8b7880e 48 float initAverageL = 8.28;
sahilmgandhi 27:02ce1040f331 49 float averageDivL = 29.5; //blue
christine222 25:f827a8b7880e 50 float initAverageR = 8.75; //4.5
sahilmgandhi 27:02ce1040f331 51 float averageDivR = 29.5; //red
christine222 25:f827a8b7880e 52 float averageDivUpper = 0.9;
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 20:82836745332e 542 double kp = 0.00011;
sahilmgandhi 20:82836745332e 543 double kd = 0.00014;
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 20:82836745332e 558 if( x < diff - 40 ) // 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 20:82836745332e 563 else if( x > diff + 40 )
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 19:7b66a518b6f8 575 if (counter == 5)
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
christine222 25:f827a8b7880e 586 double HIGH_PWM_VOLTAGE = 0.1;
christine222 25:f827a8b7880e 587 double rightSpeed = 0.10;
christine222 25:f827a8b7880e 588 double leftSpeed = 0.10;
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
christine222 25:f827a8b7880e 593 left_motor.forward(0.28);
christine222 25:f827a8b7880e 594 right_motor.forward(0.25);
christine222 25:f827a8b7880e 595
christine222 25:f827a8b7880e 596 float receiverTwoReading = 0.0;
christine222 25:f827a8b7880e 597 float receiverThreeReading = 0.0;
christine222 25:f827a8b7880e 598
sahilmgandhi 19:7b66a518b6f8 599 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 600 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 601
christine222 25:f827a8b7880e 602 // float previr2 = ir2;
christine222 25:f827a8b7880e 603 // float previr3 = ir3;
christine222 25:f827a8b7880e 604
christine222 23:690b0ca34ee9 605 int state = 0;
christine222 23:690b0ca34ee9 606
sahilmgandhi 26:d20f1adac2d3 607 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
sahilmgandhi 27:02ce1040f331 608 receiverTwoReading = IRP_2.getSamples(60);
sahilmgandhi 27:02ce1040f331 609 receiverThreeReading = IRP_3.getSamples(60);
sahilmgandhi 27:02ce1040f331 610 receiverOneReading = IRP_1.getSamples(60);
sahilmgandhi 27:02ce1040f331 611 receiverFourReading = IRP_4.getSamples(60);
christine222 25:f827a8b7880e 612
sahilmgandhi 27:02ce1040f331 613 if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
sahilmgandhi 26:d20f1adac2d3 614 if (currDir % 4 == 0){
sahilmgandhi 27:02ce1040f331 615 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 616 }
sahilmgandhi 26:d20f1adac2d3 617 else if (currDir % 4 == 1){
sahilmgandhi 27:02ce1040f331 618 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 619 }
sahilmgandhi 26:d20f1adac2d3 620 else if (currDir % 4 == 2){
sahilmgandhi 27:02ce1040f331 621 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 622 }
sahilmgandhi 26:d20f1adac2d3 623 else if (currDir % 4 == 3){
sahilmgandhi 27:02ce1040f331 624 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 625 }
sahilmgandhi 26:d20f1adac2d3 626 break;
sahilmgandhi 26:d20f1adac2d3 627 }
christine222 25:f827a8b7880e 628
sahilmgandhi 27:02ce1040f331 629 if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL))){
christine222 25:f827a8b7880e 630 // both sides gone
christine222 25:f827a8b7880e 631 redLed.write(1);
christine222 25:f827a8b7880e 632 greenLed.write(1);
christine222 25:f827a8b7880e 633 blueLed.write(1);
christine222 25:f827a8b7880e 634 wait_ms(100);
christine222 25:f827a8b7880e 635 redLed.write(1);
christine222 25:f827a8b7880e 636 greenLed.write(1);
christine222 25:f827a8b7880e 637 blueLed.write(0);
christine222 25:f827a8b7880e 638 wait_ms(200);
christine222 25:f827a8b7880e 639 redLed.write(1);
christine222 25:f827a8b7880e 640 greenLed.write(1);
christine222 25:f827a8b7880e 641 blueLed.write(0);
christine222 25:f827a8b7880e 642 wait_ms(200);
christine222 25:f827a8b7880e 643 redLed.write(1);
christine222 25:f827a8b7880e 644 greenLed.write(1);
christine222 25:f827a8b7880e 645 blueLed.write(0);
christine222 25:f827a8b7880e 646 wait_ms(200);
christine222 25:f827a8b7880e 647 redLed.write(1);
christine222 25:f827a8b7880e 648 greenLed.write(1);
christine222 25:f827a8b7880e 649 blueLed.write(0);
sahilmgandhi 26:d20f1adac2d3 650
sahilmgandhi 26:d20f1adac2d3 651 int prev0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 652 int prev1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 653 int diff0 = desiredCount0 - prev0;
sahilmgandhi 26:d20f1adac2d3 654 int diff1 = desiredCount1 - prev1;
sahilmgandhi 26:d20f1adac2d3 655 int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
sahilmgandhi 26:d20f1adac2d3 656 moveForwardEncoder(valToPass);
sahilmgandhi 26:d20f1adac2d3 657 }
sahilmgandhi 26:d20f1adac2d3 658 else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone
sahilmgandhi 27:02ce1040f331 659 // if (currDir % 4 == 0){
sahilmgandhi 27:02ce1040f331 660 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 27:02ce1040f331 661 // }
sahilmgandhi 27:02ce1040f331 662 // else if (currDir % 4 == 1){
sahilmgandhi 27:02ce1040f331 663 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 27:02ce1040f331 664 // }
sahilmgandhi 27:02ce1040f331 665 // else if (currDir % 4 == 2){
sahilmgandhi 27:02ce1040f331 666 // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 27:02ce1040f331 667 // }
sahilmgandhi 27:02ce1040f331 668 // else if (currDir % 4 == 3){
sahilmgandhi 27:02ce1040f331 669 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 27:02ce1040f331 670 // }
christine222 25:f827a8b7880e 671 // RED RED RED RED RED
christine222 25:f827a8b7880e 672 state = 1;
christine222 25:f827a8b7880e 673 redLed.write(0);
christine222 25:f827a8b7880e 674 greenLed.write(1);
christine222 25:f827a8b7880e 675 blueLed.write(1);
christine222 25:f827a8b7880e 676 }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone
sahilmgandhi 27:02ce1040f331 677 // if (currDir % 4 == 0){
sahilmgandhi 27:02ce1040f331 678 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 27:02ce1040f331 679 // }
sahilmgandhi 27:02ce1040f331 680 // else if (currDir % 4 == 1){
sahilmgandhi 27:02ce1040f331 681 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 27:02ce1040f331 682 // }
sahilmgandhi 27:02ce1040f331 683 // else if (currDir % 4 == 2){
sahilmgandhi 27:02ce1040f331 684 // wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 27:02ce1040f331 685 // }
sahilmgandhi 27:02ce1040f331 686 // else if (currDir % 4 == 3){
sahilmgandhi 27:02ce1040f331 687 // wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 27:02ce1040f331 688 // }
christine222 25:f827a8b7880e 689 // BLUE BLUE BLUE BLUE
christine222 25:f827a8b7880e 690 state = 2;
christine222 25:f827a8b7880e 691 redLed.write(1);
christine222 25:f827a8b7880e 692 greenLed.write(1);
christine222 25:f827a8b7880e 693 blueLed.write(0);
sahilmgandhi 27:02ce1040f331 694 }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
sahilmgandhi 26:d20f1adac2d3 695 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 696 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 697 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 698 }
sahilmgandhi 26:d20f1adac2d3 699 else if (currDir % 4 == 1){
sahilmgandhi 26:d20f1adac2d3 700 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 701 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 702 }
sahilmgandhi 26:d20f1adac2d3 703 else if (currDir % 4 == 2){
sahilmgandhi 26:d20f1adac2d3 704 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 705 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 706 }
sahilmgandhi 26:d20f1adac2d3 707 else if (currDir % 4 == 3){
sahilmgandhi 26:d20f1adac2d3 708 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 709 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 710 }
christine222 25:f827a8b7880e 711 // both walls there
christine222 25:f827a8b7880e 712 state = 0;
christine222 25:f827a8b7880e 713 redLed.write(1);
christine222 25:f827a8b7880e 714 greenLed.write(0);
christine222 25:f827a8b7880e 715 blueLed.write(1);
christine222 25:f827a8b7880e 716 }
christine222 25:f827a8b7880e 717
christine222 25:f827a8b7880e 718 switch(state){
christine222 25:f827a8b7880e 719 case(0):{ // both walls there
christine222 25:f827a8b7880e 720 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 721 break;
christine222 25:f827a8b7880e 722 }
christine222 25:f827a8b7880e 723 case(1):{// RED RED RED RED RED
christine222 25:f827a8b7880e 724 currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
christine222 25:f827a8b7880e 725 break;
christine222 25:f827a8b7880e 726 }
christine222 25:f827a8b7880e 727 case(2):{// blue
christine222 25:f827a8b7880e 728 currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 729 break;
christine222 25:f827a8b7880e 730 }
christine222 25:f827a8b7880e 731 default:{
christine222 25:f827a8b7880e 732 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 733 //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 734 break;
christine222 25:f827a8b7880e 735 }
christine222 25:f827a8b7880e 736 }
christine222 25:f827a8b7880e 737
christine222 25:f827a8b7880e 738 sumError += currentError;
christine222 25:f827a8b7880e 739 derivError = currentError - previousError;
christine222 25:f827a8b7880e 740 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 25:f827a8b7880e 741 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
christine222 25:f827a8b7880e 742 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 743 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 744 } else { // r is faster than L. speed up l, slow down r
christine222 25:f827a8b7880e 745 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 746 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 747 }
christine222 25:f827a8b7880e 748 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 25:f827a8b7880e 749 if (leftSpeed < 0) leftSpeed = 0;
christine222 25:f827a8b7880e 750 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 25:f827a8b7880e 751 if (rightSpeed < 0) rightSpeed = 0;
sahilmgandhi 26:d20f1adac2d3 752
christine222 25:f827a8b7880e 753 right_motor.forward(rightSpeed);
christine222 25:f827a8b7880e 754 left_motor.forward(leftSpeed);
christine222 25:f827a8b7880e 755 pidOnEncoders();
sahilmgandhi 26:d20f1adac2d3 756
christine222 25:f827a8b7880e 757 previousError = currentError;
christine222 25:f827a8b7880e 758 ir2 = IRP_2.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 759 ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 26:d20f1adac2d3 760 }
sahilmgandhi 26:d20f1adac2d3 761 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 762 mouseY += 1;
sahilmgandhi 26:d20f1adac2d3 763 }
sahilmgandhi 26:d20f1adac2d3 764 else if (currDir % 4 == 1){
sahilmgandhi 26:d20f1adac2d3 765 mouseX + 1;
sahilmgandhi 19:7b66a518b6f8 766 }
sahilmgandhi 26:d20f1adac2d3 767 else if (currDir % 4 == 2){
sahilmgandhi 26:d20f1adac2d3 768 mouseY -= 1;
sahilmgandhi 26:d20f1adac2d3 769 }
sahilmgandhi 26:d20f1adac2d3 770 else if (currDir % 4 == 3){
sahilmgandhi 26:d20f1adac2d3 771 mouseX -= 1;
sahilmgandhi 26:d20f1adac2d3 772 }
sahilmgandhi 26:d20f1adac2d3 773
sahilmgandhi 19:7b66a518b6f8 774 left_motor.brake();
sahilmgandhi 19:7b66a518b6f8 775 right_motor.brake();
christine222 23:690b0ca34ee9 776 return;
sahilmgandhi 17:f713758f6238 777 }
christine222 25:f827a8b7880e 778
sahilmgandhi 26:d20f1adac2d3 779 bool isWallInFront(int x, int y){
sahilmgandhi 27:02ce1040f331 780 return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
sahilmgandhi 26:d20f1adac2d3 781 }
sahilmgandhi 26:d20f1adac2d3 782 bool isWallInBack(int x, int y){
sahilmgandhi 27:02ce1040f331 783 return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
sahilmgandhi 26:d20f1adac2d3 784 }
sahilmgandhi 26:d20f1adac2d3 785 bool isWallOnRight(int x, int y){
sahilmgandhi 27:02ce1040f331 786 return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
sahilmgandhi 26:d20f1adac2d3 787 }
sahilmgandhi 26:d20f1adac2d3 788 bool isWallOnLeft(int x, int y){
sahilmgandhi 27:02ce1040f331 789 return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
sahilmgandhi 26:d20f1adac2d3 790 }
sahilmgandhi 26:d20f1adac2d3 791
sahilmgandhi 26:d20f1adac2d3 792 int chooseNextMovement(){
sahilmgandhi 26:d20f1adac2d3 793 int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
sahilmgandhi 26:d20f1adac2d3 794 if (goingToCenter && (currentDistance == 0)){
sahilmgandhi 26:d20f1adac2d3 795 goingToCenter = false;
sahilmgandhi 26:d20f1adac2d3 796 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 797 }
sahilmgandhi 26:d20f1adac2d3 798 else if (!goingToCenter && (currentDistance == 0)){
sahilmgandhi 26:d20f1adac2d3 799 goingToCenter == true;
sahilmgandhi 26:d20f1adac2d3 800 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 801 }
sahilmgandhi 26:d20f1adac2d3 802
sahilmgandhi 26:d20f1adac2d3 803 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 804 int currX = 0;
sahilmgandhi 26:d20f1adac2d3 805 int currY = 0;
sahilmgandhi 26:d20f1adac2d3 806 int currDist = 0;
sahilmgandhi 26:d20f1adac2d3 807 int dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 808 if (!justTurned){
sahilmgandhi 26:d20f1adac2d3 809 cellsToVisit.push(make_pair(mouseX, mouseY));
sahilmgandhi 26:d20f1adac2d3 810 while (!cellsToVisit.empty()) {
sahilmgandhi 26:d20f1adac2d3 811 pair<int, int> curr = cellsToVisit.top();
sahilmgandhi 26:d20f1adac2d3 812 cellsToVisit.pop();
sahilmgandhi 26:d20f1adac2d3 813 currX = curr.first;
sahilmgandhi 26:d20f1adac2d3 814 currY = curr.second;
sahilmgandhi 26:d20f1adac2d3 815 currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
sahilmgandhi 26:d20f1adac2d3 816 int minDist = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 817
sahilmgandhi 26:d20f1adac2d3 818 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 819 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 820 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 821 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 822 }
sahilmgandhi 26:d20f1adac2d3 823 }
sahilmgandhi 27:02ce1040f331 824 if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 825 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 826 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 827 }
sahilmgandhi 26:d20f1adac2d3 828 }
sahilmgandhi 27:02ce1040f331 829 if (currY + 1 < MAZE_LEN && !isWallInFront( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 830 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 831 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 832 }
sahilmgandhi 26:d20f1adac2d3 833 }
sahilmgandhi 27:02ce1040f331 834 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 835 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 836 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 837 }
sahilmgandhi 26:d20f1adac2d3 838 }
sahilmgandhi 26:d20f1adac2d3 839 } else {
sahilmgandhi 26:d20f1adac2d3 840 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 841 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 842 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 843 }
sahilmgandhi 26:d20f1adac2d3 844 }
sahilmgandhi 26:d20f1adac2d3 845 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 846 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 847 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 848 }
sahilmgandhi 26:d20f1adac2d3 849 }
sahilmgandhi 26:d20f1adac2d3 850 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 851 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 852 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 853 }
sahilmgandhi 26:d20f1adac2d3 854 }
sahilmgandhi 26:d20f1adac2d3 855 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 856 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 857 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 858 }
sahilmgandhi 26:d20f1adac2d3 859 }
sahilmgandhi 26:d20f1adac2d3 860 }
sahilmgandhi 26:d20f1adac2d3 861
sahilmgandhi 26:d20f1adac2d3 862 if (minDist == INT_MAX)
sahilmgandhi 26:d20f1adac2d3 863 continue;
sahilmgandhi 26:d20f1adac2d3 864 if (currDist > minDist)
sahilmgandhi 26:d20f1adac2d3 865 continue;
sahilmgandhi 26:d20f1adac2d3 866 if (currDist <= minDist) {
sahilmgandhi 26:d20f1adac2d3 867 manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
sahilmgandhi 26:d20f1adac2d3 868 }
sahilmgandhi 26:d20f1adac2d3 869 if (hasVisited(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 870 if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 871 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 872 }
sahilmgandhi 26:d20f1adac2d3 873 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 874 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 875 }
sahilmgandhi 26:d20f1adac2d3 876 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 877 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 878 }
sahilmgandhi 26:d20f1adac2d3 879 if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 880 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 881 }
sahilmgandhi 26:d20f1adac2d3 882 } else {
sahilmgandhi 26:d20f1adac2d3 883 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 884 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 885 }
sahilmgandhi 26:d20f1adac2d3 886 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 887 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 888 }
sahilmgandhi 26:d20f1adac2d3 889 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 890 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 891 }
sahilmgandhi 26:d20f1adac2d3 892 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 893 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 894 }
sahilmgandhi 26:d20f1adac2d3 895 }
sahilmgandhi 26:d20f1adac2d3 896 }
sahilmgandhi 26:d20f1adac2d3 897
sahilmgandhi 26:d20f1adac2d3 898 int minDistance = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 899 if (currDir % 4 == 0) {
sahilmgandhi 26:d20f1adac2d3 900 if (mouseX + 1 < MAZE_LEN && !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 >= 0 && !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 < MAZE_LEN && !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 >= 0 && !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 == 2) {
sahilmgandhi 26:d20f1adac2d3 925 if (mouseX - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 926 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 927 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 928 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 929 }
sahilmgandhi 26:d20f1adac2d3 930 }
sahilmgandhi 26:d20f1adac2d3 931 if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 932 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 933 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 934 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 935 }
sahilmgandhi 26:d20f1adac2d3 936 }
sahilmgandhi 26:d20f1adac2d3 937 if (mouseY - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 938 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 939 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 940 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 941 }
sahilmgandhi 26:d20f1adac2d3 942 }
sahilmgandhi 26:d20f1adac2d3 943 if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 944 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 945 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 946 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 947 }
sahilmgandhi 26:d20f1adac2d3 948 }
sahilmgandhi 26:d20f1adac2d3 949 } else if (currDir % 4 == 1) {
sahilmgandhi 26:d20f1adac2d3 950 if (mouseY - 1 >= 0 && !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 < MAZE_LEN && !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 < MAZE_LEN && !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 >= 0 && !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 } else if (currDir % 4 == 3) {
sahilmgandhi 26:d20f1adac2d3 975 if (mouseY + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 976 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 977 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 978 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 979 }
sahilmgandhi 26:d20f1adac2d3 980 }
sahilmgandhi 26:d20f1adac2d3 981 if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 982 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 983 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 984 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 985 }
sahilmgandhi 26:d20f1adac2d3 986 }
sahilmgandhi 26:d20f1adac2d3 987 if (mouseX - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 988 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 989 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 990 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 991 }
sahilmgandhi 26:d20f1adac2d3 992 }
sahilmgandhi 26:d20f1adac2d3 993 if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 994 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 995 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 996 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 997 }
sahilmgandhi 26:d20f1adac2d3 998 }
sahilmgandhi 26:d20f1adac2d3 999 }
sahilmgandhi 26:d20f1adac2d3 1000 }
sahilmgandhi 26:d20f1adac2d3 1001 else{
sahilmgandhi 26:d20f1adac2d3 1002 justTurned = false;
sahilmgandhi 26:d20f1adac2d3 1003 dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 1004 }
sahilmgandhi 26:d20f1adac2d3 1005
sahilmgandhi 26:d20f1adac2d3 1006 return dirToGo;
sahilmgandhi 26:d20f1adac2d3 1007 }
sahilmgandhi 26:d20f1adac2d3 1008
sahilmgandhi 26:d20f1adac2d3 1009 bool hasVisited(int x, int y){
sahilmgandhi 26:d20f1adac2d3 1010 return visitedCells[MAZE_LEN - 1 - y][x];
sahilmgandhi 26:d20f1adac2d3 1011 }
sahilmgandhi 26:d20f1adac2d3 1012
sahilmgandhi 26:d20f1adac2d3 1013 void changeManhattanDistance(bool headCenter){
sahilmgandhi 26:d20f1adac2d3 1014 if (headCenter){
sahilmgandhi 26:d20f1adac2d3 1015 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1016 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1017 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(7 - i);
sahilmgandhi 26:d20f1adac2d3 1018 }
sahilmgandhi 26:d20f1adac2d3 1019 }
sahilmgandhi 26:d20f1adac2d3 1020 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1021 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1022 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(8 - i);
sahilmgandhi 26:d20f1adac2d3 1023 }
sahilmgandhi 26:d20f1adac2d3 1024 }
sahilmgandhi 26:d20f1adac2d3 1025 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1026 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1027 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(7 - i);
sahilmgandhi 26:d20f1adac2d3 1028 }
sahilmgandhi 26:d20f1adac2d3 1029 }
sahilmgandhi 26:d20f1adac2d3 1030 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1031 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1032 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(8 - i);
sahilmgandhi 26:d20f1adac2d3 1033 }
sahilmgandhi 26:d20f1adac2d3 1034 }
sahilmgandhi 26:d20f1adac2d3 1035 }
sahilmgandhi 26:d20f1adac2d3 1036 else {
sahilmgandhi 26:d20f1adac2d3 1037 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1038 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1039 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1040 }
sahilmgandhi 26:d20f1adac2d3 1041 }
sahilmgandhi 26:d20f1adac2d3 1042 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1043 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1044 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1045 }
sahilmgandhi 26:d20f1adac2d3 1046 }
sahilmgandhi 26:d20f1adac2d3 1047 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1048 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1049 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1050 }
sahilmgandhi 26:d20f1adac2d3 1051 }
sahilmgandhi 26:d20f1adac2d3 1052 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1053 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1054 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1055 }
sahilmgandhi 26:d20f1adac2d3 1056 }
sahilmgandhi 26:d20f1adac2d3 1057 }
sahilmgandhi 26:d20f1adac2d3 1058 }
sahilmgandhi 26:d20f1adac2d3 1059
sahilmgandhi 0:a03c771ab78e 1060 int main()
sahilmgandhi 0:a03c771ab78e 1061 {
christine222 3:880f15be8c72 1062 //Set highest bandwidth.
christine222 23:690b0ca34ee9 1063 //gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 1064 serial.baud(9600);
christine222 23:690b0ca34ee9 1065 //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 26:d20f1adac2d3 1066
sahilmgandhi 1:8a4b2f573923 1067 wait (0.1);
sahilmgandhi 26:d20f1adac2d3 1068
sahilmgandhi 2:771db996cee0 1069 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 1070 greenLed.write(0);
sahilmgandhi 2:771db996cee0 1071 blueLed.write(1);
sahilmgandhi 26:d20f1adac2d3 1072
kyleliangus 9:1d8e4da058cd 1073 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 1074 //right_motor.forward(0.1);
sahilmgandhi 26:d20f1adac2d3 1075
kyleliangus 8:a0760acdc59e 1076 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 1077 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 1078 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 1079 // PB_3 is B of left
vanshg 11:8fc2b703086b 1080 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 1081 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 1082
vanshg 10:810d1849da9d 1083 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 1084 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 1085 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 1086 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 1087 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 1088 */
sahilmgandhi 26:d20f1adac2d3 1089
kyleliangus 12:5790e56a056f 1090 // 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 1091 // of alternate function specified
kyleliangus 12:5790e56a056f 1092 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 1093 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 1094 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 1095 //
kyleliangus 12:5790e56a056f 1096 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 1097 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 1098 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 1099 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 1100 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 1101 // CR1 reenabales
kyleliangus 12:5790e56a056f 1102 // then read CNT reg of timer
sahilmgandhi 26:d20f1adac2d3 1103
sahilmgandhi 26:d20f1adac2d3 1104
vanshg 10:810d1849da9d 1105 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 1106 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 1107 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 1108 dipButton4.rise(&enableButton4);
sahilmgandhi 26:d20f1adac2d3 1109
vanshg 10:810d1849da9d 1110 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 1111 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 1112 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 1113 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 1114
sahilmgandhi 26:d20f1adac2d3 1115 // if(dipFlags == 0x1){
sahilmgandhi 26:d20f1adac2d3 1116 turnRight180();
sahilmgandhi 26:d20f1adac2d3 1117 wait_ms(1000);
sahilmgandhi 26:d20f1adac2d3 1118 // }else{
sahilmgandhi 26:d20f1adac2d3 1119 // turnRight();
sahilmgandhi 26:d20f1adac2d3 1120 // IRP_1.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 1121 // IRP_4.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 1122 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 1123 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 1124
sahilmgandhi 26:d20f1adac2d3 1125 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 1126 // turnLeft();
sahilmgandhi 26:d20f1adac2d3 1127 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 1128 // }
sahilmgandhi 26:d20f1adac2d3 1129
sahilmgandhi 26:d20f1adac2d3 1130
sahilmgandhi 26:d20f1adac2d3 1131 // init the wall, and mouse loc arrays:
sahilmgandhi 26:d20f1adac2d3 1132 wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
sahilmgandhi 26:d20f1adac2d3 1133 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 1134
christine222 25:f827a8b7880e 1135 //right_motor.forward( 0.2 );
christine222 25:f827a8b7880e 1136 //left_motor.forward( 0.2 );
sahilmgandhi 26:d20f1adac2d3 1137 //turnRight180();
sahilmgandhi 26:d20f1adac2d3 1138 //wait_ms(1500);
sahilmgandhi 26:d20f1adac2d3 1139 int nextMovement = 0;
sahilmgandhi 26:d20f1adac2d3 1140 while (1) {
sahilmgandhi 26:d20f1adac2d3 1141 nextMovement = chooseNextMovement();
sahilmgandhi 26:d20f1adac2d3 1142 if (nextMovement == 0){
sahilmgandhi 26:d20f1adac2d3 1143 nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 1144 }
sahilmgandhi 26:d20f1adac2d3 1145 else if (nextMovement == 1){
sahilmgandhi 26:d20f1adac2d3 1146 justTurned = true;
sahilmgandhi 26:d20f1adac2d3 1147 turnRight();
sahilmgandhi 26:d20f1adac2d3 1148 }
sahilmgandhi 26:d20f1adac2d3 1149 else if (nextMovement == 2){
sahilmgandhi 26:d20f1adac2d3 1150 justTurned = true;
sahilmgandhi 26:d20f1adac2d3 1151 turnLeft();
sahilmgandhi 26:d20f1adac2d3 1152 }
sahilmgandhi 26:d20f1adac2d3 1153 else if (nextMovement == 3){
sahilmgandhi 26:d20f1adac2d3 1154 nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 1155 }
sahilmgandhi 26:d20f1adac2d3 1156 else if (nextMovement == 4){
sahilmgandhi 26:d20f1adac2d3 1157 justTurned = true;
sahilmgandhi 26:d20f1adac2d3 1158 turnRight180();
sahilmgandhi 26:d20f1adac2d3 1159 }
sahilmgandhi 26:d20f1adac2d3 1160 wait_ms(1000); // reduce this once we fine tune this!
christine222 24:e7063765d6f0 1161
christine222 23:690b0ca34ee9 1162 //wait_ms(1500);
christine222 23:690b0ca34ee9 1163 //turnRight();
christine222 23:690b0ca34ee9 1164 //wait_ms(1500);
christine222 23:690b0ca34ee9 1165 //turnLeft();
sahilmgandhi 26:d20f1adac2d3 1166 // nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 1167 // wait_ms(500);
christine222 23:690b0ca34ee9 1168 // turnRight();
christine222 23:690b0ca34ee9 1169 // wait_ms(500);
christine222 23:690b0ca34ee9 1170 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 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 // turnLeft();
christine222 23:690b0ca34ee9 1177 // wait_ms(500);
christine222 23:690b0ca34ee9 1178 // nCellEncoderAndIR(2);
christine222 23:690b0ca34ee9 1179 // wait_ms(500);
christine222 23:690b0ca34ee9 1180 // turnRight();
christine222 23:690b0ca34ee9 1181 // wait_ms(500);
christine222 23:690b0ca34ee9 1182 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 1183 // wait_ms(500);
christine222 23:690b0ca34ee9 1184 // turnRight();
christine222 23:690b0ca34ee9 1185 // wait_ms(500);
christine222 23:690b0ca34ee9 1186 // nCellEncoderAndIR(5);
christine222 23:690b0ca34ee9 1187 // break;
christine222 23:690b0ca34ee9 1188 // turnRight180();
sahilmgandhi 26:d20f1adac2d3 1189
christine222 23:690b0ca34ee9 1190 // int number = rand() % 4 + 1;
christine222 23:690b0ca34ee9 1191 // switch(number){
christine222 23:690b0ca34ee9 1192 // case(1):{//turn right
christine222 23:690b0ca34ee9 1193 // turnRight();
christine222 23:690b0ca34ee9 1194 // break;
christine222 23:690b0ca34ee9 1195 // }
christine222 23:690b0ca34ee9 1196 // case(2):{ // turn left
christine222 23:690b0ca34ee9 1197 // turnLeft();
christine222 23:690b0ca34ee9 1198 // break;
christine222 23:690b0ca34ee9 1199 // }
christine222 23:690b0ca34ee9 1200 // case(3):{// keep going
sahilmgandhi 26:d20f1adac2d3 1201
christine222 23:690b0ca34ee9 1202 // break;
christine222 23:690b0ca34ee9 1203 // }
christine222 23:690b0ca34ee9 1204 // case(4):{// turnaround
christine222 23:690b0ca34ee9 1205 // turnRight180();
christine222 23:690b0ca34ee9 1206 // break;
christine222 23:690b0ca34ee9 1207 // }
christine222 23:690b0ca34ee9 1208 // default:{// keep going
christine222 23:690b0ca34ee9 1209 // break;
christine222 23:690b0ca34ee9 1210 // }
christine222 23:690b0ca34ee9 1211 // }
sahilmgandhi 26:d20f1adac2d3 1212
christine222 23:690b0ca34ee9 1213 // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
christine222 23:690b0ca34ee9 1214 // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
sahilmgandhi 26:d20f1adac2d3 1215
christine222 23:690b0ca34ee9 1216 // float ir3 = IRP_2.getSamples(100)/initAverageL;
christine222 23:690b0ca34ee9 1217 // float ir2 = IRP_3.getSamples(100)/initAverageR;
christine222 22:681190ff98f0 1218
sahilmgandhi 26:d20f1adac2d3 1219
christine222 25:f827a8b7880e 1220
christine222 23:690b0ca34ee9 1221 /*
christine222 23:690b0ca34ee9 1222 counter2++;
christine222 23:690b0ca34ee9 1223 counter3++;
christine222 23:690b0ca34ee9 1224 ir2tot += IRP_2.getSamples(100);
christine222 23:690b0ca34ee9 1225 ir3tot += IRP_3.getSamples(100);
sahilmgandhi 26:d20f1adac2d3 1226
sahilmgandhi 26:d20f1adac2d3 1227
christine222 23:690b0ca34ee9 1228 ir2 = ir2tot/counter2;
christine222 23:690b0ca34ee9 1229 ir3 = ir3tot/counter3;
christine222 23:690b0ca34ee9 1230
sahilmgandhi 26:d20f1adac2d3 1231
christine222 23:690b0ca34ee9 1232 serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1233 */
christine222 23:690b0ca34ee9 1234 //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
christine222 23:690b0ca34ee9 1235 //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3);
christine222 23:690b0ca34ee9 1236 //break;
christine222 23:690b0ca34ee9 1237 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
christine222 23:690b0ca34ee9 1238 //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1239 //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
sahilmgandhi 26:d20f1adac2d3 1240
sahilmgandhi 26:d20f1adac2d3 1241
christine222 23:690b0ca34ee9 1242 ////////////////////////////////////////////////////////////////
sahilmgandhi 26:d20f1adac2d3 1243
christine222 22:681190ff98f0 1244 //nCellEncoderAndIR(3);
christine222 22:681190ff98f0 1245 //break;
sahilmgandhi 26:d20f1adac2d3 1246
christine222 23:690b0ca34ee9 1247 //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 1248
christine222 21:9a6cb07bdcb6 1249 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
sahilmgandhi 26:d20f1adac2d3 1250
christine222 21:9a6cb07bdcb6 1251 //break;
christine222 21:9a6cb07bdcb6 1252 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1253 // wait(0.5);
christine222 21:9a6cb07bdcb6 1254 // handleTurns();
christine222 21:9a6cb07bdcb6 1255 // wait(0.5);
christine222 21:9a6cb07bdcb6 1256 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1257 // wait(0.5);
christine222 21:9a6cb07bdcb6 1258 // handleTurns();
christine222 21:9a6cb07bdcb6 1259 //break;
sahilmgandhi 16:d9252437bd92 1260 //pidOnEncoders();
sahilmgandhi 20:82836745332e 1261 // moveForwardUntilWallIr();
kyleliangus 8:a0760acdc59e 1262 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
kyleliangus 15:b80555a4a8b9 1263 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
sahilmgandhi 17:f713758f6238 1264 // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
christine222 21:9a6cb07bdcb6 1265 //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 1266
christine222 3:880f15be8c72 1267 //reading = Rec_4.read();
christine222 3:880f15be8c72 1268 // serial.printf("reading: %f\n", reading);
christine222 3:880f15be8c72 1269 }
sahilmgandhi 26:d20f1adac2d3 1270 }