MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@39:058fb32c24e0, 2017-05-28 (annotated)

Committer:
vanshg
Date:
Sun May 28 03:42:59 2017 +0000
Revision:
39:058fb32c24e0
Parent:
38:fe05f93009a2
Child:
40:465d2b565977
calibration n shit

Who changed what in which revision?

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