MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@46:b156ef445742, 2017-06-03 (annotated)

Committer:
sahilmgandhi
Date:
Sat Jun 03 00:22:44 2017 +0000
Revision:
46:b156ef445742
Parent:
45:8b0bee6baf38 
Final code for internal battlebot competition.

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