MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@37:3dcc95e9321c, 2017-05-27 (annotated)

Committer:
sahilmgandhi
Date:
Sat May 27 03:37:24 2017 +0000
Revision:
37:3dcc95e9321c
Parent:
36:9c4cc9944b69
Child:
38:fe05f93009a2
Need to get centered more before turns (so after moving forward). Also fix when both walls are missing!

Who changed what in which revision?

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