MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@31:9b71b44e0867, 2017-05-24 (annotated)

Committer:
sahilmgandhi
Date:
Wed May 24 21:47:21 2017 +0000
Revision:
31:9b71b44e0867
Parent:
29:ec2c5a69acd6
Child:
32:69acb14778ea
newly modified floodfill, need to get PID with new values now!

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
christine222 25:f827a8b7880e 139 float ir1 = IRP_1.getSamples(50);
christine222 25:f827a8b7880e 140 float 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 {
sahilmgandhi 19:7b66a518b6f8 343 double currentError = 0;
sahilmgandhi 19:7b66a518b6f8 344 double previousError = 0;
sahilmgandhi 19:7b66a518b6f8 345 double derivError = 0;
sahilmgandhi 19:7b66a518b6f8 346 double sumError = 0;
christine222 25:f827a8b7880e 347
sahilmgandhi 29:ec2c5a69acd6 348 double HIGH_PWM_VOLTAGE_R = 0.15;
sahilmgandhi 29:ec2c5a69acd6 349 double HIGH_PWM_VOLTAGE_L = 0.16;
sahilmgandhi 29:ec2c5a69acd6 350
sahilmgandhi 29:ec2c5a69acd6 351 double rightSpeed = 0.15;
sahilmgandhi 29:ec2c5a69acd6 352 double leftSpeed = 0.16;
christine222 25:f827a8b7880e 353
christine222 25:f827a8b7880e 354 int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 355 int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
christine222 25:f827a8b7880e 356
sahilmgandhi 29:ec2c5a69acd6 357 left_motor.forward(0.16);
sahilmgandhi 29:ec2c5a69acd6 358 right_motor.forward(0.15);
christine222 25:f827a8b7880e 359
sahilmgandhi 19:7b66a518b6f8 360 float ir2 = IRP_2.getSamples( SAMPLE_NUM );
sahilmgandhi 19:7b66a518b6f8 361 float ir3 = IRP_3.getSamples( SAMPLE_NUM );
christine222 25:f827a8b7880e 362
christine222 25:f827a8b7880e 363 // float previr2 = ir2;
christine222 25:f827a8b7880e 364 // float previr3 = ir3;
christine222 25:f827a8b7880e 365
christine222 23:690b0ca34ee9 366 int state = 0;
christine222 23:690b0ca34ee9 367
sahilmgandhi 31:9b71b44e0867 368 while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
sahilmgandhi 29:ec2c5a69acd6 369 receiverTwoReading = IRP_2.getSamples(100);
sahilmgandhi 29:ec2c5a69acd6 370 receiverThreeReading = IRP_3.getSamples(100);
sahilmgandhi 29:ec2c5a69acd6 371 receiverOneReading = IRP_1.getSamples(100);
sahilmgandhi 29:ec2c5a69acd6 372 receiverFourReading = IRP_4.getSamples(100);
christine222 25:f827a8b7880e 373
sahilmgandhi 31:9b71b44e0867 374 if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ) {
sahilmgandhi 26:d20f1adac2d3 375 break;
sahilmgandhi 26:d20f1adac2d3 376 }
christine222 25:f827a8b7880e 377
sahilmgandhi 31:9b71b44e0867 378 if((receiverThreeReading < 3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))) {
christine222 25:f827a8b7880e 379 // both sides gone
sahilmgandhi 26:d20f1adac2d3 380 int prev0 = encoder0.getPulses();
sahilmgandhi 26:d20f1adac2d3 381 int prev1 = encoder1.getPulses();
sahilmgandhi 26:d20f1adac2d3 382 int diff0 = desiredCount0 - prev0;
sahilmgandhi 26:d20f1adac2d3 383 int diff1 = desiredCount1 - prev1;
sahilmgandhi 26:d20f1adac2d3 384 int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
sahilmgandhi 26:d20f1adac2d3 385 moveForwardEncoder(valToPass);
sahilmgandhi 31:9b71b44e0867 386 } else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR) { // right wall gone
christine222 25:f827a8b7880e 387 state = 1;
christine222 25:f827a8b7880e 388 redLed.write(0);
christine222 25:f827a8b7880e 389 greenLed.write(1);
christine222 25:f827a8b7880e 390 blueLed.write(1);
sahilmgandhi 31:9b71b44e0867 391 } else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL) { // left wall gone
christine222 25:f827a8b7880e 392 // BLUE BLUE BLUE BLUE
christine222 25:f827a8b7880e 393 state = 2;
christine222 25:f827a8b7880e 394 redLed.write(1);
christine222 25:f827a8b7880e 395 greenLed.write(1);
christine222 25:f827a8b7880e 396 blueLed.write(0);
sahilmgandhi 31:9b71b44e0867 397 } else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))) {
christine222 25:f827a8b7880e 398 // both walls there
christine222 25:f827a8b7880e 399 state = 0;
christine222 25:f827a8b7880e 400 redLed.write(1);
christine222 25:f827a8b7880e 401 greenLed.write(0);
christine222 25:f827a8b7880e 402 blueLed.write(1);
christine222 25:f827a8b7880e 403 }
christine222 25:f827a8b7880e 404
sahilmgandhi 31:9b71b44e0867 405 switch(state) {
sahilmgandhi 31:9b71b44e0867 406 case(0): { // both walls there
christine222 25:f827a8b7880e 407 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 408 break;
christine222 25:f827a8b7880e 409 }
sahilmgandhi 31:9b71b44e0867 410 case(1): { // RED RED RED RED RED
christine222 25:f827a8b7880e 411 currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
sahilmgandhi 31:9b71b44e0867 412 break;
christine222 25:f827a8b7880e 413 }
sahilmgandhi 31:9b71b44e0867 414 case(2): { // blue
christine222 25:f827a8b7880e 415 currentError = (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 416 break;
christine222 25:f827a8b7880e 417 }
sahilmgandhi 31:9b71b44e0867 418 default: {
christine222 25:f827a8b7880e 419 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
christine222 25:f827a8b7880e 420 break;
christine222 25:f827a8b7880e 421 }
christine222 25:f827a8b7880e 422 }
christine222 25:f827a8b7880e 423
christine222 25:f827a8b7880e 424 sumError += currentError;
christine222 25:f827a8b7880e 425 derivError = currentError - previousError;
christine222 25:f827a8b7880e 426 double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
christine222 25:f827a8b7880e 427 if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
sahilmgandhi 29:ec2c5a69acd6 428 rightSpeed = HIGH_PWM_VOLTAGE_R - abs(PIDSum*HIGH_PWM_VOLTAGE_R);
sahilmgandhi 29:ec2c5a69acd6 429 leftSpeed = HIGH_PWM_VOLTAGE_L + abs(PIDSum*HIGH_PWM_VOLTAGE_L);
christine222 25:f827a8b7880e 430 } else { // r is faster than L. speed up l, slow down r
sahilmgandhi 29:ec2c5a69acd6 431 rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
sahilmgandhi 29:ec2c5a69acd6 432 leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
sahilmgandhi 31:9b71b44e0867 433 }
christine222 25:f827a8b7880e 434 if (leftSpeed > 0.30) leftSpeed = 0.30;
christine222 25:f827a8b7880e 435 if (leftSpeed < 0) leftSpeed = 0;
christine222 25:f827a8b7880e 436 if (rightSpeed > 0.30) rightSpeed = 0.30;
christine222 25:f827a8b7880e 437 if (rightSpeed < 0) rightSpeed = 0;
sahilmgandhi 31:9b71b44e0867 438
christine222 25:f827a8b7880e 439 right_motor.forward(rightSpeed);
christine222 25:f827a8b7880e 440 left_motor.forward(leftSpeed);
christine222 25:f827a8b7880e 441 pidOnEncoders();
sahilmgandhi 31:9b71b44e0867 442
christine222 25:f827a8b7880e 443 previousError = currentError;
sahilmgandhi 26:d20f1adac2d3 444 }
sahilmgandhi 31:9b71b44e0867 445 if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1) {
sahilmgandhi 31:9b71b44e0867 446 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 447 mouseY += 1;
sahilmgandhi 31:9b71b44e0867 448 } else if (currDir % 4 == 1) {
sahilmgandhi 28:8126a4d620e8 449 mouseX + 1;
sahilmgandhi 31:9b71b44e0867 450 } else if (currDir % 4 == 2) {
sahilmgandhi 28:8126a4d620e8 451 mouseY -= 1;
sahilmgandhi 31:9b71b44e0867 452 } else if (currDir % 4 == 3) {
sahilmgandhi 28:8126a4d620e8 453 mouseX -= 1;
sahilmgandhi 28:8126a4d620e8 454 }
sahilmgandhi 31:9b71b44e0867 455
sahilmgandhi 31:9b71b44e0867 456 // the mouse has moved forward, we need to update the wall map now
sahilmgandhi 31:9b71b44e0867 457 receiverOneReading = IRP_1.getSamples(50);
sahilmgandhi 31:9b71b44e0867 458 receiverTwoReading = IRP_2.getSamples(50);
sahilmgandhi 31:9b71b44e0867 459 receiverThreeReading = IRP_3.getSamples(50);
sahilmgandhi 31:9b71b44e0867 460 receiverFourReading = IRP_4.getSamples(50);
sahilmgandhi 31:9b71b44e0867 461
sahilmgandhi 31:9b71b44e0867 462 if (receiverOneReading >= 0.5f && receiverFourReading >= 0.5f) {
sahilmgandhi 31:9b71b44e0867 463 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 464 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 465 } else if (currDir % 4 == 1) {
sahilmgandhi 31:9b71b44e0867 466 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 467 } else if (currDir % 4 == 2) {
sahilmgandhi 31:9b71b44e0867 468 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
sahilmgandhi 31:9b71b44e0867 469 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 470 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 471 }
sahilmgandhi 31:9b71b44e0867 472 }
sahilmgandhi 31:9b71b44e0867 473 if (receiverThreeReading >= 0.1f) {
sahilmgandhi 31:9b71b44e0867 474 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 475 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 476 } else if (currDir % 4 == 1) {
sahilmgandhi 31:9b71b44e0867 477 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 478 } else if (currDir % 4 == 2) {
sahilmgandhi 31:9b71b44e0867 479 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
sahilmgandhi 31:9b71b44e0867 480 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 481 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 482 }
sahilmgandhi 31:9b71b44e0867 483 }
sahilmgandhi 31:9b71b44e0867 484 if (receiverTwoReading >= 0.1f) {
sahilmgandhi 31:9b71b44e0867 485 if (currDir % 4 == 0) {
sahilmgandhi 31:9b71b44e0867 486 wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 31:9b71b44e0867 487 } else if (currDir % 4 == 1) {
sahilmgandhi 31:9b71b44e0867 488 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
sahilmgandhi 31:9b71b44e0867 489 } else if (currDir % 4 == 2) {
sahilmgandhi 31:9b71b44e0867 490 wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
sahilmgandhi 31:9b71b44e0867 491 } else if (currDir % 4 == 3) {
sahilmgandhi 31:9b71b44e0867 492 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
sahilmgandhi 31:9b71b44e0867 493 }
sahilmgandhi 31:9b71b44e0867 494 }
sahilmgandhi 26:d20f1adac2d3 495 }
sahilmgandhi 31:9b71b44e0867 496
sahilmgandhi 19:7b66a518b6f8 497 left_motor.brake();
sahilmgandhi 19:7b66a518b6f8 498 right_motor.brake();
sahilmgandhi 17:f713758f6238 499 }
christine222 25:f827a8b7880e 500
sahilmgandhi 31:9b71b44e0867 501 bool isWallInFront(int x, int y)
sahilmgandhi 31:9b71b44e0867 502 {
sahilmgandhi 27:02ce1040f331 503 return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
sahilmgandhi 26:d20f1adac2d3 504 }
sahilmgandhi 31:9b71b44e0867 505 bool isWallInBack(int x, int y)
sahilmgandhi 31:9b71b44e0867 506 {
sahilmgandhi 27:02ce1040f331 507 return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
sahilmgandhi 26:d20f1adac2d3 508 }
sahilmgandhi 31:9b71b44e0867 509 bool isWallOnRight(int x, int y)
sahilmgandhi 31:9b71b44e0867 510 {
sahilmgandhi 27:02ce1040f331 511 return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
sahilmgandhi 26:d20f1adac2d3 512 }
sahilmgandhi 31:9b71b44e0867 513 bool isWallOnLeft(int x, int y)
sahilmgandhi 31:9b71b44e0867 514 {
sahilmgandhi 27:02ce1040f331 515 return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
sahilmgandhi 26:d20f1adac2d3 516 }
sahilmgandhi 26:d20f1adac2d3 517
sahilmgandhi 31:9b71b44e0867 518 int chooseNextMovement()
sahilmgandhi 31:9b71b44e0867 519 {
sahilmgandhi 26:d20f1adac2d3 520 int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
sahilmgandhi 31:9b71b44e0867 521 if (goingToCenter && (currentDistance == 0)) {
sahilmgandhi 26:d20f1adac2d3 522 goingToCenter = false;
sahilmgandhi 26:d20f1adac2d3 523 changeManhattanDistance(goingToCenter);
sahilmgandhi 31:9b71b44e0867 524 } else if (!goingToCenter && (currentDistance == 0)) {
sahilmgandhi 26:d20f1adac2d3 525 goingToCenter == true;
sahilmgandhi 26:d20f1adac2d3 526 changeManhattanDistance(goingToCenter);
sahilmgandhi 26:d20f1adac2d3 527 }
sahilmgandhi 26:d20f1adac2d3 528
sahilmgandhi 26:d20f1adac2d3 529 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 530 int currX = 0;
sahilmgandhi 26:d20f1adac2d3 531 int currY = 0;
sahilmgandhi 26:d20f1adac2d3 532 int currDist = 0;
sahilmgandhi 26:d20f1adac2d3 533 int dirToGo = 0;
sahilmgandhi 31:9b71b44e0867 534 if (!justTurned) {
sahilmgandhi 26:d20f1adac2d3 535 cellsToVisit.push(make_pair(mouseX, mouseY));
sahilmgandhi 26:d20f1adac2d3 536 while (!cellsToVisit.empty()) {
sahilmgandhi 26:d20f1adac2d3 537 pair<int, int> curr = cellsToVisit.top();
sahilmgandhi 26:d20f1adac2d3 538 cellsToVisit.pop();
sahilmgandhi 26:d20f1adac2d3 539 currX = curr.first;
sahilmgandhi 26:d20f1adac2d3 540 currY = curr.second;
sahilmgandhi 26:d20f1adac2d3 541 currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
sahilmgandhi 26:d20f1adac2d3 542 int minDist = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 543
sahilmgandhi 31:9b71b44e0867 544 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 545 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 546 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 547 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 548 }
sahilmgandhi 26:d20f1adac2d3 549 }
sahilmgandhi 27:02ce1040f331 550 if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 551 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 552 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 553 }
sahilmgandhi 26:d20f1adac2d3 554 }
sahilmgandhi 27:02ce1040f331 555 if (currY + 1 < MAZE_LEN && !isWallInFront( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 556 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 557 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 558 }
sahilmgandhi 26:d20f1adac2d3 559 }
sahilmgandhi 27:02ce1040f331 560 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 561 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 562 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 563 }
sahilmgandhi 26:d20f1adac2d3 564 }
sahilmgandhi 26:d20f1adac2d3 565 } else {
sahilmgandhi 26:d20f1adac2d3 566 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 567 if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 568 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
sahilmgandhi 26:d20f1adac2d3 569 }
sahilmgandhi 26:d20f1adac2d3 570 }
sahilmgandhi 26:d20f1adac2d3 571 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 572 if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
sahilmgandhi 26:d20f1adac2d3 573 minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
sahilmgandhi 26:d20f1adac2d3 574 }
sahilmgandhi 26:d20f1adac2d3 575 }
sahilmgandhi 26:d20f1adac2d3 576 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 577 if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 578 minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
sahilmgandhi 26:d20f1adac2d3 579 }
sahilmgandhi 26:d20f1adac2d3 580 }
sahilmgandhi 26:d20f1adac2d3 581 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 582 if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
sahilmgandhi 26:d20f1adac2d3 583 minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
sahilmgandhi 26:d20f1adac2d3 584 }
sahilmgandhi 26:d20f1adac2d3 585 }
sahilmgandhi 26:d20f1adac2d3 586 }
sahilmgandhi 26:d20f1adac2d3 587
sahilmgandhi 26:d20f1adac2d3 588 if (minDist == INT_MAX)
sahilmgandhi 26:d20f1adac2d3 589 continue;
sahilmgandhi 26:d20f1adac2d3 590 if (currDist > minDist)
sahilmgandhi 26:d20f1adac2d3 591 continue;
sahilmgandhi 26:d20f1adac2d3 592 if (currDist <= minDist) {
sahilmgandhi 26:d20f1adac2d3 593 manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
sahilmgandhi 26:d20f1adac2d3 594 }
sahilmgandhi 26:d20f1adac2d3 595 if (hasVisited(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 596 if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 597 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 598 }
sahilmgandhi 26:d20f1adac2d3 599 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 600 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 601 }
sahilmgandhi 26:d20f1adac2d3 602 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 603 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 604 }
sahilmgandhi 26:d20f1adac2d3 605 if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
sahilmgandhi 26:d20f1adac2d3 606 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 607 }
sahilmgandhi 26:d20f1adac2d3 608 } else {
sahilmgandhi 26:d20f1adac2d3 609 if (currY + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 610 cellsToVisit.push(make_pair(currX, currY + 1));
sahilmgandhi 26:d20f1adac2d3 611 }
sahilmgandhi 26:d20f1adac2d3 612 if (currX + 1 < MAZE_LEN) {
sahilmgandhi 26:d20f1adac2d3 613 cellsToVisit.push(make_pair(currX + 1, currY));
sahilmgandhi 26:d20f1adac2d3 614 }
sahilmgandhi 26:d20f1adac2d3 615 if (currY - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 616 cellsToVisit.push(make_pair(currX, currY - 1));
sahilmgandhi 26:d20f1adac2d3 617 }
sahilmgandhi 26:d20f1adac2d3 618 if (currX - 1 >= 0) {
sahilmgandhi 26:d20f1adac2d3 619 cellsToVisit.push(make_pair(currX - 1, currY));
sahilmgandhi 26:d20f1adac2d3 620 }
sahilmgandhi 26:d20f1adac2d3 621 }
sahilmgandhi 26:d20f1adac2d3 622 }
sahilmgandhi 26:d20f1adac2d3 623
sahilmgandhi 26:d20f1adac2d3 624 int minDistance = INT_MAX;
sahilmgandhi 26:d20f1adac2d3 625 if (currDir % 4 == 0) {
sahilmgandhi 26:d20f1adac2d3 626 if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 627 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 628 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 629 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 630 }
sahilmgandhi 26:d20f1adac2d3 631 }
sahilmgandhi 26:d20f1adac2d3 632 if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 633 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 634 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 635 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 636 }
sahilmgandhi 26:d20f1adac2d3 637 }
sahilmgandhi 26:d20f1adac2d3 638 if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 639 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 640 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 641 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 642 }
sahilmgandhi 26:d20f1adac2d3 643 }
sahilmgandhi 26:d20f1adac2d3 644 if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 645 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 646 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 647 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 648 }
sahilmgandhi 26:d20f1adac2d3 649 }
sahilmgandhi 26:d20f1adac2d3 650 } else if (currDir % 4 == 2) {
sahilmgandhi 26:d20f1adac2d3 651 if (mouseX - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 652 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 653 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 654 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 655 }
sahilmgandhi 26:d20f1adac2d3 656 }
sahilmgandhi 26:d20f1adac2d3 657 if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 658 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 659 minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 660 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 661 }
sahilmgandhi 26:d20f1adac2d3 662 }
sahilmgandhi 26:d20f1adac2d3 663 if (mouseY - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 664 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 665 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 666 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 667 }
sahilmgandhi 26:d20f1adac2d3 668 }
sahilmgandhi 26:d20f1adac2d3 669 if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 670 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 671 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 672 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 673 }
sahilmgandhi 26:d20f1adac2d3 674 }
sahilmgandhi 26:d20f1adac2d3 675 } else if (currDir % 4 == 1) {
sahilmgandhi 26:d20f1adac2d3 676 if (mouseY - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 677 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 678 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 679 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 680 }
sahilmgandhi 26:d20f1adac2d3 681 }
sahilmgandhi 26:d20f1adac2d3 682 if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 683 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 684 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 685 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 686 }
sahilmgandhi 26:d20f1adac2d3 687 }
sahilmgandhi 26:d20f1adac2d3 688 if (mouseX + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 689 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 690 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 691 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 692 }
sahilmgandhi 26:d20f1adac2d3 693 }
sahilmgandhi 26:d20f1adac2d3 694 if (mouseX - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 695 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 696 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 697 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 698 }
sahilmgandhi 26:d20f1adac2d3 699 }
sahilmgandhi 26:d20f1adac2d3 700 } else if (currDir % 4 == 3) {
sahilmgandhi 26:d20f1adac2d3 701 if (mouseY + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 702 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 703 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 704 dirToGo = 1;
sahilmgandhi 26:d20f1adac2d3 705 }
sahilmgandhi 26:d20f1adac2d3 706 }
sahilmgandhi 26:d20f1adac2d3 707 if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 708 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 709 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
sahilmgandhi 26:d20f1adac2d3 710 dirToGo = 2;
sahilmgandhi 26:d20f1adac2d3 711 }
sahilmgandhi 26:d20f1adac2d3 712 }
sahilmgandhi 26:d20f1adac2d3 713 if (mouseX - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 714 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 715 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
sahilmgandhi 26:d20f1adac2d3 716 dirToGo = 3;
sahilmgandhi 26:d20f1adac2d3 717 }
sahilmgandhi 26:d20f1adac2d3 718 }
sahilmgandhi 26:d20f1adac2d3 719 if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
sahilmgandhi 26:d20f1adac2d3 720 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
sahilmgandhi 26:d20f1adac2d3 721 minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
sahilmgandhi 26:d20f1adac2d3 722 dirToGo = 4;
sahilmgandhi 26:d20f1adac2d3 723 }
sahilmgandhi 26:d20f1adac2d3 724 }
sahilmgandhi 26:d20f1adac2d3 725 }
sahilmgandhi 31:9b71b44e0867 726 } else {
sahilmgandhi 26:d20f1adac2d3 727 justTurned = false;
sahilmgandhi 26:d20f1adac2d3 728 dirToGo = 0;
sahilmgandhi 26:d20f1adac2d3 729 }
sahilmgandhi 26:d20f1adac2d3 730
sahilmgandhi 26:d20f1adac2d3 731 return dirToGo;
sahilmgandhi 26:d20f1adac2d3 732 }
sahilmgandhi 26:d20f1adac2d3 733
sahilmgandhi 31:9b71b44e0867 734 bool hasVisited(int x, int y)
sahilmgandhi 31:9b71b44e0867 735 {
sahilmgandhi 26:d20f1adac2d3 736 return visitedCells[MAZE_LEN - 1 - y][x];
sahilmgandhi 26:d20f1adac2d3 737 }
sahilmgandhi 26:d20f1adac2d3 738
sahilmgandhi 31:9b71b44e0867 739 void changeManhattanDistance(bool headCenter)
sahilmgandhi 31:9b71b44e0867 740 {
sahilmgandhi 31:9b71b44e0867 741 if (headCenter) {
sahilmgandhi 29:ec2c5a69acd6 742 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 743 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 744 distanceToCenter[i][j] = manhattanDistances[i][j];
sahilmgandhi 26:d20f1adac2d3 745 }
sahilmgandhi 26:d20f1adac2d3 746 }
sahilmgandhi 29:ec2c5a69acd6 747 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 748 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 749 manhattanDistances[i][j] = distanceToStart[i][j];
sahilmgandhi 26:d20f1adac2d3 750 }
sahilmgandhi 26:d20f1adac2d3 751 }
sahilmgandhi 31:9b71b44e0867 752 } else {
sahilmgandhi 31:9b71b44e0867 753 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 754 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 755 distanceToStart[i][j] = manhattanDistances[i][j];
sahilmgandhi 29:ec2c5a69acd6 756 }
sahilmgandhi 29:ec2c5a69acd6 757 }
sahilmgandhi 29:ec2c5a69acd6 758 for (int i = 0; i < MAZE_LEN; i++) {
sahilmgandhi 29:ec2c5a69acd6 759 for (int j = 0; j < MAZE_LEN; j++) {
sahilmgandhi 29:ec2c5a69acd6 760 manhattanDistances[i][j] = distanceToCenter[i][j];
sahilmgandhi 29:ec2c5a69acd6 761 }
sahilmgandhi 29:ec2c5a69acd6 762 }
sahilmgandhi 29:ec2c5a69acd6 763 }
sahilmgandhi 29:ec2c5a69acd6 764 }
sahilmgandhi 29:ec2c5a69acd6 765
sahilmgandhi 31:9b71b44e0867 766 void initializeDistances()
sahilmgandhi 31:9b71b44e0867 767 {
sahilmgandhi 29:ec2c5a69acd6 768 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 31:9b71b44e0867 769 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 31:9b71b44e0867 770 distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 771 }
sahilmgandhi 31:9b71b44e0867 772 }
sahilmgandhi 31:9b71b44e0867 773 for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
sahilmgandhi 31:9b71b44e0867 774 for (int j = 0; j < MAZE_LEN / 2; j++) {
sahilmgandhi 31:9b71b44e0867 775 distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
sahilmgandhi 26:d20f1adac2d3 776 }
sahilmgandhi 31:9b71b44e0867 777 }
sahilmgandhi 31:9b71b44e0867 778 for (int i = 0; i < MAZE_LEN / 2; i++) {
sahilmgandhi 31:9b71b44e0867 779 for (int j = MAZE_LEN / 2; j < MAZE_LEN; 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 = MAZE_LEN / 2; j < MAZE_LEN; 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 26:d20f1adac2d3 788 }
sahilmgandhi 31:9b71b44e0867 789
sahilmgandhi 0:a03c771ab78e 790 int main()
sahilmgandhi 0:a03c771ab78e 791 {
christine222 3:880f15be8c72 792 //Set highest bandwidth.
christine222 23:690b0ca34ee9 793 //gyro.setLpBandwidth(LPFBW_42HZ);
sahilmgandhi 29:ec2c5a69acd6 794 initializeDistances();
christine222 3:880f15be8c72 795 serial.baud(9600);
christine222 23:690b0ca34ee9 796 //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
sahilmgandhi 31:9b71b44e0867 797
sahilmgandhi 1:8a4b2f573923 798 wait (0.1);
sahilmgandhi 31:9b71b44e0867 799
sahilmgandhi 2:771db996cee0 800 redLed.write(1);
sahilmgandhi 14:9e7bb03ddccb 801 greenLed.write(0);
sahilmgandhi 2:771db996cee0 802 blueLed.write(1);
sahilmgandhi 31:9b71b44e0867 803
kyleliangus 9:1d8e4da058cd 804 //left_motor.forward(0.1);
kyleliangus 9:1d8e4da058cd 805 //right_motor.forward(0.1);
sahilmgandhi 31:9b71b44e0867 806
kyleliangus 8:a0760acdc59e 807 // PA_1 is A of right
kyleliangus 8:a0760acdc59e 808 // PA_0 is B of right
kyleliangus 8:a0760acdc59e 809 // PA_5 is A of left
kyleliangus 8:a0760acdc59e 810 // PB_3 is B of left
vanshg 11:8fc2b703086b 811 //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 26:d20f1adac2d3 812 // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
sahilmgandhi 31:9b71b44e0867 813
vanshg 10:810d1849da9d 814 // TODO: Setting all the registers and what not for Quadrature Encoders
sahilmgandhi 14:9e7bb03ddccb 815 /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
sahilmgandhi 14:9e7bb03ddccb 816 RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
sahilmgandhi 14:9e7bb03ddccb 817 GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
sahilmgandhi 14:9e7bb03ddccb 818 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
sahilmgandhi 14:9e7bb03ddccb 819 */
sahilmgandhi 31:9b71b44e0867 820
kyleliangus 12:5790e56a056f 821 // 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 822 // of alternate function specified
kyleliangus 12:5790e56a056f 823 // 4 modes sets AHB1ENR
kyleliangus 12:5790e56a056f 824 // Now TMR: enable clock with timer, APB1ENR
kyleliangus 12:5790e56a056f 825 // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
kyleliangus 12:5790e56a056f 826 //
kyleliangus 12:5790e56a056f 827 // Encoder mode: disable timer before changing timer to encoder
kyleliangus 12:5790e56a056f 828 // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
kyleliangus 12:5790e56a056f 829 // CCMR sets sample rate and set the channel to input
kyleliangus 12:5790e56a056f 830 // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
kyleliangus 12:5790e56a056f 831 // SMCR - encoder mode
kyleliangus 12:5790e56a056f 832 // CR1 reenabales
kyleliangus 12:5790e56a056f 833 // then read CNT reg of timer
sahilmgandhi 31:9b71b44e0867 834
sahilmgandhi 31:9b71b44e0867 835
vanshg 10:810d1849da9d 836 dipButton1.rise(&enableButton1);
vanshg 10:810d1849da9d 837 dipButton2.rise(&enableButton2);
vanshg 10:810d1849da9d 838 dipButton3.rise(&enableButton3);
vanshg 10:810d1849da9d 839 dipButton4.rise(&enableButton4);
sahilmgandhi 31:9b71b44e0867 840
vanshg 10:810d1849da9d 841 dipButton1.fall(&disableButton1);
vanshg 10:810d1849da9d 842 dipButton2.fall(&disableButton2);
vanshg 10:810d1849da9d 843 dipButton3.fall(&disableButton3);
vanshg 10:810d1849da9d 844 dipButton4.fall(&disableButton4);
sahilmgandhi 7:6f5cb6377bd4 845
sahilmgandhi 26:d20f1adac2d3 846 // if(dipFlags == 0x1){
sahilmgandhi 26:d20f1adac2d3 847 // }else{
sahilmgandhi 26:d20f1adac2d3 848 // turnRight();
sahilmgandhi 26:d20f1adac2d3 849 // IRP_1.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 850 // IRP_4.calibrateSensor();
sahilmgandhi 26:d20f1adac2d3 851 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
sahilmgandhi 26:d20f1adac2d3 852 // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
sahilmgandhi 26:d20f1adac2d3 853
sahilmgandhi 26:d20f1adac2d3 854 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 855 // turnLeft();
sahilmgandhi 26:d20f1adac2d3 856 // wait_ms(300);
sahilmgandhi 26:d20f1adac2d3 857 // }
sahilmgandhi 26:d20f1adac2d3 858
sahilmgandhi 26:d20f1adac2d3 859
sahilmgandhi 26:d20f1adac2d3 860 // init the wall, and mouse loc arrays:
sahilmgandhi 26:d20f1adac2d3 861 wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
sahilmgandhi 26:d20f1adac2d3 862 visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
sahilmgandhi 26:d20f1adac2d3 863
sahilmgandhi 28:8126a4d620e8 864 int prevEncoder0Count = 0;
sahilmgandhi 28:8126a4d620e8 865 int prevEncoder1Count = 0;
sahilmgandhi 28:8126a4d620e8 866 int currEncoder0Count = 0;
sahilmgandhi 28:8126a4d620e8 867 int currEncoder1Count = 0;
sahilmgandhi 28:8126a4d620e8 868
sahilmgandhi 28:8126a4d620e8 869 bool overrideFloodFill = false;
christine222 25:f827a8b7880e 870 //right_motor.forward( 0.2 );
christine222 25:f827a8b7880e 871 //left_motor.forward( 0.2 );
sahilmgandhi 26:d20f1adac2d3 872 //turnRight180();
sahilmgandhi 26:d20f1adac2d3 873 //wait_ms(1500);
sahilmgandhi 26:d20f1adac2d3 874 int nextMovement = 0;
sahilmgandhi 26:d20f1adac2d3 875 while (1) {
sahilmgandhi 31:9b71b44e0867 876 // prevEncoder0Count = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 877 // prevEncoder1Count = encoder1.getPulses();
sahilmgandhi 29:ec2c5a69acd6 878 //
sahilmgandhi 29:ec2c5a69acd6 879 // if (!overrideFloodFill){
sahilmgandhi 29:ec2c5a69acd6 880 // nextMovement = chooseNextMovement();
sahilmgandhi 29:ec2c5a69acd6 881 // if (nextMovement == 0){
sahilmgandhi 29:ec2c5a69acd6 882 // nCellEncoderAndIR(1);
sahilmgandhi 29:ec2c5a69acd6 883 // }
sahilmgandhi 29:ec2c5a69acd6 884 // else if (nextMovement == 1){
sahilmgandhi 29:ec2c5a69acd6 885 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 886 // turnRight();
sahilmgandhi 29:ec2c5a69acd6 887 // }
sahilmgandhi 29:ec2c5a69acd6 888 // else if (nextMovement == 2){
sahilmgandhi 29:ec2c5a69acd6 889 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 890 // turnLeft();
sahilmgandhi 29:ec2c5a69acd6 891 // }
sahilmgandhi 29:ec2c5a69acd6 892 // else if (nextMovement == 3){
sahilmgandhi 29:ec2c5a69acd6 893 // nCellEncoderAndIR(1);
sahilmgandhi 29:ec2c5a69acd6 894 // }
sahilmgandhi 29:ec2c5a69acd6 895 // else if (nextMovement == 4){
sahilmgandhi 29:ec2c5a69acd6 896 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 897 // turnRight180();
sahilmgandhi 29:ec2c5a69acd6 898 // }
sahilmgandhi 29:ec2c5a69acd6 899 // }
sahilmgandhi 29:ec2c5a69acd6 900 // else{
sahilmgandhi 29:ec2c5a69acd6 901 // overrideFloodFill = false;
sahilmgandhi 29:ec2c5a69acd6 902 // if ((rand()%2 + 1) == 1){
sahilmgandhi 29:ec2c5a69acd6 903 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 904 // turnLeft();
sahilmgandhi 29:ec2c5a69acd6 905 // }
sahilmgandhi 29:ec2c5a69acd6 906 // else{
sahilmgandhi 29:ec2c5a69acd6 907 // justTurned = true;
sahilmgandhi 29:ec2c5a69acd6 908 // turnRight();
sahilmgandhi 29:ec2c5a69acd6 909 // }
sahilmgandhi 29:ec2c5a69acd6 910 // }
sahilmgandhi 29:ec2c5a69acd6 911 // currEncoder0Count = encoder0.getPulses();
sahilmgandhi 29:ec2c5a69acd6 912 // currEncoder1Count = encoder1.getPulses();
sahilmgandhi 31:9b71b44e0867 913 //
sahilmgandhi 29:ec2c5a69acd6 914 // if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
sahilmgandhi 29:ec2c5a69acd6 915 // overrideFloodFill = true;
sahilmgandhi 29:ec2c5a69acd6 916 // }
sahilmgandhi 29:ec2c5a69acd6 917 //
sahilmgandhi 29:ec2c5a69acd6 918 // wait_ms(300); // reduce this once we fine tune this!
christine222 24:e7063765d6f0 919
christine222 23:690b0ca34ee9 920 //wait_ms(1500);
christine222 23:690b0ca34ee9 921 //turnRight();
christine222 23:690b0ca34ee9 922 //wait_ms(1500);
christine222 23:690b0ca34ee9 923 //turnLeft();
sahilmgandhi 26:d20f1adac2d3 924 // nCellEncoderAndIR(1);
sahilmgandhi 26:d20f1adac2d3 925 // wait_ms(500);
christine222 23:690b0ca34ee9 926 // turnRight();
christine222 23:690b0ca34ee9 927 // wait_ms(500);
christine222 23:690b0ca34ee9 928 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 929 // wait_ms(500);
christine222 23:690b0ca34ee9 930 // turnRight();
christine222 23:690b0ca34ee9 931 // wait_ms(500);
christine222 23:690b0ca34ee9 932 // nCellEncoderAndIR(1);
christine222 23:690b0ca34ee9 933 // wait_ms(500);
christine222 23:690b0ca34ee9 934 // turnLeft();
christine222 23:690b0ca34ee9 935 // wait_ms(500);
christine222 23:690b0ca34ee9 936 // nCellEncoderAndIR(2);
christine222 23:690b0ca34ee9 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(5);
christine222 23:690b0ca34ee9 945 // break;
christine222 23:690b0ca34ee9 946 // turnRight180();
sahilmgandhi 31:9b71b44e0867 947
christine222 23:690b0ca34ee9 948 // int number = rand() % 4 + 1;
christine222 23:690b0ca34ee9 949 // switch(number){
christine222 23:690b0ca34ee9 950 // case(1):{//turn right
christine222 23:690b0ca34ee9 951 // turnRight();
christine222 23:690b0ca34ee9 952 // break;
christine222 23:690b0ca34ee9 953 // }
christine222 23:690b0ca34ee9 954 // case(2):{ // turn left
christine222 23:690b0ca34ee9 955 // turnLeft();
christine222 23:690b0ca34ee9 956 // break;
christine222 23:690b0ca34ee9 957 // }
christine222 23:690b0ca34ee9 958 // case(3):{// keep going
sahilmgandhi 31:9b71b44e0867 959
christine222 23:690b0ca34ee9 960 // break;
christine222 23:690b0ca34ee9 961 // }
christine222 23:690b0ca34ee9 962 // case(4):{// turnaround
christine222 23:690b0ca34ee9 963 // turnRight180();
christine222 23:690b0ca34ee9 964 // break;
christine222 23:690b0ca34ee9 965 // }
christine222 23:690b0ca34ee9 966 // default:{// keep going
christine222 23:690b0ca34ee9 967 // break;
christine222 23:690b0ca34ee9 968 // }
christine222 23:690b0ca34ee9 969 // }
sahilmgandhi 31:9b71b44e0867 970
christine222 23:690b0ca34ee9 971 // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
christine222 23:690b0ca34ee9 972 // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
sahilmgandhi 31:9b71b44e0867 973
christine222 23:690b0ca34ee9 974 // float ir3 = IRP_2.getSamples(100)/initAverageL;
christine222 23:690b0ca34ee9 975 // float ir2 = IRP_3.getSamples(100)/initAverageR;
sahilmgandhi 31:9b71b44e0867 976
sahilmgandhi 31:9b71b44e0867 977
christine222 25:f827a8b7880e 978
christine222 23:690b0ca34ee9 979 /*
christine222 23:690b0ca34ee9 980 counter2++;
sahilmgandhi 31:9b71b44e0867 981 counter3++;
christine222 23:690b0ca34ee9 982 ir2tot += IRP_2.getSamples(100);
christine222 23:690b0ca34ee9 983 ir3tot += IRP_3.getSamples(100);
sahilmgandhi 31:9b71b44e0867 984
sahilmgandhi 31:9b71b44e0867 985
christine222 23:690b0ca34ee9 986 ir2 = ir2tot/counter2;
christine222 23:690b0ca34ee9 987 ir3 = ir3tot/counter3;
sahilmgandhi 31:9b71b44e0867 988
sahilmgandhi 31:9b71b44e0867 989
christine222 23:690b0ca34ee9 990 serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 991 */
christine222 23:690b0ca34ee9 992 //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
christine222 23:690b0ca34ee9 993 //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3);
christine222 23:690b0ca34ee9 994 //break;
christine222 23:690b0ca34ee9 995 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
christine222 23:690b0ca34ee9 996 //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
christine222 23:690b0ca34ee9 997 //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
sahilmgandhi 31:9b71b44e0867 998
sahilmgandhi 31:9b71b44e0867 999
christine222 23:690b0ca34ee9 1000 ////////////////////////////////////////////////////////////////
sahilmgandhi 31:9b71b44e0867 1001
christine222 22:681190ff98f0 1002 //nCellEncoderAndIR(3);
christine222 22:681190ff98f0 1003 //break;
sahilmgandhi 31:9b71b44e0867 1004
christine222 23:690b0ca34ee9 1005 //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 1006
christine222 21:9a6cb07bdcb6 1007 //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
sahilmgandhi 31:9b71b44e0867 1008
christine222 21:9a6cb07bdcb6 1009 //break;
christine222 21:9a6cb07bdcb6 1010 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1011 // wait(0.5);
christine222 21:9a6cb07bdcb6 1012 // handleTurns();
christine222 21:9a6cb07bdcb6 1013 // wait(0.5);
christine222 21:9a6cb07bdcb6 1014 // moveForwardCellEncoder(1);
christine222 21:9a6cb07bdcb6 1015 // wait(0.5);
christine222 21:9a6cb07bdcb6 1016 // handleTurns();
christine222 21:9a6cb07bdcb6 1017 //break;
sahilmgandhi 16:d9252437bd92 1018 //pidOnEncoders();
sahilmgandhi 31:9b71b44e0867 1019 // moveForwardUntilWallIr();
kyleliangus 15:b80555a4a8b9 1020 //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses());
sahilmgandhi 31:9b71b44e0867 1021 double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
sahilmgandhi 29:ec2c5a69acd6 1022 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 1023
christine222 3:880f15be8c72 1024 //reading = Rec_4.read();
christine222 3:880f15be8c72 1025 // serial.printf("reading: %f\n", reading);
christine222 3:880f15be8c72 1026 }
sahilmgandhi 26:d20f1adac2d3 1027 }