MacroRat / MouseCode

Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

main.cpp@29:ec2c5a69acd6, 2017-05-24 (annotated)

Committer:
sahilmgandhi
Date:
Wed May 24 01:57:01 2017 +0000
Revision:
29:ec2c5a69acd6
Parent:
28:8126a4d620e8
Child:
31:9b71b44e0867
Need to change ir2-ir3 to now be ir1 - ir4

Who changed what in which revision?

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