Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Committer:
sahilmgandhi
Date:
Fri Jun 02 19:35:26 2017 +0000
Revision:
45:8b0bee6baf38
Parent:
44:85bf2c0cd518
Child:
46:b156ef445742
Final CAMM AAMC Working Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sahilmgandhi 0:a03c771ab78e 1 #include "mbed.h"
sahilmgandhi 31:9b71b44e0867 2
kyleliangus 6:3d68fedd6fd9 3 #include "irpair.h"
kyleliangus 4:b5b7836ca2b0 4 #include "main.h"
kyleliangus 4:b5b7836ca2b0 5 #include "motor.h"
sahilmgandhi 31:9b71b44e0867 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"
christine222 25:f827a8b7880e 14
sahilmgandhi 33:68ce1f74ab5f 15 /*LEFT/RIGHT BOTH WALLS
sahilmgandhi 33:68ce1f74ab5f 16 0.34 0.12
sahilmgandhi 33:68ce1f74ab5f 17
sahilmgandhi 33:68ce1f74ab5f 18 LEFT/RIGHT LEFT WALL GONE
sahilmgandhi 33:68ce1f74ab5f 19 0.02 0.11
sahilmgandhi 31:9b71b44e0867 20
sahilmgandhi 33:68ce1f74ab5f 21 LEFT/RIGHT RIGTH WALL GONE
sahilmgandhi 33:68ce1f74ab5f 22 0.33 0.008
sahilmgandhi 31:9b71b44e0867 23
sahilmgandhi 33:68ce1f74ab5f 24 LEFT/RIGHT BOTH WALLS GONE
sahilmgandhi 33:68ce1f74ab5f 25 0.02 0.008
sahilmgandhi 33:68ce1f74ab5f 26
sahilmgandhi 33:68ce1f74ab5f 27 LEFT/RIGHT CLOSE TO RIGHT WALL
sahilmgandhi 33:68ce1f74ab5f 28 0.14 0.47
sahilmgandhi 31:9b71b44e0867 29
sahilmgandhi 33:68ce1f74ab5f 30 LEFT/RIGHT CLOSE TO LEFT WALL
sahilmgandhi 33:68ce1f74ab5f 31 0.89 0.05
sahilmgandhi 31:9b71b44e0867 32
sahilmgandhi 33:68ce1f74ab5f 33 FRONT IRS NEAR WALL (STOPPING DISTANCE)
sahilmgandhi 33:68ce1f74ab5f 34 0.41 0.49
sahilmgandhi 31:9b71b44e0867 35
sahilmgandhi 33:68ce1f74ab5f 36 FRONT IRS NO WALL AHEAD (FOR ATLEAST 1 FULL CELL)
sahilmgandhi 33:68ce1f74ab5f 37 0.07 0.06
sahilmgandhi 31:9b71b44e0867 38
sahilmgandhi 33:68ce1f74ab5f 39 */
sahilmgandhi 34:69342782fb68 40
christine222 44:85bf2c0cd518 41 #define IP_CONSTANT 1.6
sahilmgandhi 37:3dcc95e9321c 42 #define II_CONSTANT 0.00001
christine222 44:85bf2c0cd518 43 #define ID_CONSTANT 0.24
sahilmgandhi 29:ec2c5a69acd6 44
sahilmgandhi 16:d9252437bd92 45 void pidOnEncoders();
sahilmgandhi 29:ec2c5a69acd6 46
sahilmgandhi 31:9b71b44e0867 47 void moveForwardEncoder(double num)
sahilmgandhi 31:9b71b44e0867 48 {
christine222 21:9a6cb07bdcb6 49 int count0;
christine222 21:9a6cb07bdcb6 50 int count1;
christine222 21:9a6cb07bdcb6 51 count0 = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 52 count1 = encoder1.getPulses();
christine222 21:9a6cb07bdcb6 53 int initial1 = count1;
christine222 21:9a6cb07bdcb6 54 int initial0 = count0;
christine222 21:9a6cb07bdcb6 55 int diff = count0 - count1;
sahilmgandhi 40:465d2b565977 56 double kp = 0.00022;
sahilmgandhi 40:465d2b565977 57 double kd = 0.00020;
christine222 21:9a6cb07bdcb6 58 int prev = 0;
christine222 25:f827a8b7880e 59
kyleliangus 43:f22168a05c3e 60 double speed0 = WHEEL_SPEED;
sahilmgandhi 37:3dcc95e9321c 61 double speed1 = WHEEL_SPEED;
sahilmgandhi 45:8b0bee6baf38 62
sahilmgandhi 40:465d2b565977 63 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 40:465d2b565977 64 receiverFourReading = IRP_4.getSamples(100);
christine222 21:9a6cb07bdcb6 65 right_motor.move(speed0);
christine222 21:9a6cb07bdcb6 66 left_motor.move(speed1);
christine222 44:85bf2c0cd518 67 wait_ms(100);
sahilmgandhi 31:9b71b44e0867 68
kyleliangus 43:f22168a05c3e 69 double distance_to_go = (oneCellCount)*num;
kyleliangus 42:75257e6c4c76 70
kyleliangus 42:75257e6c4c76 71 while( ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) {
sahilmgandhi 31:9b71b44e0867 72
sahilmgandhi 45:8b0bee6baf38 73 if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) {
kyleliangus 43:f22168a05c3e 74 left_motor.move( speed0/900 );
kyleliangus 43:f22168a05c3e 75 right_motor.move( speed1/900 );
sahilmgandhi 45:8b0bee6baf38 76 } else {
kyleliangus 43:f22168a05c3e 77 left_motor.move(speed0);
kyleliangus 43:f22168a05c3e 78 right_motor.move(speed1);
kyleliangus 43:f22168a05c3e 79 wait_us(16000);
kyleliangus 43:f22168a05c3e 80 pidOnEncoders();
kyleliangus 42:75257e6c4c76 81 }
sahilmgandhi 45:8b0bee6baf38 82
sahilmgandhi 38:fe05f93009a2 83 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 38:fe05f93009a2 84 receiverFourReading = IRP_4.getSamples(100);
sahilmgandhi 38:fe05f93009a2 85 }
sahilmgandhi 45:8b0bee6baf38 86
sahilmgandhi 38:fe05f93009a2 87 right_motor.brake();
sahilmgandhi 38:fe05f93009a2 88 left_motor.brake();
sahilmgandhi 38:fe05f93009a2 89 return;
sahilmgandhi 38:fe05f93009a2 90 }
sahilmgandhi 38:fe05f93009a2 91
sahilmgandhi 45:8b0bee6baf38 92 void encoderAfterTurn(double num)
sahilmgandhi 45:8b0bee6baf38 93 {
sahilmgandhi 38:fe05f93009a2 94 int count0;
sahilmgandhi 38:fe05f93009a2 95 int count1;
sahilmgandhi 38:fe05f93009a2 96 count0 = encoder0.getPulses();
sahilmgandhi 38:fe05f93009a2 97 count1 = encoder1.getPulses();
sahilmgandhi 38:fe05f93009a2 98 int initial1 = count1;
sahilmgandhi 38:fe05f93009a2 99 int initial0 = count0;
sahilmgandhi 38:fe05f93009a2 100 int diff = count0 - count1;
kyleliangus 43:f22168a05c3e 101 double kp = 0.00008;
kyleliangus 43:f22168a05c3e 102 double kd = 0.0000;
sahilmgandhi 38:fe05f93009a2 103 int prev = 0;
sahilmgandhi 38:fe05f93009a2 104
kyleliangus 43:f22168a05c3e 105 double speed0 = WHEEL_SPEED; // left wheel
sahilmgandhi 38:fe05f93009a2 106 double speed1 = WHEEL_SPEED;
sahilmgandhi 40:465d2b565977 107 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 40:465d2b565977 108 receiverFourReading = IRP_4.getSamples(100);
sahilmgandhi 40:465d2b565977 109
sahilmgandhi 38:fe05f93009a2 110 right_motor.move(speed0);
sahilmgandhi 38:fe05f93009a2 111 left_motor.move(speed1);
christine222 44:85bf2c0cd518 112 wait_us(100000);
sahilmgandhi 38:fe05f93009a2 113
kyleliangus 43:f22168a05c3e 114 double distance_to_go = (oneCellCount)*num;
kyleliangus 42:75257e6c4c76 115
kyleliangus 42:75257e6c4c76 116 while( ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) {
sahilmgandhi 38:fe05f93009a2 117
sahilmgandhi 45:8b0bee6baf38 118 if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) {
kyleliangus 43:f22168a05c3e 119 left_motor.move( speed0/900 );
kyleliangus 43:f22168a05c3e 120 right_motor.move( speed1/900 );
sahilmgandhi 45:8b0bee6baf38 121 } else {
kyleliangus 43:f22168a05c3e 122 left_motor.move(speed0);
kyleliangus 43:f22168a05c3e 123 right_motor.move(speed1);
kyleliangus 43:f22168a05c3e 124 wait_us(16000);
kyleliangus 43:f22168a05c3e 125 pidOnEncoders();
kyleliangus 42:75257e6c4c76 126 }
sahilmgandhi 40:465d2b565977 127 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 40:465d2b565977 128 receiverFourReading = IRP_4.getSamples(100);
christine222 21:9a6cb07bdcb6 129 }
sahilmgandhi 31:9b71b44e0867 130
christine222 23:690b0ca34ee9 131 right_motor.brake();
christine222 23:690b0ca34ee9 132 left_motor.brake();
kyleliangus 43:f22168a05c3e 133
kyleliangus 43:f22168a05c3e 134 double curr0 = encoder0.getPulses();
kyleliangus 43:f22168a05c3e 135 double curr1 = encoder1.getPulses();
sahilmgandhi 45:8b0bee6baf38 136 if ((curr0 - initial0) >= distance_to_go*0.6 && (curr1 - initial1) >= distance_to_go*0.6) {
sahilmgandhi 40:465d2b565977 137 if (currDir % 4 == 0) {
sahilmgandhi 40:465d2b565977 138 mouseY += 1;
sahilmgandhi 40:465d2b565977 139 } else if (currDir % 4 == 1) {
sahilmgandhi 40:465d2b565977 140 mouseX += 1;
sahilmgandhi 40:465d2b565977 141 } else if (currDir % 4 == 2) {
sahilmgandhi 40:465d2b565977 142 mouseY -= 1;
sahilmgandhi 40:465d2b565977 143 } else if (currDir % 4 == 3) {
sahilmgandhi 40:465d2b565977 144 mouseX -= 1;
sahilmgandhi 40:465d2b565977 145 }
sahilmgandhi 40:465d2b565977 146
sahilmgandhi 40:465d2b565977 147 // the mouse has moved forward, we need to update the wall map now
sahilmgandhi 40:465d2b565977 148 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 40:465d2b565977 149 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 40:465d2b565977 150 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 40:465d2b565977 151 receiverFourReading = IRP_4.getSamples(100);
sahilmgandhi 40:465d2b565977 152
kyleliangus 43:f22168a05c3e 153 // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
kyleliangus 43:f22168a05c3e 154 // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
sahilmgandhi 40:465d2b565977 155
sahilmgandhi 40:465d2b565977 156 if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
kyleliangus 43:f22168a05c3e 157 // serial.printf("Front wall is there\n");
sahilmgandhi 40:465d2b565977 158 if (currDir % 4 == 0) {
sahilmgandhi 40:465d2b565977 159 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 40:465d2b565977 160 } else if (currDir % 4 == 1) {
sahilmgandhi 40:465d2b565977 161 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 40:465d2b565977 162 } else if (currDir % 4 == 2) {
sahilmgandhi 40:465d2b565977 163 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 40:465d2b565977 164 } else if (currDir % 4 == 3) {
sahilmgandhi 40:465d2b565977 165 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
sahilmgandhi 40:465d2b565977 166 }
sahilmgandhi 40:465d2b565977 167 }
sahilmgandhi 40:465d2b565977 168 if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
sahilmgandhi 40:465d2b565977 169 // do nothing, the walls are not there
sahilmgandhi 40:465d2b565977 170 } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there RED
kyleliangus 43:f22168a05c3e 171 // serial.printf("Left wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
sahilmgandhi 40:465d2b565977 172 if (currDir % 4 == 0) {
sahilmgandhi 40:465d2b565977 173 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
sahilmgandhi 40:465d2b565977 174 } else if (currDir % 4 == 1) {
sahilmgandhi 40:465d2b565977 175 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 40:465d2b565977 176 } else if (currDir % 4 == 2) {
sahilmgandhi 40:465d2b565977 177 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 40:465d2b565977 178 } else if (currDir % 4 == 3) {
sahilmgandhi 40:465d2b565977 179 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 40:465d2b565977 180 }
sahilmgandhi 40:465d2b565977 181 } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
kyleliangus 43:f22168a05c3e 182 // serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
sahilmgandhi 40:465d2b565977 183 if (currDir % 4 == 0) {
sahilmgandhi 40:465d2b565977 184 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 40:465d2b565977 185 } else if (currDir % 4 == 1) {
sahilmgandhi 40:465d2b565977 186 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 40:465d2b565977 187 } else if (currDir % 4 == 2) {
sahilmgandhi 40:465d2b565977 188 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
sahilmgandhi 40:465d2b565977 189 } else if (currDir % 4 == 3) {
sahilmgandhi 40:465d2b565977 190 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 40:465d2b565977 191 }
sahilmgandhi 40:465d2b565977 192 } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
kyleliangus 43:f22168a05c3e 193 // serial.printf("Both walls \n");
sahilmgandhi 45:8b0bee6baf38 194 if (currDir %4 == 0) {
sahilmgandhi 40:465d2b565977 195 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
sahilmgandhi 40:465d2b565977 196 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
sahilmgandhi 45:8b0bee6baf38 197 } else if (currDir %4 == 1) {
sahilmgandhi 40:465d2b565977 198 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
sahilmgandhi 40:465d2b565977 199 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
sahilmgandhi 45:8b0bee6baf38 200 } else if (currDir % 4 == 2) {
sahilmgandhi 40:465d2b565977 201 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
sahilmgandhi 40:465d2b565977 202 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
sahilmgandhi 45:8b0bee6baf38 203 } else if (currDir %4 == 3) {
sahilmgandhi 40:465d2b565977 204 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
sahilmgandhi 40:465d2b565977 205 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
sahilmgandhi 40:465d2b565977 206 }
sahilmgandhi 40:465d2b565977 207 }
kyleliangus 43:f22168a05c3e 208 }
christine222 23:690b0ca34ee9 209 }
sahilmgandhi 31:9b71b44e0867 210
sahilmgandhi 14:9e7bb03ddccb 211 void printDipFlag()
sahilmgandhi 14:9e7bb03ddccb 212 {
sahilmgandhi 45:8b0bee6baf38 213 if (DEBUGGING)
sahilmgandhi 40:465d2b565977 214 serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 215 }
sahilmgandhi 31:9b71b44e0867 216
sahilmgandhi 14:9e7bb03ddccb 217 void enableButton1()
sahilmgandhi 14:9e7bb03ddccb 218 {
vanshg 10:810d1849da9d 219 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 220 printDipFlag();
vanshg 10:810d1849da9d 221 }
sahilmgandhi 14:9e7bb03ddccb 222 void enableButton2()
sahilmgandhi 14:9e7bb03ddccb 223 {
vanshg 10:810d1849da9d 224 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 225 printDipFlag();
vanshg 10:810d1849da9d 226 }
sahilmgandhi 14:9e7bb03ddccb 227 void enableButton3()
sahilmgandhi 14:9e7bb03ddccb 228 {
vanshg 10:810d1849da9d 229 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 230 printDipFlag();
vanshg 10:810d1849da9d 231 }
sahilmgandhi 14:9e7bb03ddccb 232 void enableButton4()
sahilmgandhi 14:9e7bb03ddccb 233 {
vanshg 10:810d1849da9d 234 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 235 printDipFlag();
vanshg 10:810d1849da9d 236 }
sahilmgandhi 14:9e7bb03ddccb 237 void disableButton1()
sahilmgandhi 14:9e7bb03ddccb 238 {
vanshg 10:810d1849da9d 239 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 240 printDipFlag();
vanshg 10:810d1849da9d 241 }
sahilmgandhi 14:9e7bb03ddccb 242 void disableButton2()
sahilmgandhi 14:9e7bb03ddccb 243 {
vanshg 10:810d1849da9d 244 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 245 printDipFlag();
vanshg 10:810d1849da9d 246 }
sahilmgandhi 14:9e7bb03ddccb 247 void disableButton3()
sahilmgandhi 14:9e7bb03ddccb 248 {
vanshg 10:810d1849da9d 249 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 250 printDipFlag();
vanshg 10:810d1849da9d 251 }
sahilmgandhi 14:9e7bb03ddccb 252 void disableButton4()
sahilmgandhi 14:9e7bb03ddccb 253 {
vanshg 10:810d1849da9d 254 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 255 printDipFlag();
kyleliangus 9:1d8e4da058cd 256 }
sahilmgandhi 31:9b71b44e0867 257
kyleliangus 15:b80555a4a8b9 258 void pidOnEncoders()
kyleliangus 15:b80555a4a8b9 259 {
kyleliangus 15:b80555a4a8b9 260 int count0;
kyleliangus 15:b80555a4a8b9 261 int count1;
kyleliangus 15:b80555a4a8b9 262 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 263 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 264 int diff = count0 - count1;
sahilmgandhi 33:68ce1f74ab5f 265 double kp = 0.00016;
sahilmgandhi 28:8126a4d620e8 266 double kd = 0.00016;
kyleliangus 15:b80555a4a8b9 267 int prev = 0;
sahilmgandhi 31:9b71b44e0867 268
sahilmgandhi 16:d9252437bd92 269 int counter = 0;
sahilmgandhi 31:9b71b44e0867 270 while(1) {
kyleliangus 15:b80555a4a8b9 271 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 272 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 273 int x = count0 - count1;
kyleliangus 15:b80555a4a8b9 274 //double d = kp * x + kd * ( x - prev );
kyleliangus 15:b80555a4a8b9 275 double kppart = kp * x;
kyleliangus 15:b80555a4a8b9 276 double kdpart = kd * (x-prev);
kyleliangus 15:b80555a4a8b9 277 double d = kppart + kdpart;
sahilmgandhi 31:9b71b44e0867 278
kyleliangus 15:b80555a4a8b9 279 //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
sahilmgandhi 31:9b71b44e0867 280 if( x < diff - 60 ) { // count1 is bigger, right wheel pushed forward
kyleliangus 15:b80555a4a8b9 281 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 282 right_motor.move( d );
sahilmgandhi 31:9b71b44e0867 283 } else if( x > diff + 60 ) {
kyleliangus 15:b80555a4a8b9 284 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 285 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 286 }
sahilmgandhi 16:d9252437bd92 287 // else
sahilmgandhi 16:d9252437bd92 288 // {
sahilmgandhi 16:d9252437bd92 289 // left_motor.brake();
sahilmgandhi 31:9b71b44e0867 290 // right_motor.brake();
sahilmgandhi 16:d9252437bd92 291 // }
kyleliangus 15:b80555a4a8b9 292 prev = x;
sahilmgandhi 16:d9252437bd92 293 counter++;
sahilmgandhi 28:8126a4d620e8 294 if (counter == 10)
sahilmgandhi 16:d9252437bd92 295 break;
kyleliangus 15:b80555a4a8b9 296 }
kyleliangus 15:b80555a4a8b9 297 }
sahilmgandhi 31:9b71b44e0867 298
sahilmgandhi 31:9b71b44e0867 299 void nCellEncoderAndIR(double cellCount)
sahilmgandhi 31:9b71b44e0867 300 {
sahilmgandhi 19:7b66a518b6f8 301 double currentError = 0;
sahilmgandhi 19:7b66a518b6f8 302 double previousError = 0;
sahilmgandhi 19:7b66a518b6f8 303 double derivError = 0;
sahilmgandhi 19:7b66a518b6f8 304 double sumError = 0;
christine222 25:f827a8b7880e 305
sahilmgandhi 37:3dcc95e9321c 306 double HIGH_PWM_VOLTAGE_R = WHEEL_SPEED;
sahilmgandhi 37:3dcc95e9321c 307 double HIGH_PWM_VOLTAGE_L = WHEEL_SPEED;
sahilmgandhi 29:ec2c5a69acd6 308
sahilmgandhi 37:3dcc95e9321c 309 double rightSpeed = WHEEL_SPEED;
sahilmgandhi 37:3dcc95e9321c 310 double leftSpeed = WHEEL_SPEED;
christine222 25:f827a8b7880e 311
christine222 25:f827a8b7880e 312 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 313 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 314
sahilmgandhi 37:3dcc95e9321c 315 left_motor.forward(WHEEL_SPEED);
sahilmgandhi 37:3dcc95e9321c 316 right_motor.forward(WHEEL_SPEED);
christine222 25:f827a8b7880e 317
sahilmgandhi 19:7b66a518b6f8 318 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 319 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 320
christine222 23:690b0ca34ee9 321 int state = 0;
christine222 23:690b0ca34ee9 322
sahilmgandhi 31:9b71b44e0867 323 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
sahilmgandhi 33:68ce1f74ab5f 324 // serial.printf("The desiredCount0 is: %d \t The desiredCount1 is: %d\t the 0encoderval is :%d\t the 1encoderval is : %d\t\n", desiredCount0, desiredCount1, encoder0.getPulses(), encoder1.getPulses());
kyleliangus 32:69acb14778ea 325 receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 32:69acb14778ea 326 receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 32:69acb14778ea 327 receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
kyleliangus 32:69acb14778ea 328 receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
sahilmgandhi 33:68ce1f74ab5f 329 // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg);
vanshg 39:058fb32c24e0 330 if( receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) {
sahilmgandhi 26:d20f1adac2d3 331 break;
sahilmgandhi 26:d20f1adac2d3 332 }
christine222 25:f827a8b7880e 333
sahilmgandhi 33:68ce1f74ab5f 334 if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
christine222 25:f827a8b7880e 335 // both sides gone
sahilmgandhi 33:68ce1f74ab5f 336 double prev0 = encoder0.getPulses();
sahilmgandhi 33:68ce1f74ab5f 337 double prev1 = encoder1.getPulses();
sahilmgandhi 33:68ce1f74ab5f 338 double diff0 = desiredCount0 - prev0;
sahilmgandhi 33:68ce1f74ab5f 339 double diff1 = desiredCount1 - prev1;
sahilmgandhi 33:68ce1f74ab5f 340 double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
vanshg 39:058fb32c24e0 341 redLed.write(1);
vanshg 39:058fb32c24e0 342 greenLed.write(0);
vanshg 39:058fb32c24e0 343 blueLed.write(0);
sahilmgandhi 33:68ce1f74ab5f 344 // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
sahilmgandhi 26:d20f1adac2d3 345 moveForwardEncoder(valToPass);
sahilmgandhi 33:68ce1f74ab5f 346 continue;
vanshg 39:058fb32c24e0 347 } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED
christine222 25:f827a8b7880e 348 state = 1;
sahilmgandhi 40:465d2b565977 349 // double prev0 = encoder0.getPulses();
sahilmgandhi 40:465d2b565977 350 // double prev1 = encoder1.getPulses();
sahilmgandhi 40:465d2b565977 351 // double diff0 = desiredCount0 - prev0;
sahilmgandhi 40:465d2b565977 352 // double diff1 = desiredCount1 - prev1;
sahilmgandhi 40:465d2b565977 353 // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
christine222 25:f827a8b7880e 354 redLed.write(0);
christine222 25:f827a8b7880e 355 greenLed.write(1);
christine222 25:f827a8b7880e 356 blueLed.write(1);
sahilmgandhi 40:465d2b565977 357 // moveForwardEncoder(valToPass);
sahilmgandhi 40:465d2b565977 358 // break;
vanshg 39:058fb32c24e0 359 } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
christine222 25:f827a8b7880e 360 // BLUE BLUE BLUE BLUE
christine222 25:f827a8b7880e 361 state = 2;
sahilmgandhi 40:465d2b565977 362 // double prev0 = encoder0.getPulses();
sahilmgandhi 40:465d2b565977 363 // double prev1 = encoder1.getPulses();
sahilmgandhi 40:465d2b565977 364 // double diff0 = desiredCount0 - prev0;
sahilmgandhi 40:465d2b565977 365 // double diff1 = desiredCount1 - prev1;
sahilmgandhi 40:465d2b565977 366 // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
christine222 25:f827a8b7880e 367 redLed.write(1);
christine222 25:f827a8b7880e 368 greenLed.write(1);
christine222 25:f827a8b7880e 369 blueLed.write(0);
sahilmgandhi 40:465d2b565977 370 // moveForwardEncoder(valToPass);
sahilmgandhi 40:465d2b565977 371 // break;
sahilmgandhi 33:68ce1f74ab5f 372 } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
christine222 25:f827a8b7880e 373 // both walls there
christine222 25:f827a8b7880e 374 state = 0;
christine222 25:f827a8b7880e 375 redLed.write(1);
christine222 25:f827a8b7880e 376 greenLed.write(0);
christine222 25:f827a8b7880e 377 blueLed.write(1);
christine222 25:f827a8b7880e 378 }
christine222 25:f827a8b7880e 379
kyleliangus 32:69acb14778ea 380 //serial.printf("Entering switch\n");
sahilmgandhi 31:9b71b44e0867 381 switch(state) {
sahilmgandhi 31:9b71b44e0867 382 case(0): { // both walls there
sahilmgandhi 33:68ce1f74ab5f 383 currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg);
christine222 25:f827a8b7880e 384 break;
christine222 25:f827a8b7880e 385 }
sahilmgandhi 31:9b71b44e0867 386 case(1): { // RED RED RED RED RED
sahilmgandhi 33:68ce1f74ab5f 387 currentError = (receiverTwoReading - IRP_2.sensorAvg) - (IRP_3.sensorAvg*0.001);
sahilmgandhi 31:9b71b44e0867 388 break;
christine222 25:f827a8b7880e 389 }
sahilmgandhi 31:9b71b44e0867 390 case(2): { // blue
sahilmgandhi 33:68ce1f74ab5f 391 currentError = (IRP_2.sensorAvg*0.001) - (receiverThreeReading - IRP_3.sensorAvg);
christine222 25:f827a8b7880e 392 break;
christine222 25:f827a8b7880e 393 }
sahilmgandhi 31:9b71b44e0867 394 default: {
sahilmgandhi 33:68ce1f74ab5f 395 currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg);
christine222 25:f827a8b7880e 396 break;
christine222 25:f827a8b7880e 397 }
christine222 25:f827a8b7880e 398 }
kyleliangus 32:69acb14778ea 399 //serial.printf("Exiting switch");
christine222 25:f827a8b7880e 400
christine222 25:f827a8b7880e 401 sumError += currentError;
christine222 25:f827a8b7880e 402 derivError = currentError - previousError;
christine222 25:f827a8b7880e 403 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
vanshg 39:058fb32c24e0 404
vanshg 39:058fb32c24e0 405 double prev0 = encoder0.getPulses();
vanshg 39:058fb32c24e0 406 double prev1 = encoder1.getPulses();
vanshg 39:058fb32c24e0 407 double diff0 = desiredCount0 - prev0;
vanshg 39:058fb32c24e0 408 double diff1 = desiredCount1 - prev1;
vanshg 39:058fb32c24e0 409 double kp = .1/1000;
vanshg 39:058fb32c24e0 410
vanshg 39:058fb32c24e0 411
vanshg 39:058fb32c24e0 412 rightSpeed = HIGH_PWM_VOLTAGE_R;
vanshg 39:058fb32c24e0 413 leftSpeed = HIGH_PWM_VOLTAGE_L;
vanshg 39:058fb32c24e0 414 if (diff1 < 1000 || diff0 < 1000) {
vanshg 39:058fb32c24e0 415 rightSpeed = kp * HIGH_PWM_VOLTAGE_R;
vanshg 39:058fb32c24e0 416 leftSpeed = kp * HIGH_PWM_VOLTAGE_L;
vanshg 39:058fb32c24e0 417 }
vanshg 39:058fb32c24e0 418
christine222 25:f827a8b7880e 419 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
vanshg 39:058fb32c24e0 420 rightSpeed = rightSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_R);
vanshg 39:058fb32c24e0 421 leftSpeed = leftSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_L);
christine222 25:f827a8b7880e 422 } else { // r is faster than L. speed up l, slow down r
vanshg 39:058fb32c24e0 423 rightSpeed = rightSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
vanshg 39:058fb32c24e0 424 leftSpeed = leftSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
sahilmgandhi 31:9b71b44e0867 425 }
sahilmgandhi 34:69342782fb68 426
vanshg 39:058fb32c24e0 427
kyleliangus 32:69acb14778ea 428 //serial.printf("%f, %f\n", leftSpeed, rightSpeed);
christine222 25:f827a8b7880e 429 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 25:f827a8b7880e 430 if (leftSpeed < 0) leftSpeed = 0;
christine222 25:f827a8b7880e 431 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 25:f827a8b7880e 432 if (rightSpeed < 0) rightSpeed = 0;
sahilmgandhi 31:9b71b44e0867 433
christine222 25:f827a8b7880e 434 right_motor.forward(rightSpeed);
christine222 25:f827a8b7880e 435 left_motor.forward(leftSpeed);
sahilmgandhi 33:68ce1f74ab5f 436 pidOnEncoders();
sahilmgandhi 31:9b71b44e0867 437
christine222 25:f827a8b7880e 438 previousError = currentError;
sahilmgandhi 26:d20f1adac2d3 439 }
sahilmgandhi 45:8b0bee6baf38 440
sahilmgandhi 34:69342782fb68 441 // GO BACK A BIT BEFORE BRAKING??
sahilmgandhi 34:69342782fb68 442 left_motor.backward(0.01);
sahilmgandhi 34:69342782fb68 443 right_motor.backward(0.01);
sahilmgandhi 37:3dcc95e9321c 444 wait_us(150);
sahilmgandhi 34:69342782fb68 445 // DELETE THIS IF IT DOES NOT WORK!!
sahilmgandhi 45:8b0bee6baf38 446
sahilmgandhi 34:69342782fb68 447 left_motor.brake();
sahilmgandhi 34:69342782fb68 448 right_motor.brake();
christine222 44:85bf2c0cd518 449 if (encoder0.getPulses() >= 0.4*desiredCount0 && encoder1.getPulses() >= 0.4*desiredCount1) {
sahilmgandhi 31:9b71b44e0867 450 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 451 mouseY += 1;
sahilmgandhi 31:9b71b44e0867 452 } else if (currDir % 4 == 1) {
kyleliangus 32:69acb14778ea 453 mouseX += 1;
sahilmgandhi 31:9b71b44e0867 454 } else if (currDir % 4 == 2) {
sahilmgandhi 28:8126a4d620e8 455 mouseY -= 1;
sahilmgandhi 31:9b71b44e0867 456 } else if (currDir % 4 == 3) {
sahilmgandhi 28:8126a4d620e8 457 mouseX -= 1;
sahilmgandhi 28:8126a4d620e8 458 }
sahilmgandhi 31:9b71b44e0867 459
sahilmgandhi 31:9b71b44e0867 460 // the mouse has moved forward, we need to update the wall map now
sahilmgandhi 40:465d2b565977 461 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 40:465d2b565977 462 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 40:465d2b565977 463 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 40:465d2b565977 464 receiverFourReading = IRP_4.getSamples(100);
sahilmgandhi 31:9b71b44e0867 465
sahilmgandhi 41:56a34315dd75 466 // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
sahilmgandhi 41:56a34315dd75 467 // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
vanshg 39:058fb32c24e0 468
vanshg 39:058fb32c24e0 469 if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
sahilmgandhi 41:56a34315dd75 470 // serial.printf("Front wall is there\n");
sahilmgandhi 31:9b71b44e0867 471 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 472 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 473 } else if (currDir % 4 == 1) {
sahilmgandhi 31:9b71b44e0867 474 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 475 } else if (currDir % 4 == 2) {
vanshg 39:058fb32c24e0 476 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
vanshg 39:058fb32c24e0 477 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 478 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
vanshg 39:058fb32c24e0 479 }
vanshg 39:058fb32c24e0 480 }
vanshg 39:058fb32c24e0 481 if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
vanshg 39:058fb32c24e0 482 // do nothing, the walls are not there
vanshg 39:058fb32c24e0 483 } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there RED
sahilmgandhi 41:56a34315dd75 484 // serial.printf("Left wall\n");
vanshg 39:058fb32c24e0 485 if (currDir % 4 == 0) {
vanshg 39:058fb32c24e0 486 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
vanshg 39:058fb32c24e0 487 } else if (currDir % 4 == 1) {
vanshg 39:058fb32c24e0 488 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
vanshg 39:058fb32c24e0 489 } else if (currDir % 4 == 2) {
vanshg 39:058fb32c24e0 490 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 491 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 492 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 493 }
vanshg 39:058fb32c24e0 494 } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
sahilmgandhi 41:56a34315dd75 495 // serial.printf("Right wall\n");
sahilmgandhi 31:9b71b44e0867 496 if (currDir % 4 == 0) {
vanshg 39:058fb32c24e0 497 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 498 } else if (currDir % 4 == 1) {
vanshg 39:058fb32c24e0 499 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 500 } else if (currDir % 4 == 2) {
vanshg 39:058fb32c24e0 501 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
sahilmgandhi 31:9b71b44e0867 502 } else if (currDir % 4 == 3) {
vanshg 39:058fb32c24e0 503 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 504 }
vanshg 39:058fb32c24e0 505 } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
sahilmgandhi 41:56a34315dd75 506 // serial.printf("Both walls \n");
sahilmgandhi 45:8b0bee6baf38 507 if (currDir %4 == 0) {
vanshg 39:058fb32c24e0 508 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
vanshg 39:058fb32c24e0 509 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
sahilmgandhi 45:8b0bee6baf38 510 } else if (currDir %4 == 1) {
vanshg 39:058fb32c24e0 511 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
vanshg 39:058fb32c24e0 512 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
sahilmgandhi 45:8b0bee6baf38 513 } else if (currDir % 4 == 2) {
vanshg 39:058fb32c24e0 514 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
vanshg 39:058fb32c24e0 515 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
sahilmgandhi 45:8b0bee6baf38 516 } else if (currDir %4 == 3) {
vanshg 39:058fb32c24e0 517 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
vanshg 39:058fb32c24e0 518 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 519 }
sahilmgandhi 31:9b71b44e0867 520 }
sahilmgandhi 26:d20f1adac2d3 521 }
sahilmgandhi 17:f713758f6238 522 }
christine222 25:f827a8b7880e 523
sahilmgandhi 34:69342782fb68 524 bool isWallInFront(int x, int y)
sahilmgandhi 34:69342782fb68 525 {
sahilmgandhi 41:56a34315dd75 526 return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
sahilmgandhi 26:d20f1adac2d3 527 }
sahilmgandhi 34:69342782fb68 528 bool isWallInBack(int x, int y)
sahilmgandhi 34:69342782fb68 529 {
sahilmgandhi 41:56a34315dd75 530 return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
sahilmgandhi 26:d20f1adac2d3 531 }
sahilmgandhi 34:69342782fb68 532 bool isWallOnRight(int x, int y)
sahilmgandhi 34:69342782fb68 533 {
sahilmgandhi 41:56a34315dd75 534 return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
sahilmgandhi 26:d20f1adac2d3 535 }
sahilmgandhi 34:69342782fb68 536 bool isWallOnLeft(int x, int y)
sahilmgandhi 34:69342782fb68 537 {
sahilmgandhi 41:56a34315dd75 538 return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
sahilmgandhi 26:d20f1adac2d3 539 }
sahilmgandhi 26:d20f1adac2d3 540
sahilmgandhi 31:9b71b44e0867 541 int chooseNextMovement()
sahilmgandhi 31:9b71b44e0867 542 {
sahilmgandhi 26:d20f1adac2d3 543 int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
sahilmgandhi 31:9b71b44e0867 544 if (goingToCenter && (currentDistance == 0)) {
sahilmgandhi 26:d20f1adac2d3 545 goingToCenter = false;
sahilmgandhi 26:d20f1adac2d3 546 changeManhattanDistance(goingToCenter);
sahilmgandhi 31:9b71b44e0867 547 } else if (!goingToCenter && (currentDistance == 0)) {
kyleliangus 32:69acb14778ea 548 goingToCenter = true;
sahilmgandhi 26:d20f1adac2d3 549 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 550 }
sahilmgandhi 26:d20f1adac2d3 551
sahilmgandhi 26:d20f1adac2d3 552 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 553 int currX = 0;
sahilmgandhi 26:d20f1adac2d3 554 int currY = 0;
sahilmgandhi 26:d20f1adac2d3 555 int currDist = 0;
sahilmgandhi 26:d20f1adac2d3 556 int dirToGo = 0;
sahilmgandhi 31:9b71b44e0867 557 if (!justTurned) {
sahilmgandhi 26:d20f1adac2d3 558 cellsToVisit.push(make_pair(mouseX, mouseY));
sahilmgandhi 26:d20f1adac2d3 559 while (!cellsToVisit.empty()) {
sahilmgandhi 26:d20f1adac2d3 560 pair<int, int> curr = cellsToVisit.top();
sahilmgandhi 26:d20f1adac2d3 561 cellsToVisit.pop();
sahilmgandhi 26:d20f1adac2d3 562 currX = curr.first;
sahilmgandhi 26:d20f1adac2d3 563 currY = curr.second;
sahilmgandhi 26:d20f1adac2d3 564 currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
sahilmgandhi 26:d20f1adac2d3 565 int minDist = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 566
sahilmgandhi 31:9b71b44e0867 567 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 568 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 569 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 570 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 571 }
sahilmgandhi 26:d20f1adac2d3 572 }
sahilmgandhi 27:02ce1040f331 573 if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 574 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 575 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 576 }
sahilmgandhi 26:d20f1adac2d3 577 }
sahilmgandhi 27:02ce1040f331 578 if (currY + 1 < MAZE_LEN && !isWallInFront( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 579 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 580 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 581 }
sahilmgandhi 26:d20f1adac2d3 582 }
sahilmgandhi 27:02ce1040f331 583 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 584 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 585 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 586 }
sahilmgandhi 26:d20f1adac2d3 587 }
sahilmgandhi 26:d20f1adac2d3 588 } else {
sahilmgandhi 26:d20f1adac2d3 589 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 590 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 591 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 592 }
sahilmgandhi 26:d20f1adac2d3 593 }
sahilmgandhi 26:d20f1adac2d3 594 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 595 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 596 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 597 }
sahilmgandhi 26:d20f1adac2d3 598 }
sahilmgandhi 26:d20f1adac2d3 599 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 600 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 601 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 602 }
sahilmgandhi 26:d20f1adac2d3 603 }
sahilmgandhi 26:d20f1adac2d3 604 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 605 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 606 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 607 }
sahilmgandhi 26:d20f1adac2d3 608 }
sahilmgandhi 26:d20f1adac2d3 609 }
sahilmgandhi 26:d20f1adac2d3 610
sahilmgandhi 26:d20f1adac2d3 611 if (minDist == INT_MAX)
sahilmgandhi 26:d20f1adac2d3 612 continue;
sahilmgandhi 26:d20f1adac2d3 613 if (currDist > minDist)
sahilmgandhi 26:d20f1adac2d3 614 continue;
sahilmgandhi 26:d20f1adac2d3 615 if (currDist <= minDist) {
sahilmgandhi 26:d20f1adac2d3 616 manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
sahilmgandhi 26:d20f1adac2d3 617 }
sahilmgandhi 26:d20f1adac2d3 618 if (hasVisited(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 619 if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 620 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 621 }
sahilmgandhi 26:d20f1adac2d3 622 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 623 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 624 }
sahilmgandhi 26:d20f1adac2d3 625 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 626 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 627 }
sahilmgandhi 26:d20f1adac2d3 628 if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 629 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 630 }
sahilmgandhi 26:d20f1adac2d3 631 } else {
sahilmgandhi 26:d20f1adac2d3 632 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 633 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 634 }
sahilmgandhi 26:d20f1adac2d3 635 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 636 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 637 }
sahilmgandhi 26:d20f1adac2d3 638 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 639 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 640 }
sahilmgandhi 26:d20f1adac2d3 641 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 642 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 643 }
sahilmgandhi 26:d20f1adac2d3 644 }
sahilmgandhi 26:d20f1adac2d3 645 }
sahilmgandhi 26:d20f1adac2d3 646
sahilmgandhi 26:d20f1adac2d3 647 int minDistance = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 648 if (currDir % 4 == 0) {
sahilmgandhi 26:d20f1adac2d3 649 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 650 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 651 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 652 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 653 }
sahilmgandhi 26:d20f1adac2d3 654 }
sahilmgandhi 26:d20f1adac2d3 655 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 656 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 657 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 658 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 659 }
sahilmgandhi 26:d20f1adac2d3 660 }
sahilmgandhi 26:d20f1adac2d3 661 if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 662 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 663 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 664 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 665 }
sahilmgandhi 26:d20f1adac2d3 666 }
sahilmgandhi 26:d20f1adac2d3 667 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 668 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 669 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 670 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 671 }
sahilmgandhi 26:d20f1adac2d3 672 }
sahilmgandhi 26:d20f1adac2d3 673 } else if (currDir % 4 == 2) {
sahilmgandhi 41:56a34315dd75 674 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 675 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 676 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 677 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 678 }
sahilmgandhi 26:d20f1adac2d3 679 }
sahilmgandhi 41:56a34315dd75 680 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 681 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 682 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 683 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 684 }
sahilmgandhi 26:d20f1adac2d3 685 }
sahilmgandhi 41:56a34315dd75 686 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 687 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 688 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 689 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 690 }
sahilmgandhi 26:d20f1adac2d3 691 }
sahilmgandhi 41:56a34315dd75 692 if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 693 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 694 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 695 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 696 }
sahilmgandhi 26:d20f1adac2d3 697 }
sahilmgandhi 26:d20f1adac2d3 698 } else if (currDir % 4 == 1) {
sahilmgandhi 41:56a34315dd75 699 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 700 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 701 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 702 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 703 }
sahilmgandhi 26:d20f1adac2d3 704 }
sahilmgandhi 41:56a34315dd75 705 if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 706 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 707 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 41:56a34315dd75 708 // serial.printf("Looks like the left wall was not there\n");
sahilmgandhi 26:d20f1adac2d3 709 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 710 }
sahilmgandhi 26:d20f1adac2d3 711 }
sahilmgandhi 41:56a34315dd75 712 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 713 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 714 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 715 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 716 }
sahilmgandhi 26:d20f1adac2d3 717 }
sahilmgandhi 41:56a34315dd75 718 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 719 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 720 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 721 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 722 }
sahilmgandhi 26:d20f1adac2d3 723 }
sahilmgandhi 26:d20f1adac2d3 724 } else if (currDir % 4 == 3) {
sahilmgandhi 41:56a34315dd75 725 if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 726 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 727 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 728 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 729 }
sahilmgandhi 26:d20f1adac2d3 730 }
sahilmgandhi 41:56a34315dd75 731 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 732 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 733 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 734 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 735 }
sahilmgandhi 26:d20f1adac2d3 736 }
sahilmgandhi 41:56a34315dd75 737 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 738 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 739 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 740 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 741 }
sahilmgandhi 26:d20f1adac2d3 742 }
sahilmgandhi 41:56a34315dd75 743 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 744 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 745 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 746 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 747 }
sahilmgandhi 26:d20f1adac2d3 748 }
sahilmgandhi 26:d20f1adac2d3 749 }
sahilmgandhi 31:9b71b44e0867 750 } else {
sahilmgandhi 26:d20f1adac2d3 751 justTurned = false;
sahilmgandhi 26:d20f1adac2d3 752 dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 753 }
sahilmgandhi 26:d20f1adac2d3 754
sahilmgandhi 26:d20f1adac2d3 755 return dirToGo;
sahilmgandhi 26:d20f1adac2d3 756 }
sahilmgandhi 26:d20f1adac2d3 757
sahilmgandhi 34:69342782fb68 758 bool hasVisited(int x, int y)
sahilmgandhi 34:69342782fb68 759 {
sahilmgandhi 26:d20f1adac2d3 760 return visitedCells[MAZE_LEN - 1 - y][x];
sahilmgandhi 26:d20f1adac2d3 761 }
sahilmgandhi 26:d20f1adac2d3 762
sahilmgandhi 31:9b71b44e0867 763 void changeManhattanDistance(bool headCenter)
sahilmgandhi 31:9b71b44e0867 764 {
sahilmgandhi 31:9b71b44e0867 765 if (headCenter) {
sahilmgandhi 29:ec2c5a69acd6 766 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 767 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 768 distanceToCenter[i][j] = manhattanDistances[i][j];
sahilmgandhi 26:d20f1adac2d3 769 }
sahilmgandhi 26:d20f1adac2d3 770 }
sahilmgandhi 29:ec2c5a69acd6 771 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 772 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 773 manhattanDistances[i][j] = distanceToStart[i][j];
sahilmgandhi 26:d20f1adac2d3 774 }
sahilmgandhi 26:d20f1adac2d3 775 }
sahilmgandhi 31:9b71b44e0867 776 } else {
sahilmgandhi 31:9b71b44e0867 777 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 778 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 779 distanceToStart[i][j] = manhattanDistances[i][j];
sahilmgandhi 29:ec2c5a69acd6 780 }
sahilmgandhi 29:ec2c5a69acd6 781 }
sahilmgandhi 29:ec2c5a69acd6 782 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 783 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 784 manhattanDistances[i][j] = distanceToCenter[i][j];
sahilmgandhi 29:ec2c5a69acd6 785 }
sahilmgandhi 29:ec2c5a69acd6 786 }
sahilmgandhi 29:ec2c5a69acd6 787 }
sahilmgandhi 29:ec2c5a69acd6 788 }
sahilmgandhi 29:ec2c5a69acd6 789
sahilmgandhi 31:9b71b44e0867 790 void initializeDistances()
sahilmgandhi 31:9b71b44e0867 791 {
sahilmgandhi 29:ec2c5a69acd6 792 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 31:9b71b44e0867 793 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 31:9b71b44e0867 794 distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 795 }
sahilmgandhi 31:9b71b44e0867 796 }
sahilmgandhi 31:9b71b44e0867 797 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 31:9b71b44e0867 798 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 31:9b71b44e0867 799 distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 800 }
sahilmgandhi 31:9b71b44e0867 801 }
sahilmgandhi 31:9b71b44e0867 802 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 31:9b71b44e0867 803 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 31:9b71b44e0867 804 distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 805 }
sahilmgandhi 31:9b71b44e0867 806 }
sahilmgandhi 31:9b71b44e0867 807 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 31:9b71b44e0867 808 for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
sahilmgandhi 31:9b71b44e0867 809 distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 810 }
sahilmgandhi 31:9b71b44e0867 811 }
sahilmgandhi 26:d20f1adac2d3 812 }
sahilmgandhi 31:9b71b44e0867 813
kyleliangus 36:9c4cc9944b69 814 void waitButton4()
kyleliangus 36:9c4cc9944b69 815 {
kyleliangus 36:9c4cc9944b69 816 //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
kyleliangus 36:9c4cc9944b69 817 int init_val = dipFlags & BUTTON4_FLAG;
sahilmgandhi 45:8b0bee6baf38 818 while( (dipFlags & BUTTON4_FLAG) == init_val ) {
kyleliangus 36:9c4cc9944b69 819 // do nothing until ready
kyleliangus 36:9c4cc9944b69 820 }
kyleliangus 36:9c4cc9944b69 821 //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
vanshg 39:058fb32c24e0 822 wait( 3 );
kyleliangus 36:9c4cc9944b69 823 }
kyleliangus 36:9c4cc9944b69 824
sahilmgandhi 0:a03c771ab78e 825 int main()
sahilmgandhi 0:a03c771ab78e 826 {
christine222 3:880f15be8c72 827 //Set highest bandwidth.
christine222 23:690b0ca34ee9 828 //gyro.setLpBandwidth(LPFBW_42HZ);
sahilmgandhi 29:ec2c5a69acd6 829 initializeDistances();
christine222 3:880f15be8c72 830 serial.baud(9600);
christine222 23:690b0ca34ee9 831 //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 31:9b71b44e0867 832
sahilmgandhi 1:8a4b2f573923 833 wait (0.1);
sahilmgandhi 31:9b71b44e0867 834
sahilmgandhi 2:771db996cee0 835 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 836 greenLed.write(0);
sahilmgandhi 2:771db996cee0 837 blueLed.write(1);
sahilmgandhi 31:9b71b44e0867 838
kyleliangus 9:1d8e4da058cd 839 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 840 //right_motor.forward(0.1);
sahilmgandhi 31:9b71b44e0867 841
kyleliangus 8:a0760acdc59e 842 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 843 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 844 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 845 // PB_3 is B of left
vanshg 11:8fc2b703086b 846 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 847 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 31:9b71b44e0867 848
vanshg 10:810d1849da9d 849 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 850 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 851 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 852 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 853 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 854 */
sahilmgandhi 31:9b71b44e0867 855
kyleliangus 12:5790e56a056f 856 // 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 857 // of alternate function specified
kyleliangus 12:5790e56a056f 858 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 859 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 860 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 861 //
kyleliangus 12:5790e56a056f 862 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 863 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 864 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 865 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 866 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 867 // CR1 reenabales
kyleliangus 12:5790e56a056f 868 // then read CNT reg of timer
sahilmgandhi 31:9b71b44e0867 869
vanshg 10:810d1849da9d 870 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 871 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 872 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 873 dipButton4.rise(&enableButton4);
sahilmgandhi 31:9b71b44e0867 874
vanshg 10:810d1849da9d 875 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 876 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 877 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 878 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 879
kyleliangus 36:9c4cc9944b69 880 //waitButton4();
kyleliangus 35:a5bd9ef82210 881
sahilmgandhi 26:d20f1adac2d3 882 // init the wall, and mouse loc arrays:
sahilmgandhi 26:d20f1adac2d3 883 wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
sahilmgandhi 26:d20f1adac2d3 884 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 885
vanshg 39:058fb32c24e0 886 int prevEncoder0Count = 0;
vanshg 39:058fb32c24e0 887 int prevEncoder1Count = 0;
vanshg 39:058fb32c24e0 888 int currEncoder0Count = 0;
vanshg 39:058fb32c24e0 889 int currEncoder1Count = 0;
sahilmgandhi 28:8126a4d620e8 890
vanshg 39:058fb32c24e0 891 bool overrideFloodFill = false;
sahilmgandhi 37:3dcc95e9321c 892 right_motor.forward( WHEEL_SPEED );
sahilmgandhi 37:3dcc95e9321c 893 left_motor.forward( WHEEL_SPEED );
kyleliangus 36:9c4cc9944b69 894 //turn180();
sahilmgandhi 26:d20f1adac2d3 895 //wait_ms(1500);
vanshg 39:058fb32c24e0 896 int nextMovement = 0;
sahilmgandhi 45:8b0bee6baf38 897
sahilmgandhi 40:465d2b565977 898 // moveForwardEncoder(1);
sahilmgandhi 45:8b0bee6baf38 899
sahilmgandhi 26:d20f1adac2d3 900 while (1) {
vanshg 39:058fb32c24e0 901
sahilmgandhi 45:8b0bee6baf38 902 if (!overrideFloodFill) {
sahilmgandhi 45:8b0bee6baf38 903 nextMovement = chooseNextMovement();
sahilmgandhi 45:8b0bee6baf38 904 if (nextMovement == 0) {
sahilmgandhi 45:8b0bee6baf38 905 moveForwardEncoder(0.4);
sahilmgandhi 45:8b0bee6baf38 906 nCellEncoderAndIR(0.6);
sahilmgandhi 45:8b0bee6baf38 907 } else if (nextMovement == 1) {
sahilmgandhi 45:8b0bee6baf38 908 justTurned = true;
sahilmgandhi 45:8b0bee6baf38 909 turnRight();
sahilmgandhi 45:8b0bee6baf38 910 } else if (nextMovement == 2) {
sahilmgandhi 45:8b0bee6baf38 911 justTurned = true;
sahilmgandhi 45:8b0bee6baf38 912 turnLeft();
sahilmgandhi 45:8b0bee6baf38 913 } else if (nextMovement == 3) {
sahilmgandhi 45:8b0bee6baf38 914 nCellEncoderAndIR(1);
kyleliangus 43:f22168a05c3e 915 //encoderAfterTurn(1);
sahilmgandhi 45:8b0bee6baf38 916 } else if (nextMovement == 4) {
sahilmgandhi 45:8b0bee6baf38 917 justTurned = true;
sahilmgandhi 45:8b0bee6baf38 918 turn180();
christine222 44:85bf2c0cd518 919 }
sahilmgandhi 45:8b0bee6baf38 920 } else {
sahilmgandhi 45:8b0bee6baf38 921 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 45:8b0bee6baf38 922 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 45:8b0bee6baf38 923 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 45:8b0bee6baf38 924 receiverFourReading = IRP_4.getSamples(100);
sahilmgandhi 45:8b0bee6baf38 925
sahilmgandhi 45:8b0bee6baf38 926 if(receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) {
sahilmgandhi 45:8b0bee6baf38 927
sahilmgandhi 45:8b0bee6baf38 928 int randomNum = rand() %2;
sahilmgandhi 45:8b0bee6baf38 929 if(randomNum == 0) {
sahilmgandhi 45:8b0bee6baf38 930 turnRight();
sahilmgandhi 45:8b0bee6baf38 931 } else {
sahilmgandhi 45:8b0bee6baf38 932 turnLeft();
sahilmgandhi 45:8b0bee6baf38 933 }
sahilmgandhi 45:8b0bee6baf38 934 } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
sahilmgandhi 45:8b0bee6baf38 935 // both walls there
sahilmgandhi 45:8b0bee6baf38 936 nCellEncoderAndIR(1);
sahilmgandhi 45:8b0bee6baf38 937 redLed.write(1);
sahilmgandhi 45:8b0bee6baf38 938 greenLed.write(0);
sahilmgandhi 45:8b0bee6baf38 939 blueLed.write(1);
sahilmgandhi 45:8b0bee6baf38 940 } else if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
sahilmgandhi 45:8b0bee6baf38 941 // both sides gone
christine222 44:85bf2c0cd518 942
sahilmgandhi 45:8b0bee6baf38 943 redLed.write(1);
sahilmgandhi 45:8b0bee6baf38 944 greenLed.write(0);
sahilmgandhi 45:8b0bee6baf38 945 blueLed.write(0);
sahilmgandhi 45:8b0bee6baf38 946 // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
sahilmgandhi 45:8b0bee6baf38 947 moveForwardEncoder(1);
sahilmgandhi 45:8b0bee6baf38 948 continue;
sahilmgandhi 45:8b0bee6baf38 949 } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED
sahilmgandhi 45:8b0bee6baf38 950 turnRight();
sahilmgandhi 45:8b0bee6baf38 951 redLed.write(0);
sahilmgandhi 45:8b0bee6baf38 952 greenLed.write(1);
sahilmgandhi 45:8b0bee6baf38 953 blueLed.write(1);
sahilmgandhi 45:8b0bee6baf38 954 // moveForwardEncoder(valToPass);
sahilmgandhi 45:8b0bee6baf38 955 // break;
sahilmgandhi 45:8b0bee6baf38 956 } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
sahilmgandhi 45:8b0bee6baf38 957 turnLeft();
sahilmgandhi 45:8b0bee6baf38 958 redLed.write(1);
sahilmgandhi 45:8b0bee6baf38 959 greenLed.write(1);
sahilmgandhi 45:8b0bee6baf38 960 blueLed.write(0);
sahilmgandhi 45:8b0bee6baf38 961 // moveForwardEncoder(valToPass);
sahilmgandhi 45:8b0bee6baf38 962 // break;
sahilmgandhi 45:8b0bee6baf38 963 }
sahilmgandhi 45:8b0bee6baf38 964 }
christine222 24:e7063765d6f0 965
sahilmgandhi 45:8b0bee6baf38 966 wait_ms(500); // reduce this once we fine tune this!
christine222 44:85bf2c0cd518 967
sahilmgandhi 31:9b71b44e0867 968
sahilmgandhi 34:69342782fb68 969 //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
sahilmgandhi 45:8b0bee6baf38 970
sahilmgandhi 40:465d2b565977 971 // encoderAfterTurn(1);
vanshg 39:058fb32c24e0 972 // waitButton4();
sahilmgandhi 33:68ce1f74ab5f 973 // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() );
sahilmgandhi 33:68ce1f74ab5f 974 // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
sahilmgandhi 33:68ce1f74ab5f 975 // 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 );
kyleliangus 43:f22168a05c3e 976 //encoderAfterTurn(1);
kyleliangus 43:f22168a05c3e 977 //waitButton4();
christine222 3:880f15be8c72 978 }
sahilmgandhi 26:d20f1adac2d3 979 }