MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@35:a5bd9ef82210, 2017-05-27 (annotated)

Committer:
kyleliangus
Date:
Sat May 27 00:41:19 2017 +0000
Revision:
35:a5bd9ef82210
Parent:
34:69342782fb68
Child:
36:9c4cc9944b69
4th Dip switch down to move

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