MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@44:85bf2c0cd518, 2017-06-02 (annotated)

Committer:
christine222
Date:
Fri Jun 02 18:50:51 2017 +0000
Revision:
44:85bf2c0cd518
Parent:
43:f22168a05c3e
Child:
45:8b0bee6baf38
rngesus

Who changed what in which revision?

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