MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@42:75257e6c4c76, 2017-05-28 (annotated)

Committer:
kyleliangus
Date:
Sun May 28 06:50:22 2017 +0000
Revision:
42:75257e6c4c76
Parent:
41:56a34315dd75
Child:
43:f22168a05c3e
Deaccel on encoder move forward

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