MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@40:465d2b565977, 2017-05-28 (annotated)

Committer:
sahilmgandhi
Date:
Sun May 28 06:22:34 2017 +0000
Revision:
40:465d2b565977
Parent:
39:058fb32c24e0
Child:
41:56a34315dd75
RIP Harambe

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