MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@26:d20f1adac2d3, 2017-05-21 (annotated)

Committer:
sahilmgandhi
Date:
Sun May 21 09:58:54 2017 +0000
Revision:
26:d20f1adac2d3
Parent:
25:f827a8b7880e
Child:
27:02ce1040f331
Added floodfill to the code ... need to tune it now to get the wall array properly fixed!;

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 26:d20f1adac2d3 49 float averageDivL = 30.5; //blue
christine222 25:f827a8b7880e 50 float initAverageR = 8.75; //4.5
sahilmgandhi 26:d20f1adac2d3 51 float averageDivR = 30.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
christine222 25:f827a8b7880e 367
christine222 25:f827a8b7880e 368 double speed0 = 0.11;
christine222 25:f827a8b7880e 369 double speed1 = 0.13;
christine222 23:690b0ca34ee9 370 right_motor.move(speed0);
christine222 23:690b0ca34ee9 371 left_motor.move(speed1);
christine222 23:690b0ca34ee9 372
christine222 25:f827a8b7880e 373 float ir1 = IRP_1.getSamples(50);
christine222 25:f827a8b7880e 374 float ir4 = IRP_4.getSamples(50);
christine222 25:f827a8b7880e 375
christine222 25:f827a8b7880e 376 if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4){
christine222 24:e7063765d6f0 377 return;
christine222 24:e7063765d6f0 378 }
christine222 25:f827a8b7880e 379
christine222 23:690b0ca34ee9 380 //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
christine222 25:f827a8b7880e 381 //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){
christine222 25:f827a8b7880e 382 while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ){
christine222 23:690b0ca34ee9 383 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
christine222 23:690b0ca34ee9 384
christine222 23:690b0ca34ee9 385 count0 = encoder0.getPulses() - initial0;
christine222 23:690b0ca34ee9 386 count1 = encoder1.getPulses() - initial1;
christine222 23:690b0ca34ee9 387 int x = count0 - count1;
christine222 23:690b0ca34ee9 388 //double d = kp * x + kd * ( x - prev );
christine222 23:690b0ca34ee9 389 double kppart = kp * x;
christine222 23:690b0ca34ee9 390 double kdpart = kd * (x-prev);
christine222 23:690b0ca34ee9 391 double d = kppart + kdpart;
christine222 23:690b0ca34ee9 392
christine222 23:690b0ca34ee9 393 //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 394 if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
christine222 23:690b0ca34ee9 395 {
christine222 23:690b0ca34ee9 396 left_motor.move( speed1-0.8*d );
christine222 23:690b0ca34ee9 397 right_motor.move( speed0+d );
christine222 23:690b0ca34ee9 398 }
christine222 23:690b0ca34ee9 399 else if( x > diff + 40 )
christine222 23:690b0ca34ee9 400 {
christine222 23:690b0ca34ee9 401 left_motor.move( speed1-0.8*d );
christine222 23:690b0ca34ee9 402 right_motor.move( speed0+d );
christine222 23:690b0ca34ee9 403 }
christine222 23:690b0ca34ee9 404 // else
christine222 23:690b0ca34ee9 405 // {
christine222 23:690b0ca34ee9 406 // left_motor.brake();
christine222 23:690b0ca34ee9 407 // right_motor.brake();
christine222 23:690b0ca34ee9 408 // }
christine222 23:690b0ca34ee9 409 prev = x;
christine222 25:f827a8b7880e 410 ir1 = IRP_1.getSamples(50);
christine222 25:f827a8b7880e 411 ir4 = IRP_4.getSamples(50);
christine222 23:690b0ca34ee9 412 }
christine222 25:f827a8b7880e 413
christine222 23:690b0ca34ee9 414 //pidOnEncoders();
christine222 23:690b0ca34ee9 415 //pidBrake();
christine222 23:690b0ca34ee9 416 right_motor.brake();
christine222 23:690b0ca34ee9 417 left_motor.brake();
christine222 21:9a6cb07bdcb6 418 return;
christine222 21:9a6cb07bdcb6 419 }
sahilmgandhi 26:d20f1adac2d3 420
sahilmgandhi 14:9e7bb03ddccb 421 void moveForwardUntilWallIr()
sahilmgandhi 14:9e7bb03ddccb 422 {
kyleliangus 9:1d8e4da058cd 423 double currentError = 0;
kyleliangus 9:1d8e4da058cd 424 double previousError = 0;
kyleliangus 9:1d8e4da058cd 425 double derivError = 0;
kyleliangus 9:1d8e4da058cd 426 double sumError = 0;
christine222 25:f827a8b7880e 427
vanshg 11:8fc2b703086b 428 double HIGH_PWM_VOLTAGE = 0.1;
christine222 25:f827a8b7880e 429
christine222 25:f827a8b7880e 430 double rightSpeed = 0.25;
christine222 25:f827a8b7880e 431 double leftSpeed = 0.23;
christine222 25:f827a8b7880e 432
kyleliangus 9:1d8e4da058cd 433 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 434 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 435
vanshg 11:8fc2b703086b 436 int count = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 437 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 438
christine222 21:9a6cb07bdcb6 439 if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
christine222 23:690b0ca34ee9 440 //moveForwardWallEncoder();
christine222 22:681190ff98f0 441 }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone
christine222 22:681190ff98f0 442 //moveForwardRightWall();
christine222 22:681190ff98f0 443 }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone
christine222 22:681190ff98f0 444 //moveForwardLeftWall();
christine222 21:9a6cb07bdcb6 445 }else{
christine222 21:9a6cb07bdcb6 446 // will move forward using encoders only
christine222 21:9a6cb07bdcb6 447 // if cell ahead doesn't have a wall on either one side or both sides
christine222 25:f827a8b7880e 448
christine222 21:9a6cb07bdcb6 449 int pulseCount = (encoder0.getPulses()-count) % 5600;
christine222 21:9a6cb07bdcb6 450 if (pulseCount > 5400 && pulseCount < 5800) {
christine222 21:9a6cb07bdcb6 451 blueLed.write(0);
christine222 21:9a6cb07bdcb6 452 } else {
christine222 21:9a6cb07bdcb6 453 blueLed.write(1);
christine222 21:9a6cb07bdcb6 454 }
christine222 21:9a6cb07bdcb6 455 sumError += currentError;
christine222 25:f827a8b7880e 456 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ;
christine222 21:9a6cb07bdcb6 457 derivError = currentError - previousError;
christine222 21:9a6cb07bdcb6 458 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 21:9a6cb07bdcb6 459 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
christine222 21:9a6cb07bdcb6 460 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 461 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 462 } else { // r is faster than L. speed up l, slow down r
christine222 21:9a6cb07bdcb6 463 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 464 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 465 }
christine222 25:f827a8b7880e 466
christine222 21:9a6cb07bdcb6 467 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 21:9a6cb07bdcb6 468 if (leftSpeed < 0) leftSpeed = 0;
christine222 21:9a6cb07bdcb6 469 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 21:9a6cb07bdcb6 470 if (rightSpeed < 0) rightSpeed = 0;
christine222 25:f827a8b7880e 471
christine222 21:9a6cb07bdcb6 472 right_motor.forward(rightSpeed);
christine222 21:9a6cb07bdcb6 473 left_motor.forward(leftSpeed);
christine222 25:f827a8b7880e 474
christine222 21:9a6cb07bdcb6 475 previousError = currentError;
christine222 25:f827a8b7880e 476
christine222 21:9a6cb07bdcb6 477 ir2 = IRP_2.getSamples( SAMPLE_NUM );
christine222 21:9a6cb07bdcb6 478 ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 479
kyleliangus 9:1d8e4da058cd 480 }
christine222 25:f827a8b7880e 481
christine222 21:9a6cb07bdcb6 482 //backward();
christine222 22:681190ff98f0 483 //wait_ms(40);
christine222 21:9a6cb07bdcb6 484 //brake();
christine222 25:f827a8b7880e 485
christine222 21:9a6cb07bdcb6 486 left_motor.brake();
christine222 21:9a6cb07bdcb6 487 right_motor.brake();
kyleliangus 9:1d8e4da058cd 488 }
vanshg 10:810d1849da9d 489 }
sahilmgandhi 26:d20f1adac2d3 490
sahilmgandhi 14:9e7bb03ddccb 491 void printDipFlag()
sahilmgandhi 14:9e7bb03ddccb 492 {
vanshg 11:8fc2b703086b 493 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 494 }
sahilmgandhi 26:d20f1adac2d3 495
sahilmgandhi 14:9e7bb03ddccb 496 void enableButton1()
sahilmgandhi 14:9e7bb03ddccb 497 {
vanshg 10:810d1849da9d 498 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 499 printDipFlag();
vanshg 10:810d1849da9d 500 }
sahilmgandhi 14:9e7bb03ddccb 501 void enableButton2()
sahilmgandhi 14:9e7bb03ddccb 502 {
vanshg 10:810d1849da9d 503 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 504 printDipFlag();
vanshg 10:810d1849da9d 505 }
sahilmgandhi 14:9e7bb03ddccb 506 void enableButton3()
sahilmgandhi 14:9e7bb03ddccb 507 {
vanshg 10:810d1849da9d 508 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 509 printDipFlag();
vanshg 10:810d1849da9d 510 }
sahilmgandhi 14:9e7bb03ddccb 511 void enableButton4()
sahilmgandhi 14:9e7bb03ddccb 512 {
vanshg 10:810d1849da9d 513 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 514 printDipFlag();
vanshg 10:810d1849da9d 515 }
sahilmgandhi 14:9e7bb03ddccb 516 void disableButton1()
sahilmgandhi 14:9e7bb03ddccb 517 {
vanshg 10:810d1849da9d 518 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 519 printDipFlag();
vanshg 10:810d1849da9d 520 }
sahilmgandhi 14:9e7bb03ddccb 521 void disableButton2()
sahilmgandhi 14:9e7bb03ddccb 522 {
vanshg 10:810d1849da9d 523 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 524 printDipFlag();
vanshg 10:810d1849da9d 525 }
sahilmgandhi 14:9e7bb03ddccb 526 void disableButton3()
sahilmgandhi 14:9e7bb03ddccb 527 {
vanshg 10:810d1849da9d 528 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 529 printDipFlag();
vanshg 10:810d1849da9d 530 }
sahilmgandhi 14:9e7bb03ddccb 531 void disableButton4()
sahilmgandhi 14:9e7bb03ddccb 532 {
vanshg 10:810d1849da9d 533 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 534 printDipFlag();
kyleliangus 9:1d8e4da058cd 535 }
sahilmgandhi 26:d20f1adac2d3 536
kyleliangus 15:b80555a4a8b9 537 void pidOnEncoders()
kyleliangus 15:b80555a4a8b9 538 {
kyleliangus 15:b80555a4a8b9 539 int count0;
kyleliangus 15:b80555a4a8b9 540 int count1;
kyleliangus 15:b80555a4a8b9 541 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 542 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 543 int diff = count0 - count1;
sahilmgandhi 20:82836745332e 544 double kp = 0.00011;
sahilmgandhi 20:82836745332e 545 double kd = 0.00014;
kyleliangus 15:b80555a4a8b9 546 int prev = 0;
sahilmgandhi 26:d20f1adac2d3 547
sahilmgandhi 16:d9252437bd92 548 int counter = 0;
kyleliangus 15:b80555a4a8b9 549 while(1)
kyleliangus 15:b80555a4a8b9 550 {
kyleliangus 15:b80555a4a8b9 551 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 552 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 553 int x = count0 - count1;
kyleliangus 15:b80555a4a8b9 554 //double d = kp * x + kd * ( x - prev );
kyleliangus 15:b80555a4a8b9 555 double kppart = kp * x;
kyleliangus 15:b80555a4a8b9 556 double kdpart = kd * (x-prev);
kyleliangus 15:b80555a4a8b9 557 double d = kppart + kdpart;
kyleliangus 15:b80555a4a8b9 558
kyleliangus 15:b80555a4a8b9 559 //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 560 if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
kyleliangus 15:b80555a4a8b9 561 {
kyleliangus 15:b80555a4a8b9 562 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 563 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 564 }
sahilmgandhi 20:82836745332e 565 else if( x > diff + 40 )
kyleliangus 15:b80555a4a8b9 566 {
kyleliangus 15:b80555a4a8b9 567 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 568 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 569 }
sahilmgandhi 16:d9252437bd92 570 // else
sahilmgandhi 16:d9252437bd92 571 // {
sahilmgandhi 16:d9252437bd92 572 // left_motor.brake();
sahilmgandhi 16:d9252437bd92 573 // right_motor.brake();
sahilmgandhi 16:d9252437bd92 574 // }
kyleliangus 15:b80555a4a8b9 575 prev = x;
sahilmgandhi 16:d9252437bd92 576 counter++;
sahilmgandhi 19:7b66a518b6f8 577 if (counter == 5)
sahilmgandhi 16:d9252437bd92 578 break;
kyleliangus 15:b80555a4a8b9 579 }
kyleliangus 15:b80555a4a8b9 580 }
sahilmgandhi 26:d20f1adac2d3 581
christine222 22:681190ff98f0 582 void nCellEncoderAndIR(double cellCount){
sahilmgandhi 19:7b66a518b6f8 583 double currentError = 0;
sahilmgandhi 19:7b66a518b6f8 584 double previousError = 0;
sahilmgandhi 19:7b66a518b6f8 585 double derivError = 0;
sahilmgandhi 19:7b66a518b6f8 586 double sumError = 0;
christine222 25:f827a8b7880e 587
christine222 25:f827a8b7880e 588 double HIGH_PWM_VOLTAGE = 0.1;
christine222 25:f827a8b7880e 589 double rightSpeed = 0.10;
christine222 25:f827a8b7880e 590 double leftSpeed = 0.10;
christine222 25:f827a8b7880e 591
christine222 25:f827a8b7880e 592 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 593 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 594
christine222 25:f827a8b7880e 595 left_motor.forward(0.28);
christine222 25:f827a8b7880e 596 right_motor.forward(0.25);
christine222 25:f827a8b7880e 597
christine222 25:f827a8b7880e 598 float receiverTwoReading = 0.0;
christine222 25:f827a8b7880e 599 float receiverThreeReading = 0.0;
christine222 25:f827a8b7880e 600
sahilmgandhi 19:7b66a518b6f8 601 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 602 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 603
christine222 25:f827a8b7880e 604 // float previr2 = ir2;
christine222 25:f827a8b7880e 605 // float previr3 = ir3;
christine222 25:f827a8b7880e 606
christine222 23:690b0ca34ee9 607 int state = 0;
christine222 23:690b0ca34ee9 608
sahilmgandhi 26:d20f1adac2d3 609 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
christine222 25:f827a8b7880e 610 receiverTwoReading = IRP_2.getSamples(100);
christine222 25:f827a8b7880e 611 receiverThreeReading = IRP_3.getSamples(100);
christine222 25:f827a8b7880e 612 // previr2 = receiverTwoReading;
christine222 25:f827a8b7880e 613 // previr3 = receiverThreeReading;
christine222 25:f827a8b7880e 614 receiverOneReading = IRP_1.getSamples(100);
christine222 25:f827a8b7880e 615 receiverFourReading = IRP_4.getSamples(100);
christine222 25:f827a8b7880e 616
sahilmgandhi 26:d20f1adac2d3 617 if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){
sahilmgandhi 26:d20f1adac2d3 618 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 619 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 620 }
sahilmgandhi 26:d20f1adac2d3 621 else if (currDir % 4 == 1){
sahilmgandhi 26:d20f1adac2d3 622 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 623 }
sahilmgandhi 26:d20f1adac2d3 624 else if (currDir % 4 == 2){
sahilmgandhi 26:d20f1adac2d3 625 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 626 }
sahilmgandhi 26:d20f1adac2d3 627 else if (currDir % 4 == 3){
sahilmgandhi 26:d20f1adac2d3 628 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 629 }
sahilmgandhi 26:d20f1adac2d3 630 break;
sahilmgandhi 26:d20f1adac2d3 631 }
christine222 25:f827a8b7880e 632
sahilmgandhi 26:d20f1adac2d3 633 if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL)) ){
christine222 25:f827a8b7880e 634 // both sides gone
christine222 25:f827a8b7880e 635 redLed.write(1);
christine222 25:f827a8b7880e 636 greenLed.write(1);
christine222 25:f827a8b7880e 637 blueLed.write(1);
christine222 25:f827a8b7880e 638 wait_ms(100);
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);
christine222 25:f827a8b7880e 650 wait_ms(200);
christine222 25:f827a8b7880e 651 redLed.write(1);
christine222 25:f827a8b7880e 652 greenLed.write(1);
christine222 25:f827a8b7880e 653 blueLed.write(0);
sahilmgandhi 26:d20f1adac2d3 654
sahilmgandhi 26:d20f1adac2d3 655 int prev0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 656 int prev1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 657 int diff0 = desiredCount0 - prev0;
sahilmgandhi 26:d20f1adac2d3 658 int diff1 = desiredCount1 - prev1;
sahilmgandhi 26:d20f1adac2d3 659 int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
sahilmgandhi 26:d20f1adac2d3 660 moveForwardEncoder(valToPass);
sahilmgandhi 26:d20f1adac2d3 661 }
sahilmgandhi 26:d20f1adac2d3 662 else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone
sahilmgandhi 26:d20f1adac2d3 663 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 664 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 665 }
sahilmgandhi 26:d20f1adac2d3 666 else if (currDir % 4 == 1){
sahilmgandhi 26:d20f1adac2d3 667 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 668 }
sahilmgandhi 26:d20f1adac2d3 669 else if (currDir % 4 == 2){
sahilmgandhi 26:d20f1adac2d3 670 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 671 }
sahilmgandhi 26:d20f1adac2d3 672 else if (currDir % 4 == 3){
sahilmgandhi 26:d20f1adac2d3 673 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 674 }
christine222 25:f827a8b7880e 675 // RED RED RED RED RED
christine222 25:f827a8b7880e 676 state = 1;
christine222 25:f827a8b7880e 677 redLed.write(0);
christine222 25:f827a8b7880e 678 greenLed.write(1);
christine222 25:f827a8b7880e 679 blueLed.write(1);
christine222 25:f827a8b7880e 680 }else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){// left wall gone
sahilmgandhi 26:d20f1adac2d3 681 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 682 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 683 }
sahilmgandhi 26:d20f1adac2d3 684 else if (currDir % 4 == 1){
sahilmgandhi 26:d20f1adac2d3 685 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 686 }
sahilmgandhi 26:d20f1adac2d3 687 else if (currDir % 4 == 2){
sahilmgandhi 26:d20f1adac2d3 688 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 689 }
sahilmgandhi 26:d20f1adac2d3 690 else if (currDir % 4 == 3){
sahilmgandhi 26:d20f1adac2d3 691 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 692 }
christine222 25:f827a8b7880e 693 // BLUE BLUE BLUE BLUE
christine222 25:f827a8b7880e 694 state = 2;
christine222 25:f827a8b7880e 695 redLed.write(1);
christine222 25:f827a8b7880e 696 greenLed.write(1);
christine222 25:f827a8b7880e 697 blueLed.write(0);
christine222 25:f827a8b7880e 698 }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
sahilmgandhi 26:d20f1adac2d3 699 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 700 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 701 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 702 }
sahilmgandhi 26:d20f1adac2d3 703 else if (currDir % 4 == 1){
sahilmgandhi 26:d20f1adac2d3 704 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 705 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 706 }
sahilmgandhi 26:d20f1adac2d3 707 else if (currDir % 4 == 2){
sahilmgandhi 26:d20f1adac2d3 708 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 709 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 710 }
sahilmgandhi 26:d20f1adac2d3 711 else if (currDir % 4 == 3){
sahilmgandhi 26:d20f1adac2d3 712 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 26:d20f1adac2d3 713 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 26:d20f1adac2d3 714 }
christine222 25:f827a8b7880e 715 // both walls there
christine222 25:f827a8b7880e 716 state = 0;
christine222 25:f827a8b7880e 717 redLed.write(1);
christine222 25:f827a8b7880e 718 greenLed.write(0);
christine222 25:f827a8b7880e 719 blueLed.write(1);
christine222 25:f827a8b7880e 720 }
christine222 25:f827a8b7880e 721
christine222 25:f827a8b7880e 722 switch(state){
christine222 25:f827a8b7880e 723 case(0):{ // both walls there
christine222 25:f827a8b7880e 724 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 725 break;
christine222 25:f827a8b7880e 726 }
christine222 25:f827a8b7880e 727 case(1):{// RED RED RED RED RED
christine222 25:f827a8b7880e 728 currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
christine222 25:f827a8b7880e 729 break;
christine222 25:f827a8b7880e 730 }
christine222 25:f827a8b7880e 731 case(2):{// blue
christine222 25:f827a8b7880e 732 currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 733 break;
christine222 25:f827a8b7880e 734 }
christine222 25:f827a8b7880e 735 default:{
christine222 25:f827a8b7880e 736 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 737 //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 738 break;
christine222 25:f827a8b7880e 739 }
christine222 25:f827a8b7880e 740 }
christine222 25:f827a8b7880e 741
christine222 25:f827a8b7880e 742 sumError += currentError;
christine222 25:f827a8b7880e 743 derivError = currentError - previousError;
christine222 25:f827a8b7880e 744 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 25:f827a8b7880e 745 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
christine222 25:f827a8b7880e 746 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 747 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 748 } else { // r is faster than L. speed up l, slow down r
christine222 25:f827a8b7880e 749 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 750 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 25:f827a8b7880e 751 }
christine222 25:f827a8b7880e 752 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 25:f827a8b7880e 753 if (leftSpeed < 0) leftSpeed = 0;
christine222 25:f827a8b7880e 754 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 25:f827a8b7880e 755 if (rightSpeed < 0) rightSpeed = 0;
sahilmgandhi 26:d20f1adac2d3 756
christine222 25:f827a8b7880e 757 right_motor.forward(rightSpeed);
christine222 25:f827a8b7880e 758 left_motor.forward(leftSpeed);
christine222 25:f827a8b7880e 759 pidOnEncoders();
sahilmgandhi 26:d20f1adac2d3 760
christine222 25:f827a8b7880e 761 previousError = currentError;
christine222 25:f827a8b7880e 762 ir2 = IRP_2.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 763 ir3 = IRP_3.getSamples( SAMPLE_NUM );
sahilmgandhi 26:d20f1adac2d3 764 }
sahilmgandhi 26:d20f1adac2d3 765 if (currDir % 4 == 0){
sahilmgandhi 26:d20f1adac2d3 766 mouseY += 1;
sahilmgandhi 26:d20f1adac2d3 767 }
sahilmgandhi 26:d20f1adac2d3 768 else if (currDir % 4 == 1){
sahilmgandhi 26:d20f1adac2d3 769 mouseX + 1;
sahilmgandhi 19:7b66a518b6f8 770 }
sahilmgandhi 26:d20f1adac2d3 771 else if (currDir % 4 == 2){
sahilmgandhi 26:d20f1adac2d3 772 mouseY -= 1;
sahilmgandhi 26:d20f1adac2d3 773 }
sahilmgandhi 26:d20f1adac2d3 774 else if (currDir % 4 == 3){
sahilmgandhi 26:d20f1adac2d3 775 mouseX -= 1;
sahilmgandhi 26:d20f1adac2d3 776 }
sahilmgandhi 26:d20f1adac2d3 777
sahilmgandhi 19:7b66a518b6f8 778 left_motor.brake();
sahilmgandhi 19:7b66a518b6f8 779 right_motor.brake();
christine222 23:690b0ca34ee9 780 return;
sahilmgandhi 17:f713758f6238 781 }
christine222 25:f827a8b7880e 782
sahilmgandhi 26:d20f1adac2d3 783 bool isWallInFront(int x, int y){
sahilmgandhi 26:d20f1adac2d3 784 return (wallArray[MAZE_LEN - y - 1][x] & 0x1);
sahilmgandhi 26:d20f1adac2d3 785 }
sahilmgandhi 26:d20f1adac2d3 786 bool isWallInBack(int x, int y){
sahilmgandhi 26:d20f1adac2d3 787 return (wallArray[MAZE_LEN - y - 1][x] & 0x8);
sahilmgandhi 26:d20f1adac2d3 788 }
sahilmgandhi 26:d20f1adac2d3 789 bool isWallOnRight(int x, int y){
sahilmgandhi 26:d20f1adac2d3 790 return (wallArray[MAZE_LEN - y - 1][x] & 0x4);
sahilmgandhi 26:d20f1adac2d3 791 }
sahilmgandhi 26:d20f1adac2d3 792 bool isWallOnLeft(int x, int y){
sahilmgandhi 26:d20f1adac2d3 793 return (wallArray[MAZE_LEN - y - 1][x] & 0x2);
sahilmgandhi 26:d20f1adac2d3 794 }
sahilmgandhi 26:d20f1adac2d3 795
sahilmgandhi 26:d20f1adac2d3 796 int chooseNextMovement(){
sahilmgandhi 26:d20f1adac2d3 797 int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
sahilmgandhi 26:d20f1adac2d3 798 if (goingToCenter && (currentDistance == 0)){
sahilmgandhi 26:d20f1adac2d3 799 goingToCenter = false;
sahilmgandhi 26:d20f1adac2d3 800 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 801 }
sahilmgandhi 26:d20f1adac2d3 802 else if (!goingToCenter && (currentDistance == 0)){
sahilmgandhi 26:d20f1adac2d3 803 goingToCenter == true;
sahilmgandhi 26:d20f1adac2d3 804 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 805 }
sahilmgandhi 26:d20f1adac2d3 806
sahilmgandhi 26:d20f1adac2d3 807 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 808 int currX = 0;
sahilmgandhi 26:d20f1adac2d3 809 int currY = 0;
sahilmgandhi 26:d20f1adac2d3 810 int currDist = 0;
sahilmgandhi 26:d20f1adac2d3 811 int dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 812 if (!justTurned){
sahilmgandhi 26:d20f1adac2d3 813 cellsToVisit.push(make_pair(mouseX, mouseY));
sahilmgandhi 26:d20f1adac2d3 814 while (!cellsToVisit.empty()) {
sahilmgandhi 26:d20f1adac2d3 815 pair<int, int> curr = cellsToVisit.top();
sahilmgandhi 26:d20f1adac2d3 816 cellsToVisit.pop();
sahilmgandhi 26:d20f1adac2d3 817 currX = curr.first;
sahilmgandhi 26:d20f1adac2d3 818 currY = curr.second;
sahilmgandhi 26:d20f1adac2d3 819 currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
sahilmgandhi 26:d20f1adac2d3 820 int minDist = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 821
sahilmgandhi 26:d20f1adac2d3 822 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 823 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 824 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 825 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 826 }
sahilmgandhi 26:d20f1adac2d3 827 }
sahilmgandhi 26:d20f1adac2d3 828 if (currX - 1 >= 0 && !isWallOnLeft((unsigned) currX, (unsigned) currY)) {
sahilmgandhi 26:d20f1adac2d3 829 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 830 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 831 }
sahilmgandhi 26:d20f1adac2d3 832 }
sahilmgandhi 26:d20f1adac2d3 833 if (currY + 1 < MAZE_LEN && !isWallInFront((unsigned) currX, (unsigned) currY)) {
sahilmgandhi 26:d20f1adac2d3 834 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 835 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 836 }
sahilmgandhi 26:d20f1adac2d3 837 }
sahilmgandhi 26:d20f1adac2d3 838 if (currY - 1 >= 0 && !isWallInBack((unsigned) currX, (unsigned) currY)) {
sahilmgandhi 26:d20f1adac2d3 839 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 840 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 841 }
sahilmgandhi 26:d20f1adac2d3 842 }
sahilmgandhi 26:d20f1adac2d3 843 } else {
sahilmgandhi 26:d20f1adac2d3 844 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 845 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 846 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 847 }
sahilmgandhi 26:d20f1adac2d3 848 }
sahilmgandhi 26:d20f1adac2d3 849 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 850 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 851 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 852 }
sahilmgandhi 26:d20f1adac2d3 853 }
sahilmgandhi 26:d20f1adac2d3 854 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 855 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 856 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 857 }
sahilmgandhi 26:d20f1adac2d3 858 }
sahilmgandhi 26:d20f1adac2d3 859 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 860 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 861 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 862 }
sahilmgandhi 26:d20f1adac2d3 863 }
sahilmgandhi 26:d20f1adac2d3 864 }
sahilmgandhi 26:d20f1adac2d3 865
sahilmgandhi 26:d20f1adac2d3 866 if (minDist == INT_MAX)
sahilmgandhi 26:d20f1adac2d3 867 continue;
sahilmgandhi 26:d20f1adac2d3 868 if (currDist > minDist)
sahilmgandhi 26:d20f1adac2d3 869 continue;
sahilmgandhi 26:d20f1adac2d3 870 if (currDist <= minDist) {
sahilmgandhi 26:d20f1adac2d3 871 // cout << "Changing the following coordinate ( " << currX << "," << currY << ") from "
sahilmgandhi 26:d20f1adac2d3 872 // << manhattanDistances[MAZE_LEN - 1 - currY][currX] << " to " << minDist + 1 << endl;
sahilmgandhi 26:d20f1adac2d3 873 manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
sahilmgandhi 26:d20f1adac2d3 874 }
sahilmgandhi 26:d20f1adac2d3 875 if (hasVisited(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 876 if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 877 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 878 }
sahilmgandhi 26:d20f1adac2d3 879 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 880 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 881 }
sahilmgandhi 26:d20f1adac2d3 882 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 883 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 884 }
sahilmgandhi 26:d20f1adac2d3 885 if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 886 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 887 }
sahilmgandhi 26:d20f1adac2d3 888 } else {
sahilmgandhi 26:d20f1adac2d3 889 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 890 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 891 }
sahilmgandhi 26:d20f1adac2d3 892 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 893 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 894 }
sahilmgandhi 26:d20f1adac2d3 895 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 896 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 897 }
sahilmgandhi 26:d20f1adac2d3 898 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 899 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 900 }
sahilmgandhi 26:d20f1adac2d3 901 }
sahilmgandhi 26:d20f1adac2d3 902 }
sahilmgandhi 26:d20f1adac2d3 903
sahilmgandhi 26:d20f1adac2d3 904 int minDistance = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 905 if (currDir % 4 == 0) {
sahilmgandhi 26:d20f1adac2d3 906 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(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 = 1;
sahilmgandhi 26:d20f1adac2d3 910 }
sahilmgandhi 26:d20f1adac2d3 911 }
sahilmgandhi 26:d20f1adac2d3 912 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 913 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 914 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 915 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 916 }
sahilmgandhi 26:d20f1adac2d3 917 }
sahilmgandhi 26:d20f1adac2d3 918 if (mouseY + 1 < MAZE_LEN && !isWallInFront(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 = 3;
sahilmgandhi 26:d20f1adac2d3 922 }
sahilmgandhi 26:d20f1adac2d3 923 }
sahilmgandhi 26:d20f1adac2d3 924 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 925 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 926 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 927 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 928 }
sahilmgandhi 26:d20f1adac2d3 929 }
sahilmgandhi 26:d20f1adac2d3 930 } else if (currDir % 4 == 2) {
sahilmgandhi 26:d20f1adac2d3 931 if (mouseX - 1 >= 0 && !isWallOnRight(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 = 1;
sahilmgandhi 26:d20f1adac2d3 935 }
sahilmgandhi 26:d20f1adac2d3 936 }
sahilmgandhi 26:d20f1adac2d3 937 if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(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 = 2;
sahilmgandhi 26:d20f1adac2d3 941 }
sahilmgandhi 26:d20f1adac2d3 942 }
sahilmgandhi 26:d20f1adac2d3 943 if (mouseY - 1 >= 0 && !isWallInFront(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 = 3;
sahilmgandhi 26:d20f1adac2d3 947 }
sahilmgandhi 26:d20f1adac2d3 948 }
sahilmgandhi 26:d20f1adac2d3 949 if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 950 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 951 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 952 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 953 }
sahilmgandhi 26:d20f1adac2d3 954 }
sahilmgandhi 26:d20f1adac2d3 955 } else if (currDir % 4 == 1) {
sahilmgandhi 26:d20f1adac2d3 956 if (mouseY - 1 >= 0 && !isWallOnRight(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 = 1;
sahilmgandhi 26:d20f1adac2d3 960 }
sahilmgandhi 26:d20f1adac2d3 961 }
sahilmgandhi 26:d20f1adac2d3 962 if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 963 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 964 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 965 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 966 }
sahilmgandhi 26:d20f1adac2d3 967 }
sahilmgandhi 26:d20f1adac2d3 968 if (mouseX + 1 < MAZE_LEN && !isWallInFront(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 = 3;
sahilmgandhi 26:d20f1adac2d3 972 }
sahilmgandhi 26:d20f1adac2d3 973 }
sahilmgandhi 26:d20f1adac2d3 974 if (mouseX - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 975 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 976 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 977 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 978 }
sahilmgandhi 26:d20f1adac2d3 979 }
sahilmgandhi 26:d20f1adac2d3 980 } else if (currDir % 4 == 3) {
sahilmgandhi 26:d20f1adac2d3 981 if (mouseY + 1 < MAZE_LEN && !isWallOnRight(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 = 1;
sahilmgandhi 26:d20f1adac2d3 985 }
sahilmgandhi 26:d20f1adac2d3 986 }
sahilmgandhi 26:d20f1adac2d3 987 if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 988 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 989 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 990 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 991 }
sahilmgandhi 26:d20f1adac2d3 992 }
sahilmgandhi 26:d20f1adac2d3 993 if (mouseX - 1 >= 0 && !isWallInFront(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 = 3;
sahilmgandhi 26:d20f1adac2d3 997 }
sahilmgandhi 26:d20f1adac2d3 998 }
sahilmgandhi 26:d20f1adac2d3 999 if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 1000 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 1001 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 1002 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 1003 }
sahilmgandhi 26:d20f1adac2d3 1004 }
sahilmgandhi 26:d20f1adac2d3 1005 }
sahilmgandhi 26:d20f1adac2d3 1006 }
sahilmgandhi 26:d20f1adac2d3 1007 else{
sahilmgandhi 26:d20f1adac2d3 1008 justTurned = false;
sahilmgandhi 26:d20f1adac2d3 1009 dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 1010 }
sahilmgandhi 26:d20f1adac2d3 1011
sahilmgandhi 26:d20f1adac2d3 1012 return dirToGo;
sahilmgandhi 26:d20f1adac2d3 1013 }
sahilmgandhi 26:d20f1adac2d3 1014
sahilmgandhi 26:d20f1adac2d3 1015 bool hasVisited(int x, int y){
sahilmgandhi 26:d20f1adac2d3 1016 return visitedCells[MAZE_LEN - 1 - y][x];
sahilmgandhi 26:d20f1adac2d3 1017 }
sahilmgandhi 26:d20f1adac2d3 1018
sahilmgandhi 26:d20f1adac2d3 1019 void changeManhattanDistance(bool headCenter){
sahilmgandhi 26:d20f1adac2d3 1020 if (headCenter){
sahilmgandhi 26:d20f1adac2d3 1021 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1022 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1023 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(7 - i);
sahilmgandhi 26:d20f1adac2d3 1024 }
sahilmgandhi 26:d20f1adac2d3 1025 }
sahilmgandhi 26:d20f1adac2d3 1026 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1027 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1028 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(8 - i);
sahilmgandhi 26:d20f1adac2d3 1029 }
sahilmgandhi 26:d20f1adac2d3 1030 }
sahilmgandhi 26:d20f1adac2d3 1031 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1032 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1033 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(7 - i);
sahilmgandhi 26:d20f1adac2d3 1034 }
sahilmgandhi 26:d20f1adac2d3 1035 }
sahilmgandhi 26:d20f1adac2d3 1036 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1037 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1038 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(8 - i);
sahilmgandhi 26:d20f1adac2d3 1039 }
sahilmgandhi 26:d20f1adac2d3 1040 }
sahilmgandhi 26:d20f1adac2d3 1041 }
sahilmgandhi 26:d20f1adac2d3 1042 else {
sahilmgandhi 26:d20f1adac2d3 1043 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1044 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1045 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1046 }
sahilmgandhi 26:d20f1adac2d3 1047 }
sahilmgandhi 26:d20f1adac2d3 1048 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1049 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 26:d20f1adac2d3 1050 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1051 }
sahilmgandhi 26:d20f1adac2d3 1052 }
sahilmgandhi 26:d20f1adac2d3 1053 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 26:d20f1adac2d3 1054 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1055 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1056 }
sahilmgandhi 26:d20f1adac2d3 1057 }
sahilmgandhi 26:d20f1adac2d3 1058 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 26:d20f1adac2d3 1059 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 26:d20f1adac2d3 1060 manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 1061 }
sahilmgandhi 26:d20f1adac2d3 1062 }
sahilmgandhi 26:d20f1adac2d3 1063 }
sahilmgandhi 26:d20f1adac2d3 1064 }
sahilmgandhi 26:d20f1adac2d3 1065
sahilmgandhi 0:a03c771ab78e 1066 int main()
sahilmgandhi 0:a03c771ab78e 1067 {
christine222 3:880f15be8c72 1068 //Set highest bandwidth.
christine222 23:690b0ca34ee9 1069 //gyro.setLpBandwidth(LPFBW_42HZ);
christine222 3:880f15be8c72 1070 serial.baud(9600);
christine222 23:690b0ca34ee9 1071 //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 26:d20f1adac2d3 1072
sahilmgandhi 1:8a4b2f573923 1073 wait (0.1);
sahilmgandhi 26:d20f1adac2d3 1074
sahilmgandhi 2:771db996cee0 1075 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 1076 greenLed.write(0);
sahilmgandhi 2:771db996cee0 1077 blueLed.write(1);
sahilmgandhi 26:d20f1adac2d3 1078
kyleliangus 9:1d8e4da058cd 1079 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 1080 //right_motor.forward(0.1);
sahilmgandhi 26:d20f1adac2d3 1081
kyleliangus 8:a0760acdc59e 1082 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 1083 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 1084 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 1085 // PB_3 is B of left
vanshg 11:8fc2b703086b 1086 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 1087 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 1088
vanshg 10:810d1849da9d 1089 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 1090 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 1091 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 1092 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 1093 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 1094 */
sahilmgandhi 26:d20f1adac2d3 1095
kyleliangus 12:5790e56a056f 1096 // 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 1097 // of alternate function specified
kyleliangus 12:5790e56a056f 1098 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 1099 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 1100 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 1101 //
kyleliangus 12:5790e56a056f 1102 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 1103 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 1104 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 1105 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 1106 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 1107 // CR1 reenabales
kyleliangus 12:5790e56a056f 1108 // then read CNT reg of timer
sahilmgandhi 26:d20f1adac2d3 1109
sahilmgandhi 26:d20f1adac2d3 1110
vanshg 10:810d1849da9d 1111 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 1112 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 1113 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 1114 dipButton4.rise(&enableButton4);
sahilmgandhi 26:d20f1adac2d3 1115
vanshg 10:810d1849da9d 1116 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 1117 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 1118 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 1119 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 1120
sahilmgandhi 26:d20f1adac2d3 1121 // if(dipFlags == 0x1){
sahilmgandhi 26:d20f1adac2d3 1122 turnRight180();
sahilmgandhi 26:d20f1adac2d3 1123 wait_ms(1000);
sahilmgandhi 26:d20f1adac2d3 1124 // }else{
sahilmgandhi 26:d20f1adac2d3 1125 // turnRight();
sahilmgandhi 26:d20f1adac2d3 1126 // IRP_1.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 1127 // IRP_4.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 1128 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 1129 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 1130
sahilmgandhi 26:d20f1adac2d3 1131 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 1132 // turnLeft();
sahilmgandhi 26:d20f1adac2d3 1133 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 1134 // }
sahilmgandhi 26:d20f1adac2d3 1135
sahilmgandhi 26:d20f1adac2d3 1136
sahilmgandhi 26:d20f1adac2d3 1137 // init the wall, and mouse loc arrays:
sahilmgandhi 26:d20f1adac2d3 1138 wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
sahilmgandhi 26:d20f1adac2d3 1139 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 1140
christine222 25:f827a8b7880e 1141 //right_motor.forward( 0.2 );
christine222 25:f827a8b7880e 1142 //left_motor.forward( 0.2 );
sahilmgandhi 26:d20f1adac2d3 1143 //turnRight180();
sahilmgandhi 26:d20f1adac2d3 1144 //wait_ms(1500);
sahilmgandhi 26:d20f1adac2d3 1145 int nextMovement = 0;
sahilmgandhi 26:d20f1adac2d3 1146 while (1) {
sahilmgandhi 26:d20f1adac2d3 1147 nextMovement = chooseNextMovement();
sahilmgandhi 26:d20f1adac2d3 1148 if (nextMovement == 0){
sahilmgandhi 26:d20f1adac2d3 1149 nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 1150 }
sahilmgandhi 26:d20f1adac2d3 1151 else if (nextMovement == 1){
sahilmgandhi 26:d20f1adac2d3 1152 justTurned = true;
sahilmgandhi 26:d20f1adac2d3 1153 turnRight();
sahilmgandhi 26:d20f1adac2d3 1154 }
sahilmgandhi 26:d20f1adac2d3 1155 else if (nextMovement == 2){
sahilmgandhi 26:d20f1adac2d3 1156 justTurned = true;
sahilmgandhi 26:d20f1adac2d3 1157 turnLeft();
sahilmgandhi 26:d20f1adac2d3 1158 }
sahilmgandhi 26:d20f1adac2d3 1159 else if (nextMovement == 3){
sahilmgandhi 26:d20f1adac2d3 1160 nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 1161 }
sahilmgandhi 26:d20f1adac2d3 1162 else if (nextMovement == 4){
sahilmgandhi 26:d20f1adac2d3 1163 justTurned = true;
sahilmgandhi 26:d20f1adac2d3 1164 turnRight180();
sahilmgandhi 26:d20f1adac2d3 1165 }
sahilmgandhi 26:d20f1adac2d3 1166 wait_ms(1000); // reduce this once we fine tune this!
christine222 24:e7063765d6f0 1167
christine222 23:690b0ca34ee9 1168 //wait_ms(1500);
christine222 23:690b0ca34ee9 1169 //turnRight();
christine222 23:690b0ca34ee9 1170 //wait_ms(1500);
christine222 23:690b0ca34ee9 1171 //turnLeft();
sahilmgandhi 26:d20f1adac2d3 1172 // nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 1173 // wait_ms(500);
christine222 23:690b0ca34ee9 1174 // turnRight();
christine222 23:690b0ca34ee9 1175 // wait_ms(500);
christine222 23:690b0ca34ee9 1176 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 1177 // wait_ms(500);
christine222 23:690b0ca34ee9 1178 // turnRight();
christine222 23:690b0ca34ee9 1179 // wait_ms(500);
christine222 23:690b0ca34ee9 1180 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 1181 // wait_ms(500);
christine222 23:690b0ca34ee9 1182 // turnLeft();
christine222 23:690b0ca34ee9 1183 // wait_ms(500);
christine222 23:690b0ca34ee9 1184 // nCellEncoderAndIR(2);
christine222 23:690b0ca34ee9 1185 // wait_ms(500);
christine222 23:690b0ca34ee9 1186 // turnRight();
christine222 23:690b0ca34ee9 1187 // wait_ms(500);
christine222 23:690b0ca34ee9 1188 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 1189 // wait_ms(500);
christine222 23:690b0ca34ee9 1190 // turnRight();
christine222 23:690b0ca34ee9 1191 // wait_ms(500);
christine222 23:690b0ca34ee9 1192 // nCellEncoderAndIR(5);
christine222 23:690b0ca34ee9 1193 // break;
christine222 23:690b0ca34ee9 1194 // turnRight180();
sahilmgandhi 26:d20f1adac2d3 1195
christine222 23:690b0ca34ee9 1196 // int number = rand() % 4 + 1;
christine222 23:690b0ca34ee9 1197 // switch(number){
christine222 23:690b0ca34ee9 1198 // case(1):{//turn right
christine222 23:690b0ca34ee9 1199 // turnRight();
christine222 23:690b0ca34ee9 1200 // break;
christine222 23:690b0ca34ee9 1201 // }
christine222 23:690b0ca34ee9 1202 // case(2):{ // turn left
christine222 23:690b0ca34ee9 1203 // turnLeft();
christine222 23:690b0ca34ee9 1204 // break;
christine222 23:690b0ca34ee9 1205 // }
christine222 23:690b0ca34ee9 1206 // case(3):{// keep going
sahilmgandhi 26:d20f1adac2d3 1207
christine222 23:690b0ca34ee9 1208 // break;
christine222 23:690b0ca34ee9 1209 // }
christine222 23:690b0ca34ee9 1210 // case(4):{// turnaround
christine222 23:690b0ca34ee9 1211 // turnRight180();
christine222 23:690b0ca34ee9 1212 // break;
christine222 23:690b0ca34ee9 1213 // }
christine222 23:690b0ca34ee9 1214 // default:{// keep going
christine222 23:690b0ca34ee9 1215 // break;
christine222 23:690b0ca34ee9 1216 // }
christine222 23:690b0ca34ee9 1217 // }
sahilmgandhi 26:d20f1adac2d3 1218
christine222 23:690b0ca34ee9 1219 // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
christine222 23:690b0ca34ee9 1220 // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
sahilmgandhi 26:d20f1adac2d3 1221
christine222 23:690b0ca34ee9 1222 // float ir3 = IRP_2.getSamples(100)/initAverageL;
christine222 23:690b0ca34ee9 1223 // float ir2 = IRP_3.getSamples(100)/initAverageR;
christine222 22:681190ff98f0 1224
sahilmgandhi 26:d20f1adac2d3 1225
christine222 25:f827a8b7880e 1226
christine222 23:690b0ca34ee9 1227 /*
christine222 23:690b0ca34ee9 1228 counter2++;
christine222 23:690b0ca34ee9 1229 counter3++;
christine222 23:690b0ca34ee9 1230 ir2tot += IRP_2.getSamples(100);
christine222 23:690b0ca34ee9 1231 ir3tot += IRP_3.getSamples(100);
sahilmgandhi 26:d20f1adac2d3 1232
sahilmgandhi 26:d20f1adac2d3 1233
christine222 23:690b0ca34ee9 1234 ir2 = ir2tot/counter2;
christine222 23:690b0ca34ee9 1235 ir3 = ir3tot/counter3;
christine222 23:690b0ca34ee9 1236
sahilmgandhi 26:d20f1adac2d3 1237
christine222 23:690b0ca34ee9 1238 serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1239 */
christine222 23:690b0ca34ee9 1240 //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
christine222 23:690b0ca34ee9 1241 //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3);
christine222 23:690b0ca34ee9 1242 //break;
christine222 23:690b0ca34ee9 1243 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
christine222 23:690b0ca34ee9 1244 //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1245 //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
sahilmgandhi 26:d20f1adac2d3 1246
sahilmgandhi 26:d20f1adac2d3 1247
christine222 23:690b0ca34ee9 1248 ////////////////////////////////////////////////////////////////
sahilmgandhi 26:d20f1adac2d3 1249
christine222 22:681190ff98f0 1250 //nCellEncoderAndIR(3);
christine222 22:681190ff98f0 1251 //break;
sahilmgandhi 26:d20f1adac2d3 1252
christine222 23:690b0ca34ee9 1253 //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 1254
christine222 21:9a6cb07bdcb6 1255 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
sahilmgandhi 26:d20f1adac2d3 1256
christine222 21:9a6cb07bdcb6 1257 //break;
christine222 21:9a6cb07bdcb6 1258 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1259 // wait(0.5);
christine222 21:9a6cb07bdcb6 1260 // handleTurns();
christine222 21:9a6cb07bdcb6 1261 // wait(0.5);
christine222 21:9a6cb07bdcb6 1262 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1263 // wait(0.5);
christine222 21:9a6cb07bdcb6 1264 // handleTurns();
christine222 21:9a6cb07bdcb6 1265 //break;
sahilmgandhi 16:d9252437bd92 1266 //pidOnEncoders();
sahilmgandhi 20:82836745332e 1267 // moveForwardUntilWallIr();
kyleliangus 8:a0760acdc59e 1268 //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
kyleliangus 15:b80555a4a8b9 1269 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
sahilmgandhi 17:f713758f6238 1270 // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
christine222 21:9a6cb07bdcb6 1271 //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 1272
christine222 3:880f15be8c72 1273 //reading = Rec_4.read();
christine222 3:880f15be8c72 1274 // serial.printf("reading: %f\n", reading);
christine222 3:880f15be8c72 1275 }
sahilmgandhi 26:d20f1adac2d3 1276 }