MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@32:69acb14778ea, 2017-05-26 (annotated)

Committer:
kyleliangus
Date:
Fri May 26 03:46:03 2017 +0000
Revision:
32:69acb14778ea
Parent:
31:9b71b44e0867
Child:
33:68ce1f74ab5f
PID values still wonky. IR PID vs Encoder PID also wonky

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 26:d20f1adac2d3 15 //IRSAvg= >: 0.143701, 0.128285
sahilmgandhi 31:9b71b44e0867 16
sahilmgandhi 31:9b71b44e0867 17 //facing wall ir2 and ir3
sahilmgandhi 31:9b71b44e0867 18 //0.144562, 0.113971 in maze
sahilmgandhi 31:9b71b44e0867 19
sahilmgandhi 31:9b71b44e0867 20 // normal hall ir2 and ir3
sahilmgandhi 31:9b71b44e0867 21 //0.013665, 0.010889 in maze
sahilmgandhi 31:9b71b44e0867 22
sahilmgandhi 31:9b71b44e0867 23 //0.014462, 0.009138
sahilmgandhi 31:9b71b44e0867 24 // 0.013888, 0.010530
sahilmgandhi 31:9b71b44e0867 25
sahilmgandhi 31:9b71b44e0867 26
sahilmgandhi 31:9b71b44e0867 27 //no walls ir2 and ir3
sahilmgandhi 31:9b71b44e0867 28 //0.003265, 0.002904 in maze9
sahilmgandhi 31:9b71b44e0867 29
sahilmgandhi 31:9b71b44e0867 30 //0.003162, 0.003123
sahilmgandhi 31:9b71b44e0867 31 //0.003795,
sahilmgandhi 31:9b71b44e0867 32
sahilmgandhi 31:9b71b44e0867 33 //0.005297, 0.007064
sahilmgandhi 29:ec2c5a69acd6 34
sahilmgandhi 16:d9252437bd92 35 void pidOnEncoders();
sahilmgandhi 29:ec2c5a69acd6 36
sahilmgandhi 31:9b71b44e0867 37 void moveForwardCellEncoder(double cellNum)
sahilmgandhi 31:9b71b44e0867 38 {
sahilmgandhi 17:f713758f6238 39 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
sahilmgandhi 17:f713758f6238 40 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
sahilmgandhi 31:9b71b44e0867 41
sahilmgandhi 20:82836745332e 42 left_motor.forward(0.125);
sahilmgandhi 20:82836745332e 43 right_motor.forward(0.095);
sahilmgandhi 19:7b66a518b6f8 44 wait_ms(1);
sahilmgandhi 31:9b71b44e0867 45 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
sahilmgandhi 20:82836745332e 46 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 20:82836745332e 47 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 20:82836745332e 48 // 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 49 if (receiverThreeReading < ir3base) {
sahilmgandhi 20:82836745332e 50 // redLed.write(1);
sahilmgandhi 20:82836745332e 51 // blueLed.write(0);
sahilmgandhi 19:7b66a518b6f8 52 turnFlag |= RIGHT_FLAG;
sahilmgandhi 31:9b71b44e0867 53 } else if (receiverTwoReading < ir2base) {
sahilmgandhi 20:82836745332e 54 // redLed.write(0);
sahilmgandhi 20:82836745332e 55 // blueLed.write(1);
sahilmgandhi 17:f713758f6238 56 turnFlag |= LEFT_FLAG;
sahilmgandhi 20:82836745332e 57 }
sahilmgandhi 16:d9252437bd92 58 pidOnEncoders();
sahilmgandhi 16:d9252437bd92 59 }
sahilmgandhi 31:9b71b44e0867 60
sahilmgandhi 16:d9252437bd92 61 left_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 62 right_motor.brake();
sahilmgandhi 14:9e7bb03ddccb 63 }
sahilmgandhi 31:9b71b44e0867 64
sahilmgandhi 31:9b71b44e0867 65
sahilmgandhi 31:9b71b44e0867 66 void moveForwardEncoder(double num)
sahilmgandhi 31:9b71b44e0867 67 {
sahilmgandhi 31:9b71b44e0867 68
christine222 21:9a6cb07bdcb6 69 int count0;
christine222 21:9a6cb07bdcb6 70 int count1;
christine222 21:9a6cb07bdcb6 71 count0 = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 72 count1 = encoder1.getPulses();
christine222 21:9a6cb07bdcb6 73 int initial1 = count1;
christine222 21:9a6cb07bdcb6 74 int initial0 = count0;
christine222 21:9a6cb07bdcb6 75 int diff = count0 - count1;
christine222 21:9a6cb07bdcb6 76 double kp = 0.00015;
christine222 21:9a6cb07bdcb6 77 double kd = 0.00019;
christine222 21:9a6cb07bdcb6 78 int prev = 0;
christine222 25:f827a8b7880e 79
christine222 23:690b0ca34ee9 80 double speed0 = 0.10;
christine222 23:690b0ca34ee9 81 double speed1 = 0.12;
christine222 21:9a6cb07bdcb6 82 right_motor.move(speed0);
christine222 21:9a6cb07bdcb6 83 left_motor.move(speed1);
sahilmgandhi 31:9b71b44e0867 84
sahilmgandhi 31:9b71b44e0867 85 while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8) {
sahilmgandhi 31:9b71b44e0867 86 //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){
christine222 21:9a6cb07bdcb6 87 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
sahilmgandhi 31:9b71b44e0867 88
christine222 21:9a6cb07bdcb6 89 count0 = encoder0.getPulses() - initial0;
christine222 21:9a6cb07bdcb6 90 count1 = encoder1.getPulses() - initial1;
christine222 21:9a6cb07bdcb6 91 int x = count0 - count1;
christine222 21:9a6cb07bdcb6 92 //double d = kp * x + kd * ( x - prev );
christine222 21:9a6cb07bdcb6 93 double kppart = kp * x;
christine222 21:9a6cb07bdcb6 94 double kdpart = kd * (x-prev);
christine222 21:9a6cb07bdcb6 95 double d = kppart + kdpart;
sahilmgandhi 31:9b71b44e0867 96
christine222 21:9a6cb07bdcb6 97 //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 98 if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
christine222 21:9a6cb07bdcb6 99 left_motor.move( speed1-0.8*d );
christine222 21:9a6cb07bdcb6 100 right_motor.move( speed0+d );
sahilmgandhi 31:9b71b44e0867 101 } else if( x > diff + 40 ) {
christine222 21:9a6cb07bdcb6 102 left_motor.move( speed1-0.8*d );
christine222 21:9a6cb07bdcb6 103 right_motor.move( speed0+d );
christine222 21:9a6cb07bdcb6 104 }
christine222 21:9a6cb07bdcb6 105 // else
christine222 21:9a6cb07bdcb6 106 // {
christine222 21:9a6cb07bdcb6 107 // left_motor.brake();
sahilmgandhi 31:9b71b44e0867 108 // right_motor.brake();
christine222 21:9a6cb07bdcb6 109 // }
sahilmgandhi 31:9b71b44e0867 110 prev = x;
christine222 21:9a6cb07bdcb6 111 }
sahilmgandhi 31:9b71b44e0867 112
christine222 21:9a6cb07bdcb6 113 //pidOnEncoders();
christine222 23:690b0ca34ee9 114 //pidBrake();
christine222 23:690b0ca34ee9 115 right_motor.brake();
christine222 23:690b0ca34ee9 116 left_motor.brake();
christine222 23:690b0ca34ee9 117 return;
christine222 23:690b0ca34ee9 118 }
sahilmgandhi 31:9b71b44e0867 119
sahilmgandhi 31:9b71b44e0867 120
sahilmgandhi 31:9b71b44e0867 121 void moveForwardWallEncoder()
sahilmgandhi 31:9b71b44e0867 122 {
sahilmgandhi 31:9b71b44e0867 123 int count0;
christine222 23:690b0ca34ee9 124 int count1;
christine222 23:690b0ca34ee9 125 count0 = encoder0.getPulses();
christine222 23:690b0ca34ee9 126 count1 = encoder1.getPulses();
christine222 23:690b0ca34ee9 127 int initial1 = count1;
christine222 23:690b0ca34ee9 128 int initial0 = count0;
christine222 23:690b0ca34ee9 129 int diff = count0 - count1;
christine222 23:690b0ca34ee9 130 double kp = 0.00015;
christine222 23:690b0ca34ee9 131 double kd = 0.00019;
christine222 23:690b0ca34ee9 132 int prev = 0;
christine222 25:f827a8b7880e 133
christine222 25:f827a8b7880e 134 double speed0 = 0.11;
christine222 25:f827a8b7880e 135 double speed1 = 0.13;
christine222 23:690b0ca34ee9 136 right_motor.move(speed0);
christine222 23:690b0ca34ee9 137 left_motor.move(speed1);
christine222 23:690b0ca34ee9 138
kyleliangus 32:69acb14778ea 139 double ir1 = IRP_1.getSamples(50);
kyleliangus 32:69acb14778ea 140 double ir4 = IRP_4.getSamples(50);
christine222 25:f827a8b7880e 141
sahilmgandhi 31:9b71b44e0867 142 if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4) {
christine222 24:e7063765d6f0 143 return;
christine222 24:e7063765d6f0 144 }
christine222 25:f827a8b7880e 145
christine222 23:690b0ca34ee9 146 //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
christine222 25:f827a8b7880e 147 //while( (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4 ){
sahilmgandhi 31:9b71b44e0867 148 while( ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
christine222 23:690b0ca34ee9 149 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
sahilmgandhi 31:9b71b44e0867 150
christine222 23:690b0ca34ee9 151 count0 = encoder0.getPulses() - initial0;
christine222 23:690b0ca34ee9 152 count1 = encoder1.getPulses() - initial1;
christine222 23:690b0ca34ee9 153 int x = count0 - count1;
christine222 23:690b0ca34ee9 154 //double d = kp * x + kd * ( x - prev );
christine222 23:690b0ca34ee9 155 double kppart = kp * x;
christine222 23:690b0ca34ee9 156 double kdpart = kd * (x-prev);
christine222 23:690b0ca34ee9 157 double d = kppart + kdpart;
sahilmgandhi 31:9b71b44e0867 158
christine222 23:690b0ca34ee9 159 //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 160 if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
christine222 23:690b0ca34ee9 161 left_motor.move( speed1-0.8*d );
christine222 23:690b0ca34ee9 162 right_motor.move( speed0+d );
sahilmgandhi 31:9b71b44e0867 163 } else if( x > diff + 40 ) {
christine222 23:690b0ca34ee9 164 left_motor.move( speed1-0.8*d );
christine222 23:690b0ca34ee9 165 right_motor.move( speed0+d );
christine222 23:690b0ca34ee9 166 }
christine222 23:690b0ca34ee9 167 // else
christine222 23:690b0ca34ee9 168 // {
christine222 23:690b0ca34ee9 169 // left_motor.brake();
sahilmgandhi 31:9b71b44e0867 170 // right_motor.brake();
christine222 23:690b0ca34ee9 171 // }
sahilmgandhi 31:9b71b44e0867 172 prev = x;
christine222 25:f827a8b7880e 173 ir1 = IRP_1.getSamples(50);
christine222 25:f827a8b7880e 174 ir4 = IRP_4.getSamples(50);
christine222 23:690b0ca34ee9 175 }
christine222 25:f827a8b7880e 176
christine222 23:690b0ca34ee9 177 //pidOnEncoders();
christine222 23:690b0ca34ee9 178 //pidBrake();
christine222 23:690b0ca34ee9 179 right_motor.brake();
christine222 23:690b0ca34ee9 180 left_motor.brake();
christine222 21:9a6cb07bdcb6 181 return;
christine222 21:9a6cb07bdcb6 182 }
sahilmgandhi 31:9b71b44e0867 183
sahilmgandhi 14:9e7bb03ddccb 184 void moveForwardUntilWallIr()
sahilmgandhi 14:9e7bb03ddccb 185 {
kyleliangus 9:1d8e4da058cd 186 double currentError = 0;
kyleliangus 9:1d8e4da058cd 187 double previousError = 0;
kyleliangus 9:1d8e4da058cd 188 double derivError = 0;
kyleliangus 9:1d8e4da058cd 189 double sumError = 0;
christine222 25:f827a8b7880e 190
vanshg 11:8fc2b703086b 191 double HIGH_PWM_VOLTAGE = 0.1;
christine222 25:f827a8b7880e 192
christine222 25:f827a8b7880e 193 double rightSpeed = 0.25;
christine222 25:f827a8b7880e 194 double leftSpeed = 0.23;
christine222 25:f827a8b7880e 195
kyleliangus 9:1d8e4da058cd 196 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 9:1d8e4da058cd 197 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 198
vanshg 11:8fc2b703086b 199 int count = encoder0.getPulses();
christine222 21:9a6cb07bdcb6 200 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 201
christine222 21:9a6cb07bdcb6 202 if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
sahilmgandhi 31:9b71b44e0867 203 //moveForwardWallEncoder();
sahilmgandhi 31:9b71b44e0867 204 } else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005) { // left wall gone
christine222 22:681190ff98f0 205 //moveForwardRightWall();
sahilmgandhi 31:9b71b44e0867 206 } else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005) { // right wall gone
christine222 22:681190ff98f0 207 //moveForwardLeftWall();
sahilmgandhi 31:9b71b44e0867 208 } else {
sahilmgandhi 31:9b71b44e0867 209 // will move forward using encoders only
sahilmgandhi 31:9b71b44e0867 210 // if cell ahead doesn't have a wall on either one side or both sides
christine222 25:f827a8b7880e 211
christine222 21:9a6cb07bdcb6 212 int pulseCount = (encoder0.getPulses()-count) % 5600;
christine222 21:9a6cb07bdcb6 213 if (pulseCount > 5400 && pulseCount < 5800) {
christine222 21:9a6cb07bdcb6 214 blueLed.write(0);
christine222 21:9a6cb07bdcb6 215 } else {
christine222 21:9a6cb07bdcb6 216 blueLed.write(1);
christine222 21:9a6cb07bdcb6 217 }
christine222 21:9a6cb07bdcb6 218 sumError += currentError;
christine222 25:f827a8b7880e 219 currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg/initAverageL) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg/initAverageR) ) ;
christine222 21:9a6cb07bdcb6 220 derivError = currentError - previousError;
christine222 21:9a6cb07bdcb6 221 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 21:9a6cb07bdcb6 222 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
christine222 21:9a6cb07bdcb6 223 rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 224 leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 225 } else { // r is faster than L. speed up l, slow down r
christine222 21:9a6cb07bdcb6 226 rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 227 leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
christine222 21:9a6cb07bdcb6 228 }
christine222 25:f827a8b7880e 229
christine222 21:9a6cb07bdcb6 230 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 21:9a6cb07bdcb6 231 if (leftSpeed < 0) leftSpeed = 0;
christine222 21:9a6cb07bdcb6 232 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 21:9a6cb07bdcb6 233 if (rightSpeed < 0) rightSpeed = 0;
christine222 25:f827a8b7880e 234
christine222 21:9a6cb07bdcb6 235 right_motor.forward(rightSpeed);
christine222 21:9a6cb07bdcb6 236 left_motor.forward(leftSpeed);
christine222 25:f827a8b7880e 237
christine222 21:9a6cb07bdcb6 238 previousError = currentError;
christine222 25:f827a8b7880e 239
christine222 21:9a6cb07bdcb6 240 ir2 = IRP_2.getSamples( SAMPLE_NUM );
christine222 21:9a6cb07bdcb6 241 ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 242
kyleliangus 9:1d8e4da058cd 243 }
christine222 25:f827a8b7880e 244
christine222 21:9a6cb07bdcb6 245 //backward();
christine222 22:681190ff98f0 246 //wait_ms(40);
christine222 21:9a6cb07bdcb6 247 //brake();
christine222 25:f827a8b7880e 248
christine222 21:9a6cb07bdcb6 249 left_motor.brake();
christine222 21:9a6cb07bdcb6 250 right_motor.brake();
kyleliangus 9:1d8e4da058cd 251 }
vanshg 10:810d1849da9d 252 }
sahilmgandhi 31:9b71b44e0867 253
sahilmgandhi 14:9e7bb03ddccb 254 void printDipFlag()
sahilmgandhi 14:9e7bb03ddccb 255 {
vanshg 11:8fc2b703086b 256 if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
vanshg 11:8fc2b703086b 257 }
sahilmgandhi 31:9b71b44e0867 258
sahilmgandhi 14:9e7bb03ddccb 259 void enableButton1()
sahilmgandhi 14:9e7bb03ddccb 260 {
vanshg 10:810d1849da9d 261 dipFlags |= BUTTON1_FLAG;
vanshg 11:8fc2b703086b 262 printDipFlag();
vanshg 10:810d1849da9d 263 }
sahilmgandhi 14:9e7bb03ddccb 264 void enableButton2()
sahilmgandhi 14:9e7bb03ddccb 265 {
vanshg 10:810d1849da9d 266 dipFlags |= BUTTON2_FLAG;
vanshg 11:8fc2b703086b 267 printDipFlag();
vanshg 10:810d1849da9d 268 }
sahilmgandhi 14:9e7bb03ddccb 269 void enableButton3()
sahilmgandhi 14:9e7bb03ddccb 270 {
vanshg 10:810d1849da9d 271 dipFlags |= BUTTON3_FLAG;
vanshg 11:8fc2b703086b 272 printDipFlag();
vanshg 10:810d1849da9d 273 }
sahilmgandhi 14:9e7bb03ddccb 274 void enableButton4()
sahilmgandhi 14:9e7bb03ddccb 275 {
vanshg 10:810d1849da9d 276 dipFlags |= BUTTON4_FLAG;
vanshg 11:8fc2b703086b 277 printDipFlag();
vanshg 10:810d1849da9d 278 }
sahilmgandhi 14:9e7bb03ddccb 279 void disableButton1()
sahilmgandhi 14:9e7bb03ddccb 280 {
vanshg 10:810d1849da9d 281 dipFlags &= ~BUTTON1_FLAG;
vanshg 11:8fc2b703086b 282 printDipFlag();
vanshg 10:810d1849da9d 283 }
sahilmgandhi 14:9e7bb03ddccb 284 void disableButton2()
sahilmgandhi 14:9e7bb03ddccb 285 {
vanshg 10:810d1849da9d 286 dipFlags &= ~BUTTON2_FLAG;
vanshg 11:8fc2b703086b 287 printDipFlag();
vanshg 10:810d1849da9d 288 }
sahilmgandhi 14:9e7bb03ddccb 289 void disableButton3()
sahilmgandhi 14:9e7bb03ddccb 290 {
vanshg 10:810d1849da9d 291 dipFlags &= ~BUTTON3_FLAG;
vanshg 11:8fc2b703086b 292 printDipFlag();
vanshg 10:810d1849da9d 293 }
sahilmgandhi 14:9e7bb03ddccb 294 void disableButton4()
sahilmgandhi 14:9e7bb03ddccb 295 {
vanshg 10:810d1849da9d 296 dipFlags &= ~BUTTON4_FLAG;
vanshg 11:8fc2b703086b 297 printDipFlag();
kyleliangus 9:1d8e4da058cd 298 }
sahilmgandhi 31:9b71b44e0867 299
kyleliangus 15:b80555a4a8b9 300 void pidOnEncoders()
kyleliangus 15:b80555a4a8b9 301 {
kyleliangus 15:b80555a4a8b9 302 int count0;
kyleliangus 15:b80555a4a8b9 303 int count1;
kyleliangus 15:b80555a4a8b9 304 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 305 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 306 int diff = count0 - count1;
sahilmgandhi 28:8126a4d620e8 307 double kp = 0.00013;
sahilmgandhi 28:8126a4d620e8 308 double kd = 0.00016;
kyleliangus 15:b80555a4a8b9 309 int prev = 0;
sahilmgandhi 31:9b71b44e0867 310
sahilmgandhi 16:d9252437bd92 311 int counter = 0;
sahilmgandhi 31:9b71b44e0867 312 while(1) {
kyleliangus 15:b80555a4a8b9 313 count0 = encoder0.getPulses();
kyleliangus 15:b80555a4a8b9 314 count1 = encoder1.getPulses();
kyleliangus 15:b80555a4a8b9 315 int x = count0 - count1;
kyleliangus 15:b80555a4a8b9 316 //double d = kp * x + kd * ( x - prev );
kyleliangus 15:b80555a4a8b9 317 double kppart = kp * x;
kyleliangus 15:b80555a4a8b9 318 double kdpart = kd * (x-prev);
kyleliangus 15:b80555a4a8b9 319 double d = kppart + kdpart;
sahilmgandhi 31:9b71b44e0867 320
kyleliangus 15:b80555a4a8b9 321 //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 322 if( x < diff - 60 ) { // count1 is bigger, right wheel pushed forward
kyleliangus 15:b80555a4a8b9 323 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 324 right_motor.move( d );
sahilmgandhi 31:9b71b44e0867 325 } else if( x > diff + 60 ) {
kyleliangus 15:b80555a4a8b9 326 left_motor.move( -d );
kyleliangus 15:b80555a4a8b9 327 right_motor.move( d );
kyleliangus 15:b80555a4a8b9 328 }
sahilmgandhi 16:d9252437bd92 329 // else
sahilmgandhi 16:d9252437bd92 330 // {
sahilmgandhi 16:d9252437bd92 331 // left_motor.brake();
sahilmgandhi 31:9b71b44e0867 332 // right_motor.brake();
sahilmgandhi 16:d9252437bd92 333 // }
kyleliangus 15:b80555a4a8b9 334 prev = x;
sahilmgandhi 16:d9252437bd92 335 counter++;
sahilmgandhi 28:8126a4d620e8 336 if (counter == 10)
sahilmgandhi 16:d9252437bd92 337 break;
kyleliangus 15:b80555a4a8b9 338 }
kyleliangus 15:b80555a4a8b9 339 }
sahilmgandhi 31:9b71b44e0867 340
sahilmgandhi 31:9b71b44e0867 341 void nCellEncoderAndIR(double cellCount)
sahilmgandhi 31:9b71b44e0867 342 {
kyleliangus 32:69acb14778ea 343 //serial.printf("I'm inside IR!");
sahilmgandhi 19:7b66a518b6f8 344 double currentError = 0;
sahilmgandhi 19:7b66a518b6f8 345 double previousError = 0;
sahilmgandhi 19:7b66a518b6f8 346 double derivError = 0;
sahilmgandhi 19:7b66a518b6f8 347 double sumError = 0;
christine222 25:f827a8b7880e 348
sahilmgandhi 29:ec2c5a69acd6 349 double HIGH_PWM_VOLTAGE_R = 0.15;
sahilmgandhi 29:ec2c5a69acd6 350 double HIGH_PWM_VOLTAGE_L = 0.16;
sahilmgandhi 29:ec2c5a69acd6 351
sahilmgandhi 29:ec2c5a69acd6 352 double rightSpeed = 0.15;
sahilmgandhi 29:ec2c5a69acd6 353 double leftSpeed = 0.16;
christine222 25:f827a8b7880e 354
christine222 25:f827a8b7880e 355 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 356 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
kyleliangus 32:69acb14778ea 357 serial.printf("%d, %d\n", desiredCount0, desiredCount1 );
christine222 25:f827a8b7880e 358
kyleliangus 32:69acb14778ea 359 left_motor.forward(0.15);
sahilmgandhi 29:ec2c5a69acd6 360 right_motor.forward(0.15);
christine222 25:f827a8b7880e 361
sahilmgandhi 19:7b66a518b6f8 362 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 363 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 364
christine222 25:f827a8b7880e 365 // float previr2 = ir2;
christine222 25:f827a8b7880e 366 // float previr3 = ir3;
christine222 25:f827a8b7880e 367
christine222 23:690b0ca34ee9 368 int state = 0;
christine222 23:690b0ca34ee9 369
kyleliangus 32:69acb14778ea 370 //serial.printf("Enter encoder while loop\n");
kyleliangus 32:69acb14778ea 371 //serial.printf("%d, %d\n", encoder0.getPulses(), encoder1.getPulses() );
sahilmgandhi 31:9b71b44e0867 372 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
kyleliangus 32:69acb14778ea 373 //serial.printf("Entered encoder while loop\n");
kyleliangus 32:69acb14778ea 374 receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
kyleliangus 32:69acb14778ea 375 receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
kyleliangus 32:69acb14778ea 376 receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
kyleliangus 32:69acb14778ea 377 receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 378
kyleliangus 32:69acb14778ea 379 if( receiverOneReading > IRP_1.sensorAvg * 3 || receiverFourReading > IRP_4.sensorAvg * 3 ) {
sahilmgandhi 26:d20f1adac2d3 380 break;
sahilmgandhi 26:d20f1adac2d3 381 }
christine222 25:f827a8b7880e 382
sahilmgandhi 31:9b71b44e0867 383 if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))) {
christine222 25:f827a8b7880e 384 // both sides gone
sahilmgandhi 26:d20f1adac2d3 385 int prev0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 386 int prev1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 387 int diff0 = desiredCount0 - prev0;
sahilmgandhi 26:d20f1adac2d3 388 int diff1 = desiredCount1 - prev1;
sahilmgandhi 26:d20f1adac2d3 389 int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
sahilmgandhi 26:d20f1adac2d3 390 moveForwardEncoder(valToPass);
sahilmgandhi 31:9b71b44e0867 391 } else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR) { // right wall gone
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 31:9b71b44e0867 396 } else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL) { // 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 31:9b71b44e0867 402 } else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*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
christine222 25:f827a8b7880e 413 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 414 break;
christine222 25:f827a8b7880e 415 }
sahilmgandhi 31:9b71b44e0867 416 case(1): { // RED RED RED RED RED
christine222 25:f827a8b7880e 417 currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
sahilmgandhi 31:9b71b44e0867 418 break;
christine222 25:f827a8b7880e 419 }
sahilmgandhi 31:9b71b44e0867 420 case(2): { // blue
christine222 25:f827a8b7880e 421 currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 422 break;
christine222 25:f827a8b7880e 423 }
sahilmgandhi 31:9b71b44e0867 424 default: {
christine222 25:f827a8b7880e 425 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
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 }
kyleliangus 32:69acb14778ea 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);
kyleliangus 32:69acb14778ea 450 //pidOnEncoders();
sahilmgandhi 31:9b71b44e0867 451
christine222 25:f827a8b7880e 452 previousError = currentError;
sahilmgandhi 26:d20f1adac2d3 453 }
sahilmgandhi 31:9b71b44e0867 454 if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1) {
sahilmgandhi 31:9b71b44e0867 455 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 456 mouseY += 1;
sahilmgandhi 31:9b71b44e0867 457 } else if (currDir % 4 == 1) {
kyleliangus 32:69acb14778ea 458 mouseX += 1;
sahilmgandhi 31:9b71b44e0867 459 } else if (currDir % 4 == 2) {
sahilmgandhi 28:8126a4d620e8 460 mouseY -= 1;
sahilmgandhi 31:9b71b44e0867 461 } else if (currDir % 4 == 3) {
sahilmgandhi 28:8126a4d620e8 462 mouseX -= 1;
sahilmgandhi 28:8126a4d620e8 463 }
sahilmgandhi 31:9b71b44e0867 464
sahilmgandhi 31:9b71b44e0867 465 // the mouse has moved forward, we need to update the wall map now
sahilmgandhi 31:9b71b44e0867 466 receiverOneReading = IRP_1.getSamples(50);
sahilmgandhi 31:9b71b44e0867 467 receiverTwoReading = IRP_2.getSamples(50);
sahilmgandhi 31:9b71b44e0867 468 receiverThreeReading = IRP_3.getSamples(50);
sahilmgandhi 31:9b71b44e0867 469 receiverFourReading = IRP_4.getSamples(50);
sahilmgandhi 31:9b71b44e0867 470
sahilmgandhi 31:9b71b44e0867 471 if (receiverOneReading >= 0.5f && receiverFourReading >= 0.5f) {
sahilmgandhi 31:9b71b44e0867 472 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 473 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 474 } else if (currDir % 4 == 1) {
sahilmgandhi 31:9b71b44e0867 475 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 476 } else if (currDir % 4 == 2) {
sahilmgandhi 31:9b71b44e0867 477 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
sahilmgandhi 31:9b71b44e0867 478 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 479 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 480 }
sahilmgandhi 31:9b71b44e0867 481 }
sahilmgandhi 31:9b71b44e0867 482 if (receiverThreeReading >= 0.1f) {
sahilmgandhi 31:9b71b44e0867 483 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 484 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 485 } else if (currDir % 4 == 1) {
sahilmgandhi 31:9b71b44e0867 486 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 487 } else if (currDir % 4 == 2) {
sahilmgandhi 31:9b71b44e0867 488 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 31:9b71b44e0867 489 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 490 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 491 }
sahilmgandhi 31:9b71b44e0867 492 }
sahilmgandhi 31:9b71b44e0867 493 if (receiverTwoReading >= 0.1f) {
sahilmgandhi 31:9b71b44e0867 494 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 495 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 31:9b71b44e0867 496 } else if (currDir % 4 == 1) {
sahilmgandhi 31:9b71b44e0867 497 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 498 } else if (currDir % 4 == 2) {
sahilmgandhi 31:9b71b44e0867 499 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 500 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 501 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 502 }
sahilmgandhi 31:9b71b44e0867 503 }
sahilmgandhi 26:d20f1adac2d3 504 }
sahilmgandhi 31:9b71b44e0867 505
kyleliangus 32:69acb14778ea 506 //serial.printf("What?\n");
sahilmgandhi 19:7b66a518b6f8 507 left_motor.brake();
sahilmgandhi 19:7b66a518b6f8 508 right_motor.brake();
sahilmgandhi 17:f713758f6238 509 }
christine222 25:f827a8b7880e 510
sahilmgandhi 31:9b71b44e0867 511 bool isWallInFront(int x, int y)
sahilmgandhi 31:9b71b44e0867 512 {
sahilmgandhi 27:02ce1040f331 513 return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
sahilmgandhi 26:d20f1adac2d3 514 }
sahilmgandhi 31:9b71b44e0867 515 bool isWallInBack(int x, int y)
sahilmgandhi 31:9b71b44e0867 516 {
sahilmgandhi 27:02ce1040f331 517 return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
sahilmgandhi 26:d20f1adac2d3 518 }
sahilmgandhi 31:9b71b44e0867 519 bool isWallOnRight(int x, int y)
sahilmgandhi 31:9b71b44e0867 520 {
sahilmgandhi 27:02ce1040f331 521 return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
sahilmgandhi 26:d20f1adac2d3 522 }
sahilmgandhi 31:9b71b44e0867 523 bool isWallOnLeft(int x, int y)
sahilmgandhi 31:9b71b44e0867 524 {
sahilmgandhi 27:02ce1040f331 525 return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
sahilmgandhi 26:d20f1adac2d3 526 }
sahilmgandhi 26:d20f1adac2d3 527
sahilmgandhi 31:9b71b44e0867 528 int chooseNextMovement()
sahilmgandhi 31:9b71b44e0867 529 {
sahilmgandhi 26:d20f1adac2d3 530 int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
sahilmgandhi 31:9b71b44e0867 531 if (goingToCenter && (currentDistance == 0)) {
sahilmgandhi 26:d20f1adac2d3 532 goingToCenter = false;
sahilmgandhi 26:d20f1adac2d3 533 changeManhattanDistance(goingToCenter);
sahilmgandhi 31:9b71b44e0867 534 } else if (!goingToCenter && (currentDistance == 0)) {
kyleliangus 32:69acb14778ea 535 goingToCenter = true;
sahilmgandhi 26:d20f1adac2d3 536 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 537 }
sahilmgandhi 26:d20f1adac2d3 538
sahilmgandhi 26:d20f1adac2d3 539 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 540 int currX = 0;
sahilmgandhi 26:d20f1adac2d3 541 int currY = 0;
sahilmgandhi 26:d20f1adac2d3 542 int currDist = 0;
sahilmgandhi 26:d20f1adac2d3 543 int dirToGo = 0;
sahilmgandhi 31:9b71b44e0867 544 if (!justTurned) {
sahilmgandhi 26:d20f1adac2d3 545 cellsToVisit.push(make_pair(mouseX, mouseY));
sahilmgandhi 26:d20f1adac2d3 546 while (!cellsToVisit.empty()) {
sahilmgandhi 26:d20f1adac2d3 547 pair<int, int> curr = cellsToVisit.top();
sahilmgandhi 26:d20f1adac2d3 548 cellsToVisit.pop();
sahilmgandhi 26:d20f1adac2d3 549 currX = curr.first;
sahilmgandhi 26:d20f1adac2d3 550 currY = curr.second;
sahilmgandhi 26:d20f1adac2d3 551 currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
sahilmgandhi 26:d20f1adac2d3 552 int minDist = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 553
sahilmgandhi 31:9b71b44e0867 554 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 555 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 556 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 557 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 558 }
sahilmgandhi 26:d20f1adac2d3 559 }
sahilmgandhi 27:02ce1040f331 560 if (currX - 1 >= 0 && !isWallOnLeft(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 (currY + 1 < MAZE_LEN && !isWallInFront( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 566 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 567 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 568 }
sahilmgandhi 26:d20f1adac2d3 569 }
sahilmgandhi 27:02ce1040f331 570 if (currY - 1 >= 0 && !isWallInBack(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 26:d20f1adac2d3 575 } else {
sahilmgandhi 26:d20f1adac2d3 576 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 577 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 578 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 579 }
sahilmgandhi 26:d20f1adac2d3 580 }
sahilmgandhi 26:d20f1adac2d3 581 if (currX - 1 >= 0) {
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 (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 587 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 588 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 589 }
sahilmgandhi 26:d20f1adac2d3 590 }
sahilmgandhi 26:d20f1adac2d3 591 if (currY - 1 >= 0) {
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 }
sahilmgandhi 26:d20f1adac2d3 597
sahilmgandhi 26:d20f1adac2d3 598 if (minDist == INT_MAX)
sahilmgandhi 26:d20f1adac2d3 599 continue;
sahilmgandhi 26:d20f1adac2d3 600 if (currDist > minDist)
sahilmgandhi 26:d20f1adac2d3 601 continue;
sahilmgandhi 26:d20f1adac2d3 602 if (currDist <= minDist) {
sahilmgandhi 26:d20f1adac2d3 603 manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
sahilmgandhi 26:d20f1adac2d3 604 }
sahilmgandhi 26:d20f1adac2d3 605 if (hasVisited(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 606 if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 607 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 608 }
sahilmgandhi 26:d20f1adac2d3 609 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 610 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 611 }
sahilmgandhi 26:d20f1adac2d3 612 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 613 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 614 }
sahilmgandhi 26:d20f1adac2d3 615 if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 616 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 617 }
sahilmgandhi 26:d20f1adac2d3 618 } else {
sahilmgandhi 26:d20f1adac2d3 619 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 620 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 621 }
sahilmgandhi 26:d20f1adac2d3 622 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 623 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 624 }
sahilmgandhi 26:d20f1adac2d3 625 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 626 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 627 }
sahilmgandhi 26:d20f1adac2d3 628 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 629 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 630 }
sahilmgandhi 26:d20f1adac2d3 631 }
sahilmgandhi 26:d20f1adac2d3 632 }
sahilmgandhi 26:d20f1adac2d3 633
sahilmgandhi 26:d20f1adac2d3 634 int minDistance = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 635 if (currDir % 4 == 0) {
sahilmgandhi 26:d20f1adac2d3 636 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 637 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 638 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 639 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 640 }
sahilmgandhi 26:d20f1adac2d3 641 }
sahilmgandhi 26:d20f1adac2d3 642 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 643 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 644 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 645 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 646 }
sahilmgandhi 26:d20f1adac2d3 647 }
sahilmgandhi 26:d20f1adac2d3 648 if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 649 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 650 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 651 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 652 }
sahilmgandhi 26:d20f1adac2d3 653 }
sahilmgandhi 26:d20f1adac2d3 654 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 655 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 656 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 657 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 658 }
sahilmgandhi 26:d20f1adac2d3 659 }
sahilmgandhi 26:d20f1adac2d3 660 } else if (currDir % 4 == 2) {
sahilmgandhi 26:d20f1adac2d3 661 if (mouseX - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 662 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 663 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 664 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 665 }
sahilmgandhi 26:d20f1adac2d3 666 }
sahilmgandhi 26:d20f1adac2d3 667 if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 668 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 669 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 670 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 671 }
sahilmgandhi 26:d20f1adac2d3 672 }
sahilmgandhi 26:d20f1adac2d3 673 if (mouseY - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 674 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 675 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 676 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 677 }
sahilmgandhi 26:d20f1adac2d3 678 }
sahilmgandhi 26:d20f1adac2d3 679 if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 680 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 681 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 682 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 683 }
sahilmgandhi 26:d20f1adac2d3 684 }
sahilmgandhi 26:d20f1adac2d3 685 } else if (currDir % 4 == 1) {
sahilmgandhi 26:d20f1adac2d3 686 if (mouseY - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 687 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 688 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 689 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 690 }
sahilmgandhi 26:d20f1adac2d3 691 }
sahilmgandhi 26:d20f1adac2d3 692 if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 693 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 694 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 695 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 696 }
sahilmgandhi 26:d20f1adac2d3 697 }
sahilmgandhi 26:d20f1adac2d3 698 if (mouseX + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 699 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 700 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 701 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 702 }
sahilmgandhi 26:d20f1adac2d3 703 }
sahilmgandhi 26:d20f1adac2d3 704 if (mouseX - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 705 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 706 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 707 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 708 }
sahilmgandhi 26:d20f1adac2d3 709 }
sahilmgandhi 26:d20f1adac2d3 710 } else if (currDir % 4 == 3) {
sahilmgandhi 26:d20f1adac2d3 711 if (mouseY + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 712 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 713 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 714 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 715 }
sahilmgandhi 26:d20f1adac2d3 716 }
sahilmgandhi 26:d20f1adac2d3 717 if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 718 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 719 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 720 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 721 }
sahilmgandhi 26:d20f1adac2d3 722 }
sahilmgandhi 26:d20f1adac2d3 723 if (mouseX - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 724 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 725 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 726 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 727 }
sahilmgandhi 26:d20f1adac2d3 728 }
sahilmgandhi 26:d20f1adac2d3 729 if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 730 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 731 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 732 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 733 }
sahilmgandhi 26:d20f1adac2d3 734 }
sahilmgandhi 26:d20f1adac2d3 735 }
sahilmgandhi 31:9b71b44e0867 736 } else {
sahilmgandhi 26:d20f1adac2d3 737 justTurned = false;
sahilmgandhi 26:d20f1adac2d3 738 dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 739 }
sahilmgandhi 26:d20f1adac2d3 740
sahilmgandhi 26:d20f1adac2d3 741 return dirToGo;
sahilmgandhi 26:d20f1adac2d3 742 }
sahilmgandhi 26:d20f1adac2d3 743
sahilmgandhi 31:9b71b44e0867 744 bool hasVisited(int x, int y)
sahilmgandhi 31:9b71b44e0867 745 {
sahilmgandhi 26:d20f1adac2d3 746 return visitedCells[MAZE_LEN - 1 - y][x];
sahilmgandhi 26:d20f1adac2d3 747 }
sahilmgandhi 26:d20f1adac2d3 748
sahilmgandhi 31:9b71b44e0867 749 void changeManhattanDistance(bool headCenter)
sahilmgandhi 31:9b71b44e0867 750 {
sahilmgandhi 31:9b71b44e0867 751 if (headCenter) {
sahilmgandhi 29:ec2c5a69acd6 752 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 753 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 754 distanceToCenter[i][j] = manhattanDistances[i][j];
sahilmgandhi 26:d20f1adac2d3 755 }
sahilmgandhi 26:d20f1adac2d3 756 }
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 manhattanDistances[i][j] = distanceToStart[i][j];
sahilmgandhi 26:d20f1adac2d3 760 }
sahilmgandhi 26:d20f1adac2d3 761 }
sahilmgandhi 31:9b71b44e0867 762 } else {
sahilmgandhi 31:9b71b44e0867 763 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 764 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 765 distanceToStart[i][j] = manhattanDistances[i][j];
sahilmgandhi 29:ec2c5a69acd6 766 }
sahilmgandhi 29:ec2c5a69acd6 767 }
sahilmgandhi 29:ec2c5a69acd6 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 manhattanDistances[i][j] = distanceToCenter[i][j];
sahilmgandhi 29:ec2c5a69acd6 771 }
sahilmgandhi 29:ec2c5a69acd6 772 }
sahilmgandhi 29:ec2c5a69acd6 773 }
sahilmgandhi 29:ec2c5a69acd6 774 }
sahilmgandhi 29:ec2c5a69acd6 775
sahilmgandhi 31:9b71b44e0867 776 void initializeDistances()
sahilmgandhi 31:9b71b44e0867 777 {
sahilmgandhi 29:ec2c5a69acd6 778 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 31:9b71b44e0867 779 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 31:9b71b44e0867 780 distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 781 }
sahilmgandhi 31:9b71b44e0867 782 }
sahilmgandhi 31:9b71b44e0867 783 for (int i = MAZE_LEN / 2; i < MAZE_LEN; 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 = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 31:9b71b44e0867 789 for (int j = MAZE_LEN / 2; j < MAZE_LEN; 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 = MAZE_LEN / 2; i < MAZE_LEN; 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 26:d20f1adac2d3 798 }
sahilmgandhi 31:9b71b44e0867 799
sahilmgandhi 0:a03c771ab78e 800 int main()
sahilmgandhi 0:a03c771ab78e 801 {
christine222 3:880f15be8c72 802 //Set highest bandwidth.
christine222 23:690b0ca34ee9 803 //gyro.setLpBandwidth(LPFBW_42HZ);
sahilmgandhi 29:ec2c5a69acd6 804 initializeDistances();
christine222 3:880f15be8c72 805 serial.baud(9600);
christine222 23:690b0ca34ee9 806 //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 31:9b71b44e0867 807
sahilmgandhi 1:8a4b2f573923 808 wait (0.1);
sahilmgandhi 31:9b71b44e0867 809
sahilmgandhi 2:771db996cee0 810 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 811 greenLed.write(0);
sahilmgandhi 2:771db996cee0 812 blueLed.write(1);
sahilmgandhi 31:9b71b44e0867 813
kyleliangus 9:1d8e4da058cd 814 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 815 //right_motor.forward(0.1);
sahilmgandhi 31:9b71b44e0867 816
kyleliangus 8:a0760acdc59e 817 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 818 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 819 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 820 // PB_3 is B of left
vanshg 11:8fc2b703086b 821 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 822 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 31:9b71b44e0867 823
vanshg 10:810d1849da9d 824 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 825 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 826 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 827 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 828 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 829 */
sahilmgandhi 31:9b71b44e0867 830
kyleliangus 12:5790e56a056f 831 // 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 832 // of alternate function specified
kyleliangus 12:5790e56a056f 833 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 834 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 835 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 836 //
kyleliangus 12:5790e56a056f 837 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 838 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 839 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 840 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 841 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 842 // CR1 reenabales
kyleliangus 12:5790e56a056f 843 // then read CNT reg of timer
sahilmgandhi 31:9b71b44e0867 844
sahilmgandhi 31:9b71b44e0867 845
vanshg 10:810d1849da9d 846 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 847 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 848 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 849 dipButton4.rise(&enableButton4);
sahilmgandhi 31:9b71b44e0867 850
vanshg 10:810d1849da9d 851 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 852 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 853 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 854 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 855
sahilmgandhi 26:d20f1adac2d3 856 // if(dipFlags == 0x1){
sahilmgandhi 26:d20f1adac2d3 857 // }else{
sahilmgandhi 26:d20f1adac2d3 858 // turnRight();
sahilmgandhi 26:d20f1adac2d3 859 // IRP_1.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 860 // IRP_4.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 861 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 862 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 863
sahilmgandhi 26:d20f1adac2d3 864 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 865 // turnLeft();
sahilmgandhi 26:d20f1adac2d3 866 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 867 // }
sahilmgandhi 26:d20f1adac2d3 868
sahilmgandhi 26:d20f1adac2d3 869
sahilmgandhi 26:d20f1adac2d3 870 // init the wall, and mouse loc arrays:
sahilmgandhi 26:d20f1adac2d3 871 wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
sahilmgandhi 26:d20f1adac2d3 872 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 873
kyleliangus 32:69acb14778ea 874 //int prevEncoder0Count = 0;
kyleliangus 32:69acb14778ea 875 //int prevEncoder1Count = 0;
kyleliangus 32:69acb14778ea 876 //int currEncoder0Count = 0;
kyleliangus 32:69acb14778ea 877 //int currEncoder1Count = 0;
sahilmgandhi 28:8126a4d620e8 878
kyleliangus 32:69acb14778ea 879 //bool overrideFloodFill = false;
christine222 25:f827a8b7880e 880 //right_motor.forward( 0.2 );
christine222 25:f827a8b7880e 881 //left_motor.forward( 0.2 );
sahilmgandhi 26:d20f1adac2d3 882 //turnRight180();
sahilmgandhi 26:d20f1adac2d3 883 //wait_ms(1500);
kyleliangus 32:69acb14778ea 884 //int nextMovement = 0;
kyleliangus 32:69acb14778ea 885 //serial.printf("I'm about to enter!");
kyleliangus 32:69acb14778ea 886 nCellEncoderAndIR(3);
sahilmgandhi 26:d20f1adac2d3 887 while (1) {
sahilmgandhi 31:9b71b44e0867 888 // prevEncoder0Count = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 889 // prevEncoder1Count = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 890 //
sahilmgandhi 29:ec2c5a69acd6 891 // if (!overrideFloodFill){
sahilmgandhi 29:ec2c5a69acd6 892 // nextMovement = chooseNextMovement();
sahilmgandhi 29:ec2c5a69acd6 893 // if (nextMovement == 0){
sahilmgandhi 29:ec2c5a69acd6 894 // nCellEncoderAndIR(1);
sahilmgandhi 29:ec2c5a69acd6 895 // }
sahilmgandhi 29:ec2c5a69acd6 896 // else if (nextMovement == 1){
sahilmgandhi 29:ec2c5a69acd6 897 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 898 // turnRight();
sahilmgandhi 29:ec2c5a69acd6 899 // }
sahilmgandhi 29:ec2c5a69acd6 900 // else if (nextMovement == 2){
sahilmgandhi 29:ec2c5a69acd6 901 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 902 // turnLeft();
sahilmgandhi 29:ec2c5a69acd6 903 // }
sahilmgandhi 29:ec2c5a69acd6 904 // else if (nextMovement == 3){
sahilmgandhi 29:ec2c5a69acd6 905 // nCellEncoderAndIR(1);
sahilmgandhi 29:ec2c5a69acd6 906 // }
sahilmgandhi 29:ec2c5a69acd6 907 // else if (nextMovement == 4){
sahilmgandhi 29:ec2c5a69acd6 908 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 909 // turnRight180();
sahilmgandhi 29:ec2c5a69acd6 910 // }
sahilmgandhi 29:ec2c5a69acd6 911 // }
sahilmgandhi 29:ec2c5a69acd6 912 // else{
sahilmgandhi 29:ec2c5a69acd6 913 // overrideFloodFill = false;
sahilmgandhi 29:ec2c5a69acd6 914 // if ((rand()%2 + 1) == 1){
sahilmgandhi 29:ec2c5a69acd6 915 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 916 // turnLeft();
sahilmgandhi 29:ec2c5a69acd6 917 // }
sahilmgandhi 29:ec2c5a69acd6 918 // else{
sahilmgandhi 29:ec2c5a69acd6 919 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 920 // turnRight();
sahilmgandhi 29:ec2c5a69acd6 921 // }
sahilmgandhi 29:ec2c5a69acd6 922 // }
sahilmgandhi 29:ec2c5a69acd6 923 // currEncoder0Count = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 924 // currEncoder1Count = encoder1.getPulses();
sahilmgandhi 31:9b71b44e0867 925 //
sahilmgandhi 29:ec2c5a69acd6 926 // if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
sahilmgandhi 29:ec2c5a69acd6 927 // overrideFloodFill = true;
sahilmgandhi 29:ec2c5a69acd6 928 // }
sahilmgandhi 29:ec2c5a69acd6 929 //
sahilmgandhi 29:ec2c5a69acd6 930 // wait_ms(300); // reduce this once we fine tune this!
christine222 24:e7063765d6f0 931
christine222 23:690b0ca34ee9 932 //wait_ms(1500);
christine222 23:690b0ca34ee9 933 //turnRight();
christine222 23:690b0ca34ee9 934 //wait_ms(1500);
christine222 23:690b0ca34ee9 935 //turnLeft();
sahilmgandhi 26:d20f1adac2d3 936 // nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 937 // wait_ms(500);
christine222 23:690b0ca34ee9 938 // turnRight();
christine222 23:690b0ca34ee9 939 // wait_ms(500);
christine222 23:690b0ca34ee9 940 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 941 // wait_ms(500);
christine222 23:690b0ca34ee9 942 // turnRight();
christine222 23:690b0ca34ee9 943 // wait_ms(500);
christine222 23:690b0ca34ee9 944 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 945 // wait_ms(500);
christine222 23:690b0ca34ee9 946 // turnLeft();
christine222 23:690b0ca34ee9 947 // wait_ms(500);
christine222 23:690b0ca34ee9 948 // nCellEncoderAndIR(2);
christine222 23:690b0ca34ee9 949 // wait_ms(500);
christine222 23:690b0ca34ee9 950 // turnRight();
christine222 23:690b0ca34ee9 951 // wait_ms(500);
christine222 23:690b0ca34ee9 952 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 953 // wait_ms(500);
christine222 23:690b0ca34ee9 954 // turnRight();
christine222 23:690b0ca34ee9 955 // wait_ms(500);
christine222 23:690b0ca34ee9 956 // nCellEncoderAndIR(5);
christine222 23:690b0ca34ee9 957 // break;
christine222 23:690b0ca34ee9 958 // turnRight180();
sahilmgandhi 31:9b71b44e0867 959
christine222 23:690b0ca34ee9 960 // int number = rand() % 4 + 1;
christine222 23:690b0ca34ee9 961 // switch(number){
christine222 23:690b0ca34ee9 962 // case(1):{//turn right
christine222 23:690b0ca34ee9 963 // turnRight();
christine222 23:690b0ca34ee9 964 // break;
christine222 23:690b0ca34ee9 965 // }
christine222 23:690b0ca34ee9 966 // case(2):{ // turn left
christine222 23:690b0ca34ee9 967 // turnLeft();
christine222 23:690b0ca34ee9 968 // break;
christine222 23:690b0ca34ee9 969 // }
christine222 23:690b0ca34ee9 970 // case(3):{// keep going
sahilmgandhi 31:9b71b44e0867 971
christine222 23:690b0ca34ee9 972 // break;
christine222 23:690b0ca34ee9 973 // }
christine222 23:690b0ca34ee9 974 // case(4):{// turnaround
christine222 23:690b0ca34ee9 975 // turnRight180();
christine222 23:690b0ca34ee9 976 // break;
christine222 23:690b0ca34ee9 977 // }
christine222 23:690b0ca34ee9 978 // default:{// keep going
christine222 23:690b0ca34ee9 979 // break;
christine222 23:690b0ca34ee9 980 // }
christine222 23:690b0ca34ee9 981 // }
sahilmgandhi 31:9b71b44e0867 982
christine222 23:690b0ca34ee9 983 // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
christine222 23:690b0ca34ee9 984 // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
sahilmgandhi 31:9b71b44e0867 985
christine222 23:690b0ca34ee9 986 // float ir3 = IRP_2.getSamples(100)/initAverageL;
christine222 23:690b0ca34ee9 987 // float ir2 = IRP_3.getSamples(100)/initAverageR;
sahilmgandhi 31:9b71b44e0867 988
sahilmgandhi 31:9b71b44e0867 989
christine222 25:f827a8b7880e 990
christine222 23:690b0ca34ee9 991 /*
christine222 23:690b0ca34ee9 992 counter2++;
sahilmgandhi 31:9b71b44e0867 993 counter3++;
christine222 23:690b0ca34ee9 994 ir2tot += IRP_2.getSamples(100);
christine222 23:690b0ca34ee9 995 ir3tot += IRP_3.getSamples(100);
sahilmgandhi 31:9b71b44e0867 996
sahilmgandhi 31:9b71b44e0867 997
christine222 23:690b0ca34ee9 998 ir2 = ir2tot/counter2;
christine222 23:690b0ca34ee9 999 ir3 = ir3tot/counter3;
sahilmgandhi 31:9b71b44e0867 1000
sahilmgandhi 31:9b71b44e0867 1001
christine222 23:690b0ca34ee9 1002 serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1003 */
christine222 23:690b0ca34ee9 1004 //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
christine222 23:690b0ca34ee9 1005 //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3);
christine222 23:690b0ca34ee9 1006 //break;
christine222 23:690b0ca34ee9 1007 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
christine222 23:690b0ca34ee9 1008 //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 1009 //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
sahilmgandhi 31:9b71b44e0867 1010
sahilmgandhi 31:9b71b44e0867 1011
christine222 23:690b0ca34ee9 1012 ////////////////////////////////////////////////////////////////
sahilmgandhi 31:9b71b44e0867 1013
kyleliangus 32:69acb14778ea 1014 //nCellEncoderAndIR(4);
kyleliangus 32:69acb14778ea 1015 //while(1);
christine222 22:681190ff98f0 1016 //break;
sahilmgandhi 31:9b71b44e0867 1017
christine222 23:690b0ca34ee9 1018 //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100));
christine222 25:f827a8b7880e 1019
christine222 21:9a6cb07bdcb6 1020 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
sahilmgandhi 31:9b71b44e0867 1021
christine222 21:9a6cb07bdcb6 1022 //break;
christine222 21:9a6cb07bdcb6 1023 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1024 // wait(0.5);
christine222 21:9a6cb07bdcb6 1025 // handleTurns();
christine222 21:9a6cb07bdcb6 1026 // wait(0.5);
christine222 21:9a6cb07bdcb6 1027 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1028 // wait(0.5);
christine222 21:9a6cb07bdcb6 1029 // handleTurns();
christine222 21:9a6cb07bdcb6 1030 //break;
sahilmgandhi 16:d9252437bd92 1031 //pidOnEncoders();
sahilmgandhi 31:9b71b44e0867 1032 // moveForwardUntilWallIr();
kyleliangus 15:b80555a4a8b9 1033 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
kyleliangus 32:69acb14778ea 1034 //double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
kyleliangus 32:69acb14778ea 1035 //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 );
sahilmgandhi 31:9b71b44e0867 1036
christine222 3:880f15be8c72 1037 //reading = Rec_4.read();
christine222 3:880f15be8c72 1038 // serial.printf("reading: %f\n", reading);
christine222 3:880f15be8c72 1039 }
sahilmgandhi 26:d20f1adac2d3 1040 }