Sungwoo Han / Mbed 2 deprecated 4180_Final_MasterWithBT_Fin

ece 4180 final project master mbed code

Dependencies:   4DGL-uLCD-SE HC_SR04_Ultrasonic_Library mbed

main.cpp@0:92d0f9e69c15, 2016-12-09 (annotated)

Committer:
ihansw
Date:
Fri Dec 09 15:32:51 2016 +0000
Revision:
0:92d0f9e69c15
ece 4180 final project master mbed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ihansw 0:92d0f9e69c15 1 #include "mbed.h"
ihansw 0:92d0f9e69c15 2 #include "ultrasonic.h"
ihansw 0:92d0f9e69c15 3 #include "uLCD_4DGL.h"//uLCD class
ihansw 0:92d0f9e69c15 4 //BusOut myled(LED1,LED2,LED3,LED4);
ihansw 0:92d0f9e69c15 5 DigitalOut led1(LED1);
ihansw 0:92d0f9e69c15 6 DigitalOut led2(LED2);
ihansw 0:92d0f9e69c15 7 DigitalOut led3(LED3);
ihansw 0:92d0f9e69c15 8 DigitalOut led4(LED4);
ihansw 0:92d0f9e69c15 9
ihansw 0:92d0f9e69c15 10 //uLCD_4DGL uLCD(p9, p10, p7); // tx rx reset create a global lcd object
ihansw 0:92d0f9e69c15 11
ihansw 0:92d0f9e69c15 12
ihansw 0:92d0f9e69c15 13 Serial blue(p28,p27);//tx rx for bluetooth
ihansw 0:92d0f9e69c15 14
ihansw 0:92d0f9e69c15 15 Serial uart(p9,p10);//Sungwoo
ihansw 0:92d0f9e69c15 16
ihansw 0:92d0f9e69c15 17
ihansw 0:92d0f9e69c15 18 ////////////////////////////////////////////////
ihansw 0:92d0f9e69c15 19
ihansw 0:92d0f9e69c15 20 volatile int distan0 = 0;//front
ihansw 0:92d0f9e69c15 21 volatile int distan1 = 0;//left
ihansw 0:92d0f9e69c15 22 volatile int distan2r = 0;//back right
ihansw 0:92d0f9e69c15 23 volatile int distan2l = 0;//back left
ihansw 0:92d0f9e69c15 24 volatile int distan3 = 0;//right
ihansw 0:92d0f9e69c15 25 volatile int distan1b = 0;//left2
ihansw 0:92d0f9e69c15 26 volatile int distan3b = 0;//right2
ihansw 0:92d0f9e69c15 27
ihansw 0:92d0f9e69c15 28 //volatile char s[1];
ihansw 0:92d0f9e69c15 29
ihansw 0:92d0f9e69c15 30 void dist0(int distance0)
ihansw 0:92d0f9e69c15 31 {
ihansw 0:92d0f9e69c15 32 //put code here to execute when the distance has changed
ihansw 0:92d0f9e69c15 33 //uLCD.locate(0,1);
ihansw 0:92d0f9e69c15 34 //uLCD.printf("DistanceFront %d \r\n", distance0);
ihansw 0:92d0f9e69c15 35 distan0 = distance0;
ihansw 0:92d0f9e69c15 36 }
ihansw 0:92d0f9e69c15 37 void dist1(int distance1)
ihansw 0:92d0f9e69c15 38 {
ihansw 0:92d0f9e69c15 39 //uLCD.locate(0,3);
ihansw 0:92d0f9e69c15 40 //uLCD.printf("DistanceLeft %d \r\n", distance1);
ihansw 0:92d0f9e69c15 41 distan1 = distance1;
ihansw 0:92d0f9e69c15 42 }
ihansw 0:92d0f9e69c15 43
ihansw 0:92d0f9e69c15 44 void dist1b(int distance1b)
ihansw 0:92d0f9e69c15 45 {
ihansw 0:92d0f9e69c15 46 //uLCD.locate(0,5);
ihansw 0:92d0f9e69c15 47 //uLCD.printf("DistanceLeftB %d \r\n", distance1b);
ihansw 0:92d0f9e69c15 48 distan1b = distance1b;
ihansw 0:92d0f9e69c15 49 }
ihansw 0:92d0f9e69c15 50 void dist2r(int distance2r)
ihansw 0:92d0f9e69c15 51 {
ihansw 0:92d0f9e69c15 52 //uLCD.locate(0,7);
ihansw 0:92d0f9e69c15 53 //uLCD.printf("DisatnceBack %d \r\n", distance2);
ihansw 0:92d0f9e69c15 54 distan2r = distance2r;
ihansw 0:92d0f9e69c15 55
ihansw 0:92d0f9e69c15 56 }
ihansw 0:92d0f9e69c15 57 void dist2l(int distance2l)
ihansw 0:92d0f9e69c15 58 {
ihansw 0:92d0f9e69c15 59 distan2l = distance2l;
ihansw 0:92d0f9e69c15 60 }
ihansw 0:92d0f9e69c15 61 void dist3(int distance3)
ihansw 0:92d0f9e69c15 62 {
ihansw 0:92d0f9e69c15 63 //uLCD.locate(0,9);
ihansw 0:92d0f9e69c15 64 //uLCD.printf("DistanceRight %d \r\n", distance3);
ihansw 0:92d0f9e69c15 65 distan3 = distance3;
ihansw 0:92d0f9e69c15 66 }
ihansw 0:92d0f9e69c15 67
ihansw 0:92d0f9e69c15 68 void dist3b(int distance3b)
ihansw 0:92d0f9e69c15 69 {
ihansw 0:92d0f9e69c15 70 //uLCD.locate(0,11);
ihansw 0:92d0f9e69c15 71 //uLCD.printf("DistanceRightB %d \r\n", distance3b);
ihansw 0:92d0f9e69c15 72 distan3b = distance3b;
ihansw 0:92d0f9e69c15 73 }
ihansw 0:92d0f9e69c15 74 ultrasonic front(p8, p11, .1, .5, &dist0);//trig echo
ihansw 0:92d0f9e69c15 75 //have updates every .1 seconds and a timeout after 1
ihansw 0:92d0f9e69c15 76 //second, and call dist when the distance changes
ihansw 0:92d0f9e69c15 77 ultrasonic left1(p25, p26, .1, .5, &dist1);
ihansw 0:92d0f9e69c15 78 ultrasonic left2(p12, p13, .1, .5, &dist1b);
ihansw 0:92d0f9e69c15 79 ultrasonic backl(p21, p22, .1, .5, &dist2l);
ihansw 0:92d0f9e69c15 80 ultrasonic backr(p5, p6, .1, .5, &dist2r);
ihansw 0:92d0f9e69c15 81 ultrasonic right1(p29, p30, .1, .5, &dist3);
ihansw 0:92d0f9e69c15 82 ultrasonic right2(p23, p24, .1, .5, &dist3b);
ihansw 0:92d0f9e69c15 83
ihansw 0:92d0f9e69c15 84 ///////////////////////////////////////////////
ihansw 0:92d0f9e69c15 85
ihansw 0:92d0f9e69c15 86 int main()
ihansw 0:92d0f9e69c15 87 {
ihansw 0:92d0f9e69c15 88 char bnum=0;
ihansw 0:92d0f9e69c15 89 char bhit=0;
ihansw 0:92d0f9e69c15 90
ihansw 0:92d0f9e69c15 91 //Sungwoo
ihansw 0:92d0f9e69c15 92 uart.baud(9600);
ihansw 0:92d0f9e69c15 93
ihansw 0:92d0f9e69c15 94 char f = 'f'; // Move forward
ihansw 0:92d0f9e69c15 95 char b = 'b'; // Move Backward
ihansw 0:92d0f9e69c15 96 char l = 'l'; // Turn Left
ihansw 0:92d0f9e69c15 97 char r = 'r'; // Turn Right
ihansw 0:92d0f9e69c15 98 char w = 'w'; // Park on the left
ihansw 0:92d0f9e69c15 99 char x = 'x'; // Pakk on the right
ihansw 0:92d0f9e69c15 100
ihansw 0:92d0f9e69c15 101 char a = 'a'; // tilt to the left
ihansw 0:92d0f9e69c15 102 char z = 'z'; // tilt to the right
ihansw 0:92d0f9e69c15 103 char p = 'p'; // Parked
ihansw 0:92d0f9e69c15 104
ihansw 0:92d0f9e69c15 105 char g = 'g'; // tilt angle to the left (CCW)
ihansw 0:92d0f9e69c15 106 char h = 'h'; // tilt angle to the right (CW)
ihansw 0:92d0f9e69c15 107
ihansw 0:92d0f9e69c15 108
ihansw 0:92d0f9e69c15 109 ////////////////////////////////
ihansw 0:92d0f9e69c15 110
ihansw 0:92d0f9e69c15 111 //bool variables for each sensor
ihansw 0:92d0f9e69c15 112 bool fSensor = false;
ihansw 0:92d0f9e69c15 113 bool r1Sensor = false;
ihansw 0:92d0f9e69c15 114 bool r2Sensor = false;
ihansw 0:92d0f9e69c15 115 bool brSensor = false;
ihansw 0:92d0f9e69c15 116 bool blSensor = false;
ihansw 0:92d0f9e69c15 117 bool l1Sensor = false;
ihansw 0:92d0f9e69c15 118 bool l2Sensor = false;
ihansw 0:92d0f9e69c15 119
ihansw 0:92d0f9e69c15 120
ihansw 0:92d0f9e69c15 121 //char bnum=0;
ihansw 0:92d0f9e69c15 122 //char bhit=0;
ihansw 0:92d0f9e69c15 123
ihansw 0:92d0f9e69c15 124
ihansw 0:92d0f9e69c15 125
ihansw 0:92d0f9e69c15 126 //Front - 0, Left - 1, Back - 2, Right - 3
ihansw 0:92d0f9e69c15 127 int diffLR = 0;
ihansw 0:92d0f9e69c15 128 int diffLR2 = 0;
ihansw 0:92d0f9e69c15 129 //uLCD.printf("Hey");
ihansw 0:92d0f9e69c15 130
ihansw 0:92d0f9e69c15 131
ihansw 0:92d0f9e69c15 132 /////////////////////////////////////
ihansw 0:92d0f9e69c15 133
ihansw 0:92d0f9e69c15 134 while(1) {
ihansw 0:92d0f9e69c15 135 if (blue.getc()=='!') {
ihansw 0:92d0f9e69c15 136 if (blue.getc()=='B') { //button data packet
ihansw 0:92d0f9e69c15 137 bnum = blue.getc(); //button number
ihansw 0:92d0f9e69c15 138 bhit = blue.getc(); //1=hit, 0=release
ihansw 0:92d0f9e69c15 139 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
ihansw 0:92d0f9e69c15 140 //myled = bnum - '0'; //current button number will appear on LEDs
ihansw 0:92d0f9e69c15 141 switch (bnum) {
ihansw 0:92d0f9e69c15 142 case '1': //number button 1
ihansw 0:92d0f9e69c15 143
ihansw 0:92d0f9e69c15 144 if (bhit=='1')
ihansw 0:92d0f9e69c15 145 {
ihansw 0:92d0f9e69c15 146 //uLCD.locate(0,1);
ihansw 0:92d0f9e69c15 147 //uLCD.printf("Parking start");
ihansw 0:92d0f9e69c15 148
ihansw 0:92d0f9e69c15 149 //int distance = 0;
ihansw 0:92d0f9e69c15 150 front.startUpdates();//start measuring the distance
ihansw 0:92d0f9e69c15 151 left1.startUpdates();
ihansw 0:92d0f9e69c15 152 left2.startUpdates();
ihansw 0:92d0f9e69c15 153 backr.startUpdates();
ihansw 0:92d0f9e69c15 154 backl.startUpdates();
ihansw 0:92d0f9e69c15 155 right1.startUpdates();
ihansw 0:92d0f9e69c15 156 right2.startUpdates();
ihansw 0:92d0f9e69c15 157
ihansw 0:92d0f9e69c15 158 //////////////////////////parking #1////////////////////////////////
ihansw 0:92d0f9e69c15 159
ihansw 0:92d0f9e69c15 160
ihansw 0:92d0f9e69c15 161 while(1)
ihansw 0:92d0f9e69c15 162 {
ihansw 0:92d0f9e69c15 163 //Do something else here
ihansw 0:92d0f9e69c15 164 front.checkDistance(); //call checkDistance() as much as possible, as this is where
ihansw 0:92d0f9e69c15 165 //the class checks if dist needs to be called.
ihansw 0:92d0f9e69c15 166 left1.checkDistance();
ihansw 0:92d0f9e69c15 167 left2.checkDistance();
ihansw 0:92d0f9e69c15 168 backr.checkDistance();
ihansw 0:92d0f9e69c15 169 backl.checkDistance();
ihansw 0:92d0f9e69c15 170 right1.checkDistance();
ihansw 0:92d0f9e69c15 171 right2.checkDistance();
ihansw 0:92d0f9e69c15 172 /*uLCD.locate(0,5);
ihansw 0:92d0f9e69c15 173 uLCD.printf("distance0: %d", distan0);
ihansw 0:92d0f9e69c15 174 uLCD.locate(0,6);
ihansw 0:92d0f9e69c15 175 uLCD.printf("distance1: %d", distan1);*/
ihansw 0:92d0f9e69c15 176
ihansw 0:92d0f9e69c15 177 diffLR = abs(distan1 - distan3);
ihansw 0:92d0f9e69c15 178 diffLR2 = abs(distan1b - distan3b);
ihansw 0:92d0f9e69c15 179 // uLCD.locate(0,7);
ihansw 0:92d0f9e69c15 180 // uLCD.printf("diiference 12: %d", diffLR);
ihansw 0:92d0f9e69c15 181
ihansw 0:92d0f9e69c15 182 // #1 - check the front sensor
ihansw 0:92d0f9e69c15 183 if(distan0>300)
ihansw 0:92d0f9e69c15 184 {
ihansw 0:92d0f9e69c15 185 fSensor = true;
ihansw 0:92d0f9e69c15 186 //zig-zag feature in the map by adjusting the distance of left1 and right1 sensors
ihansw 0:92d0f9e69c15 187
ihansw 0:92d0f9e69c15 188 // 'a' - tilt to left.
ihansw 0:92d0f9e69c15 189 if((diffLR<100) && (distan1>distan3))
ihansw 0:92d0f9e69c15 190 {
ihansw 0:92d0f9e69c15 191 //led1 = 0;
ihansw 0:92d0f9e69c15 192 led2 = 1;
ihansw 0:92d0f9e69c15 193 led3 = 0;
ihansw 0:92d0f9e69c15 194 led4 = 0;
ihansw 0:92d0f9e69c15 195
ihansw 0:92d0f9e69c15 196 if(uart.writeable())
ihansw 0:92d0f9e69c15 197 {
ihansw 0:92d0f9e69c15 198 uart.printf("%c",a);
ihansw 0:92d0f9e69c15 199 led3 = 1;
ihansw 0:92d0f9e69c15 200 wait(0.3);
ihansw 0:92d0f9e69c15 201 }
ihansw 0:92d0f9e69c15 202
ihansw 0:92d0f9e69c15 203 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 204 //uLCD.printf("Forward, tilt to left!");
ihansw 0:92d0f9e69c15 205 }
ihansw 0:92d0f9e69c15 206 // 'z' - tilt to right.
ihansw 0:92d0f9e69c15 207 if((diffLR<100) && (distan1<distan3))
ihansw 0:92d0f9e69c15 208 {
ihansw 0:92d0f9e69c15 209 led1 = 0;
ihansw 0:92d0f9e69c15 210 led2 = 0;
ihansw 0:92d0f9e69c15 211 //led3 = 0;
ihansw 0:92d0f9e69c15 212 led4 = 1;
ihansw 0:92d0f9e69c15 213
ihansw 0:92d0f9e69c15 214 if(uart.writeable())
ihansw 0:92d0f9e69c15 215 {
ihansw 0:92d0f9e69c15 216 uart.printf("%c",z);
ihansw 0:92d0f9e69c15 217 led3 = 1;
ihansw 0:92d0f9e69c15 218 wait(0.3);
ihansw 0:92d0f9e69c15 219 }
ihansw 0:92d0f9e69c15 220
ihansw 0:92d0f9e69c15 221
ihansw 0:92d0f9e69c15 222 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 223 //uLCD.printf("Forward, tilt to right!");
ihansw 0:92d0f9e69c15 224 }
ihansw 0:92d0f9e69c15 225
ihansw 0:92d0f9e69c15 226 //feedback with one side (left/right)
ihansw 0:92d0f9e69c15 227 // compare front and back distance and adjust!!!!! // NOT BEEN CODED YET
ihansw 0:92d0f9e69c15 228 if((distan3>300) && (distan3b<100))
ihansw 0:92d0f9e69c15 229 {
ihansw 0:92d0f9e69c15 230 led1 = 1;
ihansw 0:92d0f9e69c15 231 led2 = 0;
ihansw 0:92d0f9e69c15 232 led3 = 1;
ihansw 0:92d0f9e69c15 233 led4 = 1;
ihansw 0:92d0f9e69c15 234
ihansw 0:92d0f9e69c15 235 //move forward
ihansw 0:92d0f9e69c15 236
ihansw 0:92d0f9e69c15 237 if(uart.writeable())
ihansw 0:92d0f9e69c15 238 {
ihansw 0:92d0f9e69c15 239 uart.printf("%c",f);
ihansw 0:92d0f9e69c15 240 //led3 = 1;
ihansw 0:92d0f9e69c15 241 wait(0.3);
ihansw 0:92d0f9e69c15 242 }
ihansw 0:92d0f9e69c15 243
ihansw 0:92d0f9e69c15 244 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 245 //uLCD.printf("Forward, One FB, Left\n");
ihansw 0:92d0f9e69c15 246 // SAME AS ABOVE
ihansw 0:92d0f9e69c15 247 }
ihansw 0:92d0f9e69c15 248
ihansw 0:92d0f9e69c15 249 if((distan1>300) && (distan1b<100))
ihansw 0:92d0f9e69c15 250 {
ihansw 0:92d0f9e69c15 251 led1 = 1;
ihansw 0:92d0f9e69c15 252 led2 = 1;
ihansw 0:92d0f9e69c15 253 led3 = 1;
ihansw 0:92d0f9e69c15 254 led4 = 0;
ihansw 0:92d0f9e69c15 255
ihansw 0:92d0f9e69c15 256 if(uart.writeable())
ihansw 0:92d0f9e69c15 257 {
ihansw 0:92d0f9e69c15 258 uart.printf("%c",f);
ihansw 0:92d0f9e69c15 259 wait(0.3);
ihansw 0:92d0f9e69c15 260 //led3 = 1;
ihansw 0:92d0f9e69c15 261 }
ihansw 0:92d0f9e69c15 262
ihansw 0:92d0f9e69c15 263 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 264 //uLCD.printf("Forward, One FB, Right\n");
ihansw 0:92d0f9e69c15 265 }
ihansw 0:92d0f9e69c15 266
ihansw 0:92d0f9e69c15 267
ihansw 0:92d0f9e69c15 268
ihansw 0:92d0f9e69c15 269 ////////////detecting parking spaces on the LEFT SIDE////////////
ihansw 0:92d0f9e69c15 270
ihansw 0:92d0f9e69c15 271 if((distan1>300) && (distan1b>300))
ihansw 0:92d0f9e69c15 272 {
ihansw 0:92d0f9e69c15 273 l1Sensor = true;
ihansw 0:92d0f9e69c15 274 l2Sensor = true;
ihansw 0:92d0f9e69c15 275
ihansw 0:92d0f9e69c15 276 //parking space is on the left side
ihansw 0:92d0f9e69c15 277 led1 = 0;
ihansw 0:92d0f9e69c15 278 led2 = 1;
ihansw 0:92d0f9e69c15 279 led3 = 1;
ihansw 0:92d0f9e69c15 280 led4 = 0;
ihansw 0:92d0f9e69c15 281
ihansw 0:92d0f9e69c15 282
ihansw 0:92d0f9e69c15 283 if(uart.writeable())
ihansw 0:92d0f9e69c15 284 {
ihansw 0:92d0f9e69c15 285
ihansw 0:92d0f9e69c15 286 uart.printf("%c",w);
ihansw 0:92d0f9e69c15 287 wait(3);
ihansw 0:92d0f9e69c15 288 }
ihansw 0:92d0f9e69c15 289
ihansw 0:92d0f9e69c15 290 left1.checkDistance();
ihansw 0:92d0f9e69c15 291 left2.checkDistance();
ihansw 0:92d0f9e69c15 292 backr.checkDistance();
ihansw 0:92d0f9e69c15 293 backl.checkDistance();
ihansw 0:92d0f9e69c15 294 right1.checkDistance();
ihansw 0:92d0f9e69c15 295 right2.checkDistance();
ihansw 0:92d0f9e69c15 296
ihansw 0:92d0f9e69c15 297 wait(2);
ihansw 0:92d0f9e69c15 298
ihansw 0:92d0f9e69c15 299
ihansw 0:92d0f9e69c15 300 while(abs((distan2r)-(distan2l)) >15 )
ihansw 0:92d0f9e69c15 301 {
ihansw 0:92d0f9e69c15 302 left1.checkDistance();
ihansw 0:92d0f9e69c15 303 left2.checkDistance();
ihansw 0:92d0f9e69c15 304 backr.checkDistance();
ihansw 0:92d0f9e69c15 305 backl.checkDistance();
ihansw 0:92d0f9e69c15 306 right1.checkDistance();
ihansw 0:92d0f9e69c15 307 right2.checkDistance();
ihansw 0:92d0f9e69c15 308 wait(1);
ihansw 0:92d0f9e69c15 309
ihansw 0:92d0f9e69c15 310 if(distan2r > distan2l)
ihansw 0:92d0f9e69c15 311 {
ihansw 0:92d0f9e69c15 312 uart.printf("%c",h);
ihansw 0:92d0f9e69c15 313 left1.checkDistance();
ihansw 0:92d0f9e69c15 314 left2.checkDistance();
ihansw 0:92d0f9e69c15 315 backr.checkDistance();
ihansw 0:92d0f9e69c15 316 backl.checkDistance();
ihansw 0:92d0f9e69c15 317 right1.checkDistance();
ihansw 0:92d0f9e69c15 318 right2.checkDistance();
ihansw 0:92d0f9e69c15 319
ihansw 0:92d0f9e69c15 320 wait(1);
ihansw 0:92d0f9e69c15 321 }
ihansw 0:92d0f9e69c15 322
ihansw 0:92d0f9e69c15 323 if(distan2r < distan2l)
ihansw 0:92d0f9e69c15 324 {
ihansw 0:92d0f9e69c15 325 uart.printf("%c",g);
ihansw 0:92d0f9e69c15 326 left1.checkDistance();
ihansw 0:92d0f9e69c15 327 left2.checkDistance();
ihansw 0:92d0f9e69c15 328 backr.checkDistance();
ihansw 0:92d0f9e69c15 329 backl.checkDistance();
ihansw 0:92d0f9e69c15 330 right1.checkDistance();
ihansw 0:92d0f9e69c15 331 right2.checkDistance();
ihansw 0:92d0f9e69c15 332
ihansw 0:92d0f9e69c15 333 wait(1);
ihansw 0:92d0f9e69c15 334 }
ihansw 0:92d0f9e69c15 335
ihansw 0:92d0f9e69c15 336 }
ihansw 0:92d0f9e69c15 337
ihansw 0:92d0f9e69c15 338
ihansw 0:92d0f9e69c15 339 //move forward
ihansw 0:92d0f9e69c15 340 while((distan1 > 200)||(distan3 > 200))
ihansw 0:92d0f9e69c15 341 {
ihansw 0:92d0f9e69c15 342 front.checkDistance();
ihansw 0:92d0f9e69c15 343 left1.checkDistance();
ihansw 0:92d0f9e69c15 344 left2.checkDistance();
ihansw 0:92d0f9e69c15 345 //back.checkDistance();
ihansw 0:92d0f9e69c15 346 right1.checkDistance();
ihansw 0:92d0f9e69c15 347 right2.checkDistance();
ihansw 0:92d0f9e69c15 348
ihansw 0:92d0f9e69c15 349 if((distan1 > 200)||(distan3 > 200))
ihansw 0:92d0f9e69c15 350 {
ihansw 0:92d0f9e69c15 351 uart.printf("%c",f);
ihansw 0:92d0f9e69c15 352 wait(0.3);
ihansw 0:92d0f9e69c15 353 front.checkDistance();
ihansw 0:92d0f9e69c15 354 left1.checkDistance();
ihansw 0:92d0f9e69c15 355 left2.checkDistance();
ihansw 0:92d0f9e69c15 356 //back.checkDistance();
ihansw 0:92d0f9e69c15 357 right1.checkDistance();
ihansw 0:92d0f9e69c15 358 right2.checkDistance();
ihansw 0:92d0f9e69c15 359 }
ihansw 0:92d0f9e69c15 360 }
ihansw 0:92d0f9e69c15 361
ihansw 0:92d0f9e69c15 362 while(distan0>130)
ihansw 0:92d0f9e69c15 363 {
ihansw 0:92d0f9e69c15 364
ihansw 0:92d0f9e69c15 365 front.checkDistance(); //call checkDistance() as much as possible, as this is where
ihansw 0:92d0f9e69c15 366 //the class checks if dist needs to be called.
ihansw 0:92d0f9e69c15 367 left1.checkDistance();
ihansw 0:92d0f9e69c15 368 left2.checkDistance();
ihansw 0:92d0f9e69c15 369 backr.checkDistance();
ihansw 0:92d0f9e69c15 370 backl.checkDistance();
ihansw 0:92d0f9e69c15 371 right1.checkDistance();
ihansw 0:92d0f9e69c15 372 right2.checkDistance();
ihansw 0:92d0f9e69c15 373
ihansw 0:92d0f9e69c15 374 //tilt to the right
ihansw 0:92d0f9e69c15 375 if(distan1>distan3)
ihansw 0:92d0f9e69c15 376 {
ihansw 0:92d0f9e69c15 377 uart.printf("%c",z);
ihansw 0:92d0f9e69c15 378 led3 = 1;
ihansw 0:92d0f9e69c15 379 wait(0.3);
ihansw 0:92d0f9e69c15 380 }
ihansw 0:92d0f9e69c15 381 //tilt to the left
ihansw 0:92d0f9e69c15 382 if(distan1<distan3)
ihansw 0:92d0f9e69c15 383 {
ihansw 0:92d0f9e69c15 384 uart.printf("%c",a);
ihansw 0:92d0f9e69c15 385 led3 = 1;
ihansw 0:92d0f9e69c15 386 wait(0.3);
ihansw 0:92d0f9e69c15 387 }
ihansw 0:92d0f9e69c15 388 front.checkDistance(); //call checkDistance() as much as possible, as this is where
ihansw 0:92d0f9e69c15 389 //the class checks if dist needs to be called.
ihansw 0:92d0f9e69c15 390 left1.checkDistance();
ihansw 0:92d0f9e69c15 391 left2.checkDistance();
ihansw 0:92d0f9e69c15 392 backr.checkDistance();
ihansw 0:92d0f9e69c15 393 backl.checkDistance();
ihansw 0:92d0f9e69c15 394 right1.checkDistance();
ihansw 0:92d0f9e69c15 395 right2.checkDistance();
ihansw 0:92d0f9e69c15 396 }
ihansw 0:92d0f9e69c15 397
ihansw 0:92d0f9e69c15 398 uart.printf("%c",r);
ihansw 0:92d0f9e69c15 399
ihansw 0:92d0f9e69c15 400
ihansw 0:92d0f9e69c15 401
ihansw 0:92d0f9e69c15 402 left1.checkDistance();
ihansw 0:92d0f9e69c15 403 left2.checkDistance();
ihansw 0:92d0f9e69c15 404 backr.checkDistance();
ihansw 0:92d0f9e69c15 405 backl.checkDistance();
ihansw 0:92d0f9e69c15 406 right1.checkDistance();
ihansw 0:92d0f9e69c15 407 right2.checkDistance();
ihansw 0:92d0f9e69c15 408
ihansw 0:92d0f9e69c15 409 wait(2);
ihansw 0:92d0f9e69c15 410
ihansw 0:92d0f9e69c15 411
ihansw 0:92d0f9e69c15 412 while(abs((distan2r)-(distan2l)) >15 )
ihansw 0:92d0f9e69c15 413 {
ihansw 0:92d0f9e69c15 414 left1.checkDistance();
ihansw 0:92d0f9e69c15 415 left2.checkDistance();
ihansw 0:92d0f9e69c15 416 backr.checkDistance();
ihansw 0:92d0f9e69c15 417 backl.checkDistance();
ihansw 0:92d0f9e69c15 418 right1.checkDistance();
ihansw 0:92d0f9e69c15 419 right2.checkDistance();
ihansw 0:92d0f9e69c15 420 wait(1);
ihansw 0:92d0f9e69c15 421
ihansw 0:92d0f9e69c15 422 if(distan2r > distan2l)
ihansw 0:92d0f9e69c15 423 {
ihansw 0:92d0f9e69c15 424 uart.printf("%c",h);
ihansw 0:92d0f9e69c15 425 left1.checkDistance();
ihansw 0:92d0f9e69c15 426 left2.checkDistance();
ihansw 0:92d0f9e69c15 427 backr.checkDistance();
ihansw 0:92d0f9e69c15 428 backl.checkDistance();
ihansw 0:92d0f9e69c15 429 right1.checkDistance();
ihansw 0:92d0f9e69c15 430 right2.checkDistance();
ihansw 0:92d0f9e69c15 431
ihansw 0:92d0f9e69c15 432 wait(1);
ihansw 0:92d0f9e69c15 433 }
ihansw 0:92d0f9e69c15 434
ihansw 0:92d0f9e69c15 435 if(distan2r < distan2l)
ihansw 0:92d0f9e69c15 436 {
ihansw 0:92d0f9e69c15 437 uart.printf("%c",g);
ihansw 0:92d0f9e69c15 438 left1.checkDistance();
ihansw 0:92d0f9e69c15 439 left2.checkDistance();
ihansw 0:92d0f9e69c15 440 backr.checkDistance();
ihansw 0:92d0f9e69c15 441 backl.checkDistance();
ihansw 0:92d0f9e69c15 442 right1.checkDistance();
ihansw 0:92d0f9e69c15 443 right2.checkDistance();
ihansw 0:92d0f9e69c15 444
ihansw 0:92d0f9e69c15 445 wait(1);
ihansw 0:92d0f9e69c15 446 }
ihansw 0:92d0f9e69c15 447
ihansw 0:92d0f9e69c15 448 }
ihansw 0:92d0f9e69c15 449
ihansw 0:92d0f9e69c15 450
ihansw 0:92d0f9e69c15 451
ihansw 0:92d0f9e69c15 452 uart.printf("%c",p);
ihansw 0:92d0f9e69c15 453 wait(20);
ihansw 0:92d0f9e69c15 454
ihansw 0:92d0f9e69c15 455 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 456 //uLCD.printf("Park, on the left!");
ihansw 0:92d0f9e69c15 457 }
ihansw 0:92d0f9e69c15 458
ihansw 0:92d0f9e69c15 459 /// /////////parking space is on the right side////////////////////
ihansw 0:92d0f9e69c15 460 if((distan3>300) && (distan3b>300))
ihansw 0:92d0f9e69c15 461 {
ihansw 0:92d0f9e69c15 462 l1Sensor = true;
ihansw 0:92d0f9e69c15 463 l2Sensor = true;
ihansw 0:92d0f9e69c15 464
ihansw 0:92d0f9e69c15 465 //parking space is on the left side
ihansw 0:92d0f9e69c15 466 led1 = 0;
ihansw 0:92d0f9e69c15 467 led2 = 1;
ihansw 0:92d0f9e69c15 468 led3 = 1;
ihansw 0:92d0f9e69c15 469 led4 = 0;
ihansw 0:92d0f9e69c15 470
ihansw 0:92d0f9e69c15 471
ihansw 0:92d0f9e69c15 472 if(uart.writeable())
ihansw 0:92d0f9e69c15 473 {
ihansw 0:92d0f9e69c15 474
ihansw 0:92d0f9e69c15 475 uart.printf("%c",x);
ihansw 0:92d0f9e69c15 476 wait(3);
ihansw 0:92d0f9e69c15 477 }
ihansw 0:92d0f9e69c15 478
ihansw 0:92d0f9e69c15 479 left1.checkDistance();
ihansw 0:92d0f9e69c15 480 left2.checkDistance();
ihansw 0:92d0f9e69c15 481 backr.checkDistance();
ihansw 0:92d0f9e69c15 482 backl.checkDistance();
ihansw 0:92d0f9e69c15 483 right1.checkDistance();
ihansw 0:92d0f9e69c15 484 right2.checkDistance();
ihansw 0:92d0f9e69c15 485
ihansw 0:92d0f9e69c15 486 wait(2);
ihansw 0:92d0f9e69c15 487
ihansw 0:92d0f9e69c15 488
ihansw 0:92d0f9e69c15 489 while(abs((distan2r)-(distan2l)) > 15 )
ihansw 0:92d0f9e69c15 490 {
ihansw 0:92d0f9e69c15 491 left1.checkDistance();
ihansw 0:92d0f9e69c15 492 left2.checkDistance();
ihansw 0:92d0f9e69c15 493 backr.checkDistance();
ihansw 0:92d0f9e69c15 494 backl.checkDistance();
ihansw 0:92d0f9e69c15 495 right1.checkDistance();
ihansw 0:92d0f9e69c15 496 right2.checkDistance();
ihansw 0:92d0f9e69c15 497 wait(1);
ihansw 0:92d0f9e69c15 498
ihansw 0:92d0f9e69c15 499 if(distan2r > distan2l)
ihansw 0:92d0f9e69c15 500 {
ihansw 0:92d0f9e69c15 501 uart.printf("%c",h);
ihansw 0:92d0f9e69c15 502 left1.checkDistance();
ihansw 0:92d0f9e69c15 503 left2.checkDistance();
ihansw 0:92d0f9e69c15 504 backr.checkDistance();
ihansw 0:92d0f9e69c15 505 backl.checkDistance();
ihansw 0:92d0f9e69c15 506 right1.checkDistance();
ihansw 0:92d0f9e69c15 507 right2.checkDistance();
ihansw 0:92d0f9e69c15 508
ihansw 0:92d0f9e69c15 509 wait(1);
ihansw 0:92d0f9e69c15 510 }
ihansw 0:92d0f9e69c15 511
ihansw 0:92d0f9e69c15 512 if(distan2r < distan2l)
ihansw 0:92d0f9e69c15 513 {
ihansw 0:92d0f9e69c15 514 uart.printf("%c",g);
ihansw 0:92d0f9e69c15 515 left1.checkDistance();
ihansw 0:92d0f9e69c15 516 left2.checkDistance();
ihansw 0:92d0f9e69c15 517 backr.checkDistance();
ihansw 0:92d0f9e69c15 518 backl.checkDistance();
ihansw 0:92d0f9e69c15 519 right1.checkDistance();
ihansw 0:92d0f9e69c15 520 right2.checkDistance();
ihansw 0:92d0f9e69c15 521
ihansw 0:92d0f9e69c15 522 wait(1);
ihansw 0:92d0f9e69c15 523 }
ihansw 0:92d0f9e69c15 524
ihansw 0:92d0f9e69c15 525 }
ihansw 0:92d0f9e69c15 526
ihansw 0:92d0f9e69c15 527
ihansw 0:92d0f9e69c15 528 //move forward
ihansw 0:92d0f9e69c15 529 while((distan1 > 200)||(distan3 > 200))
ihansw 0:92d0f9e69c15 530 {
ihansw 0:92d0f9e69c15 531 front.checkDistance();
ihansw 0:92d0f9e69c15 532 left1.checkDistance();
ihansw 0:92d0f9e69c15 533 left2.checkDistance();
ihansw 0:92d0f9e69c15 534 //back.checkDistance();
ihansw 0:92d0f9e69c15 535 right1.checkDistance();
ihansw 0:92d0f9e69c15 536 right2.checkDistance();
ihansw 0:92d0f9e69c15 537
ihansw 0:92d0f9e69c15 538 if((distan1 > 200)||(distan3 > 200))
ihansw 0:92d0f9e69c15 539 {
ihansw 0:92d0f9e69c15 540 uart.printf("%c",f);
ihansw 0:92d0f9e69c15 541 wait(0.3);
ihansw 0:92d0f9e69c15 542 front.checkDistance();
ihansw 0:92d0f9e69c15 543 left1.checkDistance();
ihansw 0:92d0f9e69c15 544 left2.checkDistance();
ihansw 0:92d0f9e69c15 545 //back.checkDistance();
ihansw 0:92d0f9e69c15 546 right1.checkDistance();
ihansw 0:92d0f9e69c15 547 right2.checkDistance();
ihansw 0:92d0f9e69c15 548 }
ihansw 0:92d0f9e69c15 549 }
ihansw 0:92d0f9e69c15 550
ihansw 0:92d0f9e69c15 551 while(distan0>130)
ihansw 0:92d0f9e69c15 552 {
ihansw 0:92d0f9e69c15 553
ihansw 0:92d0f9e69c15 554 front.checkDistance(); //call checkDistance() as much as possible, as this is where
ihansw 0:92d0f9e69c15 555 //the class checks if dist needs to be called.
ihansw 0:92d0f9e69c15 556 left1.checkDistance();
ihansw 0:92d0f9e69c15 557 left2.checkDistance();
ihansw 0:92d0f9e69c15 558 backr.checkDistance();
ihansw 0:92d0f9e69c15 559 backl.checkDistance();
ihansw 0:92d0f9e69c15 560 right1.checkDistance();
ihansw 0:92d0f9e69c15 561 right2.checkDistance();
ihansw 0:92d0f9e69c15 562
ihansw 0:92d0f9e69c15 563 //tilt to the right
ihansw 0:92d0f9e69c15 564 if(distan1>distan3)
ihansw 0:92d0f9e69c15 565 {
ihansw 0:92d0f9e69c15 566 uart.printf("%c",z);
ihansw 0:92d0f9e69c15 567 led3 = 1;
ihansw 0:92d0f9e69c15 568 wait(0.3);
ihansw 0:92d0f9e69c15 569 }
ihansw 0:92d0f9e69c15 570 //tilt to the left
ihansw 0:92d0f9e69c15 571 if(distan1<distan3)
ihansw 0:92d0f9e69c15 572 {
ihansw 0:92d0f9e69c15 573 uart.printf("%c",a);
ihansw 0:92d0f9e69c15 574 led3 = 1;
ihansw 0:92d0f9e69c15 575 wait(0.3);
ihansw 0:92d0f9e69c15 576 }
ihansw 0:92d0f9e69c15 577 front.checkDistance(); //call checkDistance() as much as possible, as this is where
ihansw 0:92d0f9e69c15 578 //the class checks if dist needs to be called.
ihansw 0:92d0f9e69c15 579 left1.checkDistance();
ihansw 0:92d0f9e69c15 580 left2.checkDistance();
ihansw 0:92d0f9e69c15 581 backr.checkDistance();
ihansw 0:92d0f9e69c15 582 backl.checkDistance();
ihansw 0:92d0f9e69c15 583 right1.checkDistance();
ihansw 0:92d0f9e69c15 584 right2.checkDistance();
ihansw 0:92d0f9e69c15 585 }
ihansw 0:92d0f9e69c15 586
ihansw 0:92d0f9e69c15 587 uart.printf("%c",l);
ihansw 0:92d0f9e69c15 588
ihansw 0:92d0f9e69c15 589
ihansw 0:92d0f9e69c15 590
ihansw 0:92d0f9e69c15 591 left1.checkDistance();
ihansw 0:92d0f9e69c15 592 left2.checkDistance();
ihansw 0:92d0f9e69c15 593 backr.checkDistance();
ihansw 0:92d0f9e69c15 594 backl.checkDistance();
ihansw 0:92d0f9e69c15 595 right1.checkDistance();
ihansw 0:92d0f9e69c15 596 right2.checkDistance();
ihansw 0:92d0f9e69c15 597
ihansw 0:92d0f9e69c15 598 wait(2);
ihansw 0:92d0f9e69c15 599
ihansw 0:92d0f9e69c15 600
ihansw 0:92d0f9e69c15 601 while(abs((distan2r)-(distan2l)) >15 )
ihansw 0:92d0f9e69c15 602 {
ihansw 0:92d0f9e69c15 603 left1.checkDistance();
ihansw 0:92d0f9e69c15 604 left2.checkDistance();
ihansw 0:92d0f9e69c15 605 backr.checkDistance();
ihansw 0:92d0f9e69c15 606 backl.checkDistance();
ihansw 0:92d0f9e69c15 607 right1.checkDistance();
ihansw 0:92d0f9e69c15 608 right2.checkDistance();
ihansw 0:92d0f9e69c15 609 wait(1);
ihansw 0:92d0f9e69c15 610
ihansw 0:92d0f9e69c15 611 if(distan2r > distan2l)
ihansw 0:92d0f9e69c15 612 {
ihansw 0:92d0f9e69c15 613 uart.printf("%c",h);
ihansw 0:92d0f9e69c15 614 left1.checkDistance();
ihansw 0:92d0f9e69c15 615 left2.checkDistance();
ihansw 0:92d0f9e69c15 616 backr.checkDistance();
ihansw 0:92d0f9e69c15 617 backl.checkDistance();
ihansw 0:92d0f9e69c15 618 right1.checkDistance();
ihansw 0:92d0f9e69c15 619 right2.checkDistance();
ihansw 0:92d0f9e69c15 620
ihansw 0:92d0f9e69c15 621 wait(1);
ihansw 0:92d0f9e69c15 622 }
ihansw 0:92d0f9e69c15 623
ihansw 0:92d0f9e69c15 624 if(distan2r < distan2l)
ihansw 0:92d0f9e69c15 625 {
ihansw 0:92d0f9e69c15 626 uart.printf("%c",g);
ihansw 0:92d0f9e69c15 627 left1.checkDistance();
ihansw 0:92d0f9e69c15 628 left2.checkDistance();
ihansw 0:92d0f9e69c15 629 backr.checkDistance();
ihansw 0:92d0f9e69c15 630 backl.checkDistance();
ihansw 0:92d0f9e69c15 631 right1.checkDistance();
ihansw 0:92d0f9e69c15 632 right2.checkDistance();
ihansw 0:92d0f9e69c15 633
ihansw 0:92d0f9e69c15 634 wait(1);
ihansw 0:92d0f9e69c15 635 }
ihansw 0:92d0f9e69c15 636
ihansw 0:92d0f9e69c15 637 }
ihansw 0:92d0f9e69c15 638
ihansw 0:92d0f9e69c15 639
ihansw 0:92d0f9e69c15 640
ihansw 0:92d0f9e69c15 641 uart.printf("%c",p);
ihansw 0:92d0f9e69c15 642 wait(20);
ihansw 0:92d0f9e69c15 643
ihansw 0:92d0f9e69c15 644 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 645 //uLCD.printf("Park, on the left!");
ihansw 0:92d0f9e69c15 646 }
ihansw 0:92d0f9e69c15 647 }
ihansw 0:92d0f9e69c15 648 //if(distan0<50)
ihansw 0:92d0f9e69c15 649
ihansw 0:92d0f9e69c15 650 //FRONT SENSOR ON
ihansw 0:92d0f9e69c15 651 else
ihansw 0:92d0f9e69c15 652 {
ihansw 0:92d0f9e69c15 653 //cornering feature
ihansw 0:92d0f9e69c15 654
ihansw 0:92d0f9e69c15 655 ///////////////////////////////////////////////turning left//////////////////////////////////////////////
ihansw 0:92d0f9e69c15 656 if((distan1>300)&&(distan1b>300))
ihansw 0:92d0f9e69c15 657 {
ihansw 0:92d0f9e69c15 658 led1 = 1;
ihansw 0:92d0f9e69c15 659 led2 = 0;
ihansw 0:92d0f9e69c15 660 led3 = 1;
ihansw 0:92d0f9e69c15 661 led4 =1;
ihansw 0:92d0f9e69c15 662
ihansw 0:92d0f9e69c15 663 uart.printf("%c",f);
ihansw 0:92d0f9e69c15 664 wait(.3);
ihansw 0:92d0f9e69c15 665
ihansw 0:92d0f9e69c15 666 if(uart.writeable())
ihansw 0:92d0f9e69c15 667 {
ihansw 0:92d0f9e69c15 668 uart.printf("%c",l);
ihansw 0:92d0f9e69c15 669 wait(2);
ihansw 0:92d0f9e69c15 670 //led3 = 1;
ihansw 0:92d0f9e69c15 671 }
ihansw 0:92d0f9e69c15 672
ihansw 0:92d0f9e69c15 673 left1.checkDistance();
ihansw 0:92d0f9e69c15 674 left2.checkDistance();
ihansw 0:92d0f9e69c15 675 right1.checkDistance();
ihansw 0:92d0f9e69c15 676 right2.checkDistance();
ihansw 0:92d0f9e69c15 677 wait(2);
ihansw 0:92d0f9e69c15 678
ihansw 0:92d0f9e69c15 679 // checking on the left. adjust the angle and make sure it's pointing at the right direction
ihansw 0:92d0f9e69c15 680 while( abs((distan3)-(distan3b)) >15 )
ihansw 0:92d0f9e69c15 681 {
ihansw 0:92d0f9e69c15 682 left1.checkDistance();
ihansw 0:92d0f9e69c15 683 left2.checkDistance();
ihansw 0:92d0f9e69c15 684 right1.checkDistance();
ihansw 0:92d0f9e69c15 685 right2.checkDistance();
ihansw 0:92d0f9e69c15 686
ihansw 0:92d0f9e69c15 687 wait(1);
ihansw 0:92d0f9e69c15 688
ihansw 0:92d0f9e69c15 689 if(distan3 > distan3b)
ihansw 0:92d0f9e69c15 690 {
ihansw 0:92d0f9e69c15 691 uart.printf("%c",h);
ihansw 0:92d0f9e69c15 692 left1.checkDistance();
ihansw 0:92d0f9e69c15 693 left2.checkDistance();
ihansw 0:92d0f9e69c15 694 right1.checkDistance();
ihansw 0:92d0f9e69c15 695 right2.checkDistance();
ihansw 0:92d0f9e69c15 696
ihansw 0:92d0f9e69c15 697 wait(1);
ihansw 0:92d0f9e69c15 698 }
ihansw 0:92d0f9e69c15 699
ihansw 0:92d0f9e69c15 700 if(distan3 <distan3b)
ihansw 0:92d0f9e69c15 701 {
ihansw 0:92d0f9e69c15 702 uart.printf("%c",g);
ihansw 0:92d0f9e69c15 703 left1.checkDistance();
ihansw 0:92d0f9e69c15 704 left2.checkDistance();
ihansw 0:92d0f9e69c15 705 right1.checkDistance();
ihansw 0:92d0f9e69c15 706 right2.checkDistance();
ihansw 0:92d0f9e69c15 707
ihansw 0:92d0f9e69c15 708 wait(1);
ihansw 0:92d0f9e69c15 709 }
ihansw 0:92d0f9e69c15 710
ihansw 0:92d0f9e69c15 711
ihansw 0:92d0f9e69c15 712 }
ihansw 0:92d0f9e69c15 713
ihansw 0:92d0f9e69c15 714 //after turning left, if there are space open on the left, focus on the left wall
ihansw 0:92d0f9e69c15 715 while((distan1>220)||(distan1b>220))
ihansw 0:92d0f9e69c15 716 {
ihansw 0:92d0f9e69c15 717 //front.checkDistance(); //call checkDistance() as much as possible, as this is where
ihansw 0:92d0f9e69c15 718 //the class checks if dist needs to be called.
ihansw 0:92d0f9e69c15 719 left1.checkDistance();
ihansw 0:92d0f9e69c15 720 left2.checkDistance();
ihansw 0:92d0f9e69c15 721 //back.checkDistance();
ihansw 0:92d0f9e69c15 722 right1.checkDistance();
ihansw 0:92d0f9e69c15 723 right2.checkDistance();
ihansw 0:92d0f9e69c15 724
ihansw 0:92d0f9e69c15 725
ihansw 0:92d0f9e69c15 726 if((distan3>distan3b))
ihansw 0:92d0f9e69c15 727 {
ihansw 0:92d0f9e69c15 728 uart.printf("%c",z);
ihansw 0:92d0f9e69c15 729 wait(.3);
ihansw 0:92d0f9e69c15 730 left1.checkDistance();
ihansw 0:92d0f9e69c15 731 left2.checkDistance();
ihansw 0:92d0f9e69c15 732 //back.checkDistance();
ihansw 0:92d0f9e69c15 733 right1.checkDistance();
ihansw 0:92d0f9e69c15 734 right2.checkDistance();
ihansw 0:92d0f9e69c15 735 }
ihansw 0:92d0f9e69c15 736 if((distan3<distan3b))
ihansw 0:92d0f9e69c15 737 {
ihansw 0:92d0f9e69c15 738 uart.printf("%c",a);
ihansw 0:92d0f9e69c15 739 wait(.3);
ihansw 0:92d0f9e69c15 740 left1.checkDistance();
ihansw 0:92d0f9e69c15 741 left2.checkDistance();
ihansw 0:92d0f9e69c15 742 //back.checkDistance();
ihansw 0:92d0f9e69c15 743 right1.checkDistance();
ihansw 0:92d0f9e69c15 744 right2.checkDistance();
ihansw 0:92d0f9e69c15 745 }
ihansw 0:92d0f9e69c15 746 }
ihansw 0:92d0f9e69c15 747 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 748 // uLCD.printf("Right!");
ihansw 0:92d0f9e69c15 749 }
ihansw 0:92d0f9e69c15 750
ihansw 0:92d0f9e69c15 751 ////////////////////////////////////////turning right/////////////////////////////////////
ihansw 0:92d0f9e69c15 752 if((distan3>300)&&(distan3b>300))
ihansw 0:92d0f9e69c15 753 {
ihansw 0:92d0f9e69c15 754 led1 = 1;
ihansw 0:92d0f9e69c15 755 led2 = 0;
ihansw 0:92d0f9e69c15 756 led3 = 1;
ihansw 0:92d0f9e69c15 757 led4 =1;
ihansw 0:92d0f9e69c15 758
ihansw 0:92d0f9e69c15 759 uart.printf("%c",f);
ihansw 0:92d0f9e69c15 760 wait(.3);
ihansw 0:92d0f9e69c15 761
ihansw 0:92d0f9e69c15 762 if(uart.writeable())
ihansw 0:92d0f9e69c15 763 {
ihansw 0:92d0f9e69c15 764 uart.printf("%c",r);
ihansw 0:92d0f9e69c15 765 wait(2);
ihansw 0:92d0f9e69c15 766 //led3 = 1;
ihansw 0:92d0f9e69c15 767 }
ihansw 0:92d0f9e69c15 768
ihansw 0:92d0f9e69c15 769 left1.checkDistance();
ihansw 0:92d0f9e69c15 770 left2.checkDistance();
ihansw 0:92d0f9e69c15 771 right1.checkDistance();
ihansw 0:92d0f9e69c15 772 right2.checkDistance();
ihansw 0:92d0f9e69c15 773 wait(2);
ihansw 0:92d0f9e69c15 774
ihansw 0:92d0f9e69c15 775 // checking on the left. adjust the angle and make sure it's pointing at the right direction
ihansw 0:92d0f9e69c15 776 while( abs((distan1)-(distan1b)) >15 )
ihansw 0:92d0f9e69c15 777 {
ihansw 0:92d0f9e69c15 778 left1.checkDistance();
ihansw 0:92d0f9e69c15 779 left2.checkDistance();
ihansw 0:92d0f9e69c15 780 right1.checkDistance();
ihansw 0:92d0f9e69c15 781 right2.checkDistance();
ihansw 0:92d0f9e69c15 782
ihansw 0:92d0f9e69c15 783 wait(1);
ihansw 0:92d0f9e69c15 784
ihansw 0:92d0f9e69c15 785 if(distan1 > distan1b)
ihansw 0:92d0f9e69c15 786 {
ihansw 0:92d0f9e69c15 787 uart.printf("%c",g);
ihansw 0:92d0f9e69c15 788 left1.checkDistance();
ihansw 0:92d0f9e69c15 789 left2.checkDistance();
ihansw 0:92d0f9e69c15 790 right1.checkDistance();
ihansw 0:92d0f9e69c15 791 right2.checkDistance();
ihansw 0:92d0f9e69c15 792
ihansw 0:92d0f9e69c15 793 wait(1);
ihansw 0:92d0f9e69c15 794 }
ihansw 0:92d0f9e69c15 795
ihansw 0:92d0f9e69c15 796 if(distan1 <distan1b)
ihansw 0:92d0f9e69c15 797 {
ihansw 0:92d0f9e69c15 798 uart.printf("%c",h);
ihansw 0:92d0f9e69c15 799 left1.checkDistance();
ihansw 0:92d0f9e69c15 800 left2.checkDistance();
ihansw 0:92d0f9e69c15 801 right1.checkDistance();
ihansw 0:92d0f9e69c15 802 right2.checkDistance();
ihansw 0:92d0f9e69c15 803
ihansw 0:92d0f9e69c15 804 wait(1);
ihansw 0:92d0f9e69c15 805 }
ihansw 0:92d0f9e69c15 806
ihansw 0:92d0f9e69c15 807
ihansw 0:92d0f9e69c15 808 }
ihansw 0:92d0f9e69c15 809
ihansw 0:92d0f9e69c15 810 //after turning right, if there are space open on the right, focus on the left wall
ihansw 0:92d0f9e69c15 811 while((distan3>220)||(distan3b>220))
ihansw 0:92d0f9e69c15 812 {
ihansw 0:92d0f9e69c15 813 //front.checkDistance(); //call checkDistance() as much as possible, as this is where
ihansw 0:92d0f9e69c15 814 //the class checks if dist needs to be called.
ihansw 0:92d0f9e69c15 815 left1.checkDistance();
ihansw 0:92d0f9e69c15 816 left2.checkDistance();
ihansw 0:92d0f9e69c15 817 //back.checkDistance();
ihansw 0:92d0f9e69c15 818 right1.checkDistance();
ihansw 0:92d0f9e69c15 819 right2.checkDistance();
ihansw 0:92d0f9e69c15 820
ihansw 0:92d0f9e69c15 821
ihansw 0:92d0f9e69c15 822 if((distan1>distan1b))
ihansw 0:92d0f9e69c15 823 {
ihansw 0:92d0f9e69c15 824 uart.printf("%c",a);
ihansw 0:92d0f9e69c15 825 wait(.3);
ihansw 0:92d0f9e69c15 826 left1.checkDistance();
ihansw 0:92d0f9e69c15 827 left2.checkDistance();
ihansw 0:92d0f9e69c15 828 //back.checkDistance();
ihansw 0:92d0f9e69c15 829 right1.checkDistance();
ihansw 0:92d0f9e69c15 830 right2.checkDistance();
ihansw 0:92d0f9e69c15 831 }
ihansw 0:92d0f9e69c15 832 if((distan1<distan1b))
ihansw 0:92d0f9e69c15 833 {
ihansw 0:92d0f9e69c15 834 uart.printf("%c",z);
ihansw 0:92d0f9e69c15 835 wait(.3);
ihansw 0:92d0f9e69c15 836 left1.checkDistance();
ihansw 0:92d0f9e69c15 837 left2.checkDistance();
ihansw 0:92d0f9e69c15 838 //back.checkDistance();
ihansw 0:92d0f9e69c15 839 right1.checkDistance();
ihansw 0:92d0f9e69c15 840 right2.checkDistance();
ihansw 0:92d0f9e69c15 841 }
ihansw 0:92d0f9e69c15 842 }
ihansw 0:92d0f9e69c15 843 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 844 // uLCD.printf("Right!");
ihansw 0:92d0f9e69c15 845 }
ihansw 0:92d0f9e69c15 846
ihansw 0:92d0f9e69c15 847 //parking stop condition
ihansw 0:92d0f9e69c15 848 if((diffLR < 30) && (diffLR2 < 30) &&(distan0 < 50))
ihansw 0:92d0f9e69c15 849 {
ihansw 0:92d0f9e69c15 850 led1 = 1;
ihansw 0:92d0f9e69c15 851 led2 = 1;
ihansw 0:92d0f9e69c15 852 led3 = 1;
ihansw 0:92d0f9e69c15 853 led4 = 1;
ihansw 0:92d0f9e69c15 854
ihansw 0:92d0f9e69c15 855 if(uart.writeable())
ihansw 0:92d0f9e69c15 856 {
ihansw 0:92d0f9e69c15 857 uart.printf("%c",p);
ihansw 0:92d0f9e69c15 858 wait(0.3);
ihansw 0:92d0f9e69c15 859 //led3 = 1;
ihansw 0:92d0f9e69c15 860 }
ihansw 0:92d0f9e69c15 861
ihansw 0:92d0f9e69c15 862 //uLCD.locate(0,14);
ihansw 0:92d0f9e69c15 863 // uLCD.printf("PARK DONE");
ihansw 0:92d0f9e69c15 864 }
ihansw 0:92d0f9e69c15 865 }
ihansw 0:92d0f9e69c15 866
ihansw 0:92d0f9e69c15 867
ihansw 0:92d0f9e69c15 868 }
ihansw 0:92d0f9e69c15 869
ihansw 0:92d0f9e69c15 870
ihansw 0:92d0f9e69c15 871 ////////////////////////////////////////////////////////////////////////
ihansw 0:92d0f9e69c15 872
ihansw 0:92d0f9e69c15 873
ihansw 0:92d0f9e69c15 874
ihansw 0:92d0f9e69c15 875 }
ihansw 0:92d0f9e69c15 876
ihansw 0:92d0f9e69c15 877 else
ihansw 0:92d0f9e69c15 878 {
ihansw 0:92d0f9e69c15 879 //add release code here
ihansw 0:92d0f9e69c15 880 }
ihansw 0:92d0f9e69c15 881 break;
ihansw 0:92d0f9e69c15 882
ihansw 0:92d0f9e69c15 883 case '2': //number button 2
ihansw 0:92d0f9e69c15 884 if (bhit=='1') {
ihansw 0:92d0f9e69c15 885 //add hit code here
ihansw 0:92d0f9e69c15 886
ihansw 0:92d0f9e69c15 887 if(uart.writeable())
ihansw 0:92d0f9e69c15 888 {
ihansw 0:92d0f9e69c15 889 uart.printf("%c",a);
ihansw 0:92d0f9e69c15 890 }
ihansw 0:92d0f9e69c15 891
ihansw 0:92d0f9e69c15 892 } else {
ihansw 0:92d0f9e69c15 893 //add release code here
ihansw 0:92d0f9e69c15 894 }
ihansw 0:92d0f9e69c15 895 break;
ihansw 0:92d0f9e69c15 896 case '3': //number button 3
ihansw 0:92d0f9e69c15 897 if (bhit=='1') {
ihansw 0:92d0f9e69c15 898 //add hit code here
ihansw 0:92d0f9e69c15 899
ihansw 0:92d0f9e69c15 900 if(uart.writeable())
ihansw 0:92d0f9e69c15 901 {
ihansw 0:92d0f9e69c15 902 uart.printf("%c",z);
ihansw 0:92d0f9e69c15 903 }
ihansw 0:92d0f9e69c15 904
ihansw 0:92d0f9e69c15 905 } else {
ihansw 0:92d0f9e69c15 906 //add release code here
ihansw 0:92d0f9e69c15 907 }
ihansw 0:92d0f9e69c15 908 break;
ihansw 0:92d0f9e69c15 909 case '4': //number button 4
ihansw 0:92d0f9e69c15 910 if (bhit=='1') {
ihansw 0:92d0f9e69c15 911 //add hit code here
ihansw 0:92d0f9e69c15 912 } else {
ihansw 0:92d0f9e69c15 913 //add release code here
ihansw 0:92d0f9e69c15 914 }
ihansw 0:92d0f9e69c15 915 break;
ihansw 0:92d0f9e69c15 916 case '5': //button 5 up arrow
ihansw 0:92d0f9e69c15 917 if (bhit=='1') {
ihansw 0:92d0f9e69c15 918 //add hit code here
ihansw 0:92d0f9e69c15 919 //uLCD.locate(0,1);
ihansw 0:92d0f9e69c15 920 //uLCD.printf("Moving Forward");
ihansw 0:92d0f9e69c15 921 led1 = 1;
ihansw 0:92d0f9e69c15 922 led2 = 0;
ihansw 0:92d0f9e69c15 923 led3 = 0;
ihansw 0:92d0f9e69c15 924 led4 = 0;
ihansw 0:92d0f9e69c15 925
ihansw 0:92d0f9e69c15 926 if(uart.writeable())
ihansw 0:92d0f9e69c15 927 {
ihansw 0:92d0f9e69c15 928 uart.printf("%c",f);
ihansw 0:92d0f9e69c15 929 //led3 = 1;
ihansw 0:92d0f9e69c15 930 }
ihansw 0:92d0f9e69c15 931
ihansw 0:92d0f9e69c15 932 } else {
ihansw 0:92d0f9e69c15 933 //add release code here
ihansw 0:92d0f9e69c15 934 }
ihansw 0:92d0f9e69c15 935 break;
ihansw 0:92d0f9e69c15 936 case '6': //button 6 down arrow
ihansw 0:92d0f9e69c15 937 if (bhit=='1') {
ihansw 0:92d0f9e69c15 938 //add hit code here
ihansw 0:92d0f9e69c15 939 // uLCD.locate(0,1);
ihansw 0:92d0f9e69c15 940 //uLCD.printf("Moving Back");
ihansw 0:92d0f9e69c15 941 led1 = 0;
ihansw 0:92d0f9e69c15 942 led2 = 0;
ihansw 0:92d0f9e69c15 943 led3 = 1;
ihansw 0:92d0f9e69c15 944 led4 = 0;
ihansw 0:92d0f9e69c15 945
ihansw 0:92d0f9e69c15 946 if(uart.writeable())
ihansw 0:92d0f9e69c15 947 {
ihansw 0:92d0f9e69c15 948 uart.printf("%c",b);
ihansw 0:92d0f9e69c15 949 }
ihansw 0:92d0f9e69c15 950
ihansw 0:92d0f9e69c15 951 } else {
ihansw 0:92d0f9e69c15 952 //add release code here
ihansw 0:92d0f9e69c15 953 }
ihansw 0:92d0f9e69c15 954 break;
ihansw 0:92d0f9e69c15 955 case '7': //button 7 left arrow
ihansw 0:92d0f9e69c15 956 if (bhit=='1') {
ihansw 0:92d0f9e69c15 957 //add hit code here
ihansw 0:92d0f9e69c15 958 //uLCD.locate(0,1);
ihansw 0:92d0f9e69c15 959 //uLCD.printf("Moving Left");
ihansw 0:92d0f9e69c15 960 led1 = 0;
ihansw 0:92d0f9e69c15 961 led2 = 1;
ihansw 0:92d0f9e69c15 962 led3 = 0;
ihansw 0:92d0f9e69c15 963 led4 = 0;
ihansw 0:92d0f9e69c15 964
ihansw 0:92d0f9e69c15 965 if(uart.writeable())
ihansw 0:92d0f9e69c15 966 {
ihansw 0:92d0f9e69c15 967 uart.printf("%c",l);
ihansw 0:92d0f9e69c15 968 }
ihansw 0:92d0f9e69c15 969
ihansw 0:92d0f9e69c15 970 } else {
ihansw 0:92d0f9e69c15 971 //add release code here
ihansw 0:92d0f9e69c15 972 }
ihansw 0:92d0f9e69c15 973 break;
ihansw 0:92d0f9e69c15 974 case '8': //button 8 right arrow
ihansw 0:92d0f9e69c15 975 if (bhit=='1') {
ihansw 0:92d0f9e69c15 976 //add hit code here
ihansw 0:92d0f9e69c15 977 //uLCD.locate(0,1);
ihansw 0:92d0f9e69c15 978 //uLCD.printf("Moving Right");
ihansw 0:92d0f9e69c15 979 led1 = 0;
ihansw 0:92d0f9e69c15 980 led2 = 0;
ihansw 0:92d0f9e69c15 981 led3 = 0;
ihansw 0:92d0f9e69c15 982 led4 = 1;
ihansw 0:92d0f9e69c15 983
ihansw 0:92d0f9e69c15 984 if(uart.writeable())
ihansw 0:92d0f9e69c15 985 {
ihansw 0:92d0f9e69c15 986 uart.printf("%c",r);
ihansw 0:92d0f9e69c15 987 }
ihansw 0:92d0f9e69c15 988
ihansw 0:92d0f9e69c15 989 } else {
ihansw 0:92d0f9e69c15 990 //add release code here
ihansw 0:92d0f9e69c15 991 }
ihansw 0:92d0f9e69c15 992 break;
ihansw 0:92d0f9e69c15 993 default:
ihansw 0:92d0f9e69c15 994 break;
ihansw 0:92d0f9e69c15 995 }
ihansw 0:92d0f9e69c15 996 }
ihansw 0:92d0f9e69c15 997 }
ihansw 0:92d0f9e69c15 998 }
ihansw 0:92d0f9e69c15 999 }
ihansw 0:92d0f9e69c15 1000 }