Sungwoo Han
ece 4180 final project master mbed code
Dependencies: 4DGL-uLCD-SE HC_SR04_Ultrasonic_Library mbed
Diff: main.cpp
- Revision:
- 0:92d0f9e69c15
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Dec 09 15:32:51 2016 +0000
@@ -0,0 +1,1000 @@
+#include "mbed.h"
+#include "ultrasonic.h"
+#include "uLCD_4DGL.h"//uLCD class
+//BusOut myled(LED1,LED2,LED3,LED4);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//uLCD_4DGL uLCD(p9, p10, p7); // tx rx reset create a global lcd object
+
+
+Serial blue(p28,p27);//tx rx for bluetooth
+
+Serial uart(p9,p10);//Sungwoo
+
+
+////////////////////////////////////////////////
+
+volatile int distan0 = 0;//front
+volatile int distan1 = 0;//left
+volatile int distan2r = 0;//back right
+volatile int distan2l = 0;//back left
+volatile int distan3 = 0;//right
+volatile int distan1b = 0;//left2
+volatile int distan3b = 0;//right2
+
+//volatile char s[1];
+
+void dist0(int distance0)
+{
+//put code here to execute when the distance has changed
+ //uLCD.locate(0,1);
+ //uLCD.printf("DistanceFront %d \r\n", distance0);
+ distan0 = distance0;
+}
+void dist1(int distance1)
+{
+ //uLCD.locate(0,3);
+ //uLCD.printf("DistanceLeft %d \r\n", distance1);
+ distan1 = distance1;
+}
+
+void dist1b(int distance1b)
+{
+ //uLCD.locate(0,5);
+ //uLCD.printf("DistanceLeftB %d \r\n", distance1b);
+ distan1b = distance1b;
+}
+void dist2r(int distance2r)
+{
+ //uLCD.locate(0,7);
+ //uLCD.printf("DisatnceBack %d \r\n", distance2);
+ distan2r = distance2r;
+
+}
+void dist2l(int distance2l)
+{
+ distan2l = distance2l;
+}
+void dist3(int distance3)
+{
+ //uLCD.locate(0,9);
+ //uLCD.printf("DistanceRight %d \r\n", distance3);
+ distan3 = distance3;
+}
+
+void dist3b(int distance3b)
+{
+ //uLCD.locate(0,11);
+ //uLCD.printf("DistanceRightB %d \r\n", distance3b);
+ distan3b = distance3b;
+}
+ultrasonic front(p8, p11, .1, .5, &dist0);//trig echo
+ //have updates every .1 seconds and a timeout after 1
+ //second, and call dist when the distance changes
+ultrasonic left1(p25, p26, .1, .5, &dist1);
+ultrasonic left2(p12, p13, .1, .5, &dist1b);
+ultrasonic backl(p21, p22, .1, .5, &dist2l);
+ultrasonic backr(p5, p6, .1, .5, &dist2r);
+ultrasonic right1(p29, p30, .1, .5, &dist3);
+ultrasonic right2(p23, p24, .1, .5, &dist3b);
+
+///////////////////////////////////////////////
+
+int main()
+{
+ char bnum=0;
+ char bhit=0;
+
+ //Sungwoo
+ uart.baud(9600);
+
+ char f = 'f'; // Move forward
+ char b = 'b'; // Move Backward
+ char l = 'l'; // Turn Left
+ char r = 'r'; // Turn Right
+ char w = 'w'; // Park on the left
+ char x = 'x'; // Pakk on the right
+
+ char a = 'a'; // tilt to the left
+ char z = 'z'; // tilt to the right
+ char p = 'p'; // Parked
+
+ char g = 'g'; // tilt angle to the left (CCW)
+ char h = 'h'; // tilt angle to the right (CW)
+
+
+ ////////////////////////////////
+
+ //bool variables for each sensor
+ bool fSensor = false;
+ bool r1Sensor = false;
+ bool r2Sensor = false;
+ bool brSensor = false;
+ bool blSensor = false;
+ bool l1Sensor = false;
+ bool l2Sensor = false;
+
+
+ //char bnum=0;
+ //char bhit=0;
+
+
+
+ //Front - 0, Left - 1, Back - 2, Right - 3
+ int diffLR = 0;
+ int diffLR2 = 0;
+ //uLCD.printf("Hey");
+
+
+ /////////////////////////////////////
+
+ while(1) {
+ if (blue.getc()=='!') {
+ if (blue.getc()=='B') { //button data packet
+ bnum = blue.getc(); //button number
+ bhit = blue.getc(); //1=hit, 0=release
+ if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+ //myled = bnum - '0'; //current button number will appear on LEDs
+ switch (bnum) {
+ case '1': //number button 1
+
+ if (bhit=='1')
+ {
+ //uLCD.locate(0,1);
+ //uLCD.printf("Parking start");
+
+ //int distance = 0;
+ front.startUpdates();//start measuring the distance
+ left1.startUpdates();
+ left2.startUpdates();
+ backr.startUpdates();
+ backl.startUpdates();
+ right1.startUpdates();
+ right2.startUpdates();
+
+ //////////////////////////parking #1////////////////////////////////
+
+
+ while(1)
+ {
+ //Do something else here
+ front.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ /*uLCD.locate(0,5);
+ uLCD.printf("distance0: %d", distan0);
+ uLCD.locate(0,6);
+ uLCD.printf("distance1: %d", distan1);*/
+
+ diffLR = abs(distan1 - distan3);
+ diffLR2 = abs(distan1b - distan3b);
+ // uLCD.locate(0,7);
+ // uLCD.printf("diiference 12: %d", diffLR);
+
+ // #1 - check the front sensor
+ if(distan0>300)
+ {
+ fSensor = true;
+ //zig-zag feature in the map by adjusting the distance of left1 and right1 sensors
+
+ // 'a' - tilt to left.
+ if((diffLR<100) && (distan1>distan3))
+ {
+ //led1 = 0;
+ led2 = 1;
+ led3 = 0;
+ led4 = 0;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",a);
+ led3 = 1;
+ wait(0.3);
+ }
+
+ //uLCD.locate(0,14);
+ //uLCD.printf("Forward, tilt to left!");
+ }
+ // 'z' - tilt to right.
+ if((diffLR<100) && (distan1<distan3))
+ {
+ led1 = 0;
+ led2 = 0;
+ //led3 = 0;
+ led4 = 1;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",z);
+ led3 = 1;
+ wait(0.3);
+ }
+
+
+ //uLCD.locate(0,14);
+ //uLCD.printf("Forward, tilt to right!");
+ }
+
+ //feedback with one side (left/right)
+ // compare front and back distance and adjust!!!!! // NOT BEEN CODED YET
+ if((distan3>300) && (distan3b<100))
+ {
+ led1 = 1;
+ led2 = 0;
+ led3 = 1;
+ led4 = 1;
+
+ //move forward
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",f);
+ //led3 = 1;
+ wait(0.3);
+ }
+
+ //uLCD.locate(0,14);
+ //uLCD.printf("Forward, One FB, Left\n");
+ // SAME AS ABOVE
+ }
+
+ if((distan1>300) && (distan1b<100))
+ {
+ led1 = 1;
+ led2 = 1;
+ led3 = 1;
+ led4 = 0;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",f);
+ wait(0.3);
+ //led3 = 1;
+ }
+
+ //uLCD.locate(0,14);
+ //uLCD.printf("Forward, One FB, Right\n");
+ }
+
+
+
+ ////////////detecting parking spaces on the LEFT SIDE////////////
+
+ if((distan1>300) && (distan1b>300))
+ {
+ l1Sensor = true;
+ l2Sensor = true;
+
+ //parking space is on the left side
+ led1 = 0;
+ led2 = 1;
+ led3 = 1;
+ led4 = 0;
+
+
+ if(uart.writeable())
+ {
+
+ uart.printf("%c",w);
+ wait(3);
+ }
+
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(2);
+
+
+ while(abs((distan2r)-(distan2l)) >15 )
+ {
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ wait(1);
+
+ if(distan2r > distan2l)
+ {
+ uart.printf("%c",h);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ if(distan2r < distan2l)
+ {
+ uart.printf("%c",g);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ }
+
+
+ //move forward
+ while((distan1 > 200)||(distan3 > 200))
+ {
+ front.checkDistance();
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ if((distan1 > 200)||(distan3 > 200))
+ {
+ uart.printf("%c",f);
+ wait(0.3);
+ front.checkDistance();
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+ }
+
+ while(distan0>130)
+ {
+
+ front.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ //tilt to the right
+ if(distan1>distan3)
+ {
+ uart.printf("%c",z);
+ led3 = 1;
+ wait(0.3);
+ }
+ //tilt to the left
+ if(distan1<distan3)
+ {
+ uart.printf("%c",a);
+ led3 = 1;
+ wait(0.3);
+ }
+ front.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+
+ uart.printf("%c",r);
+
+
+
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(2);
+
+
+ while(abs((distan2r)-(distan2l)) >15 )
+ {
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ wait(1);
+
+ if(distan2r > distan2l)
+ {
+ uart.printf("%c",h);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ if(distan2r < distan2l)
+ {
+ uart.printf("%c",g);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ }
+
+
+
+ uart.printf("%c",p);
+ wait(20);
+
+ //uLCD.locate(0,14);
+ //uLCD.printf("Park, on the left!");
+ }
+
+ /// /////////parking space is on the right side////////////////////
+ if((distan3>300) && (distan3b>300))
+ {
+ l1Sensor = true;
+ l2Sensor = true;
+
+ //parking space is on the left side
+ led1 = 0;
+ led2 = 1;
+ led3 = 1;
+ led4 = 0;
+
+
+ if(uart.writeable())
+ {
+
+ uart.printf("%c",x);
+ wait(3);
+ }
+
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(2);
+
+
+ while(abs((distan2r)-(distan2l)) > 15 )
+ {
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ wait(1);
+
+ if(distan2r > distan2l)
+ {
+ uart.printf("%c",h);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ if(distan2r < distan2l)
+ {
+ uart.printf("%c",g);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ }
+
+
+ //move forward
+ while((distan1 > 200)||(distan3 > 200))
+ {
+ front.checkDistance();
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ if((distan1 > 200)||(distan3 > 200))
+ {
+ uart.printf("%c",f);
+ wait(0.3);
+ front.checkDistance();
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+ }
+
+ while(distan0>130)
+ {
+
+ front.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ //tilt to the right
+ if(distan1>distan3)
+ {
+ uart.printf("%c",z);
+ led3 = 1;
+ wait(0.3);
+ }
+ //tilt to the left
+ if(distan1<distan3)
+ {
+ uart.printf("%c",a);
+ led3 = 1;
+ wait(0.3);
+ }
+ front.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+
+ uart.printf("%c",l);
+
+
+
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(2);
+
+
+ while(abs((distan2r)-(distan2l)) >15 )
+ {
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ wait(1);
+
+ if(distan2r > distan2l)
+ {
+ uart.printf("%c",h);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ if(distan2r < distan2l)
+ {
+ uart.printf("%c",g);
+ left1.checkDistance();
+ left2.checkDistance();
+ backr.checkDistance();
+ backl.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ }
+
+
+
+ uart.printf("%c",p);
+ wait(20);
+
+ //uLCD.locate(0,14);
+ //uLCD.printf("Park, on the left!");
+ }
+ }
+ //if(distan0<50)
+
+ //FRONT SENSOR ON
+ else
+ {
+ //cornering feature
+
+///////////////////////////////////////////////turning left//////////////////////////////////////////////
+ if((distan1>300)&&(distan1b>300))
+ {
+ led1 = 1;
+ led2 = 0;
+ led3 = 1;
+ led4 =1;
+
+ uart.printf("%c",f);
+ wait(.3);
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",l);
+ wait(2);
+ //led3 = 1;
+ }
+
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ wait(2);
+
+ // checking on the left. adjust the angle and make sure it's pointing at the right direction
+ while( abs((distan3)-(distan3b)) >15 )
+ {
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+
+ if(distan3 > distan3b)
+ {
+ uart.printf("%c",h);
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ if(distan3 <distan3b)
+ {
+ uart.printf("%c",g);
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+
+ }
+
+ //after turning left, if there are space open on the left, focus on the left wall
+ while((distan1>220)||(distan1b>220))
+ {
+ //front.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+
+ if((distan3>distan3b))
+ {
+ uart.printf("%c",z);
+ wait(.3);
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+ if((distan3<distan3b))
+ {
+ uart.printf("%c",a);
+ wait(.3);
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+ }
+ //uLCD.locate(0,14);
+ // uLCD.printf("Right!");
+ }
+
+ ////////////////////////////////////////turning right/////////////////////////////////////
+ if((distan3>300)&&(distan3b>300))
+ {
+ led1 = 1;
+ led2 = 0;
+ led3 = 1;
+ led4 =1;
+
+ uart.printf("%c",f);
+ wait(.3);
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",r);
+ wait(2);
+ //led3 = 1;
+ }
+
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ wait(2);
+
+ // checking on the left. adjust the angle and make sure it's pointing at the right direction
+ while( abs((distan1)-(distan1b)) >15 )
+ {
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+
+ if(distan1 > distan1b)
+ {
+ uart.printf("%c",g);
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+ if(distan1 <distan1b)
+ {
+ uart.printf("%c",h);
+ left1.checkDistance();
+ left2.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+ wait(1);
+ }
+
+
+ }
+
+ //after turning right, if there are space open on the right, focus on the left wall
+ while((distan3>220)||(distan3b>220))
+ {
+ //front.checkDistance(); //call checkDistance() as much as possible, as this is where
+ //the class checks if dist needs to be called.
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+
+
+ if((distan1>distan1b))
+ {
+ uart.printf("%c",a);
+ wait(.3);
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+ if((distan1<distan1b))
+ {
+ uart.printf("%c",z);
+ wait(.3);
+ left1.checkDistance();
+ left2.checkDistance();
+ //back.checkDistance();
+ right1.checkDistance();
+ right2.checkDistance();
+ }
+ }
+ //uLCD.locate(0,14);
+ // uLCD.printf("Right!");
+ }
+
+ //parking stop condition
+ if((diffLR < 30) && (diffLR2 < 30) &&(distan0 < 50))
+ {
+ led1 = 1;
+ led2 = 1;
+ led3 = 1;
+ led4 = 1;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",p);
+ wait(0.3);
+ //led3 = 1;
+ }
+
+ //uLCD.locate(0,14);
+ // uLCD.printf("PARK DONE");
+ }
+ }
+
+
+ }
+
+
+ ////////////////////////////////////////////////////////////////////////
+
+
+
+ }
+
+ else
+ {
+ //add release code here
+ }
+ break;
+
+ case '2': //number button 2
+ if (bhit=='1') {
+ //add hit code here
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",a);
+ }
+
+ } else {
+ //add release code here
+ }
+ break;
+ case '3': //number button 3
+ if (bhit=='1') {
+ //add hit code here
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",z);
+ }
+
+ } else {
+ //add release code here
+ }
+ break;
+ case '4': //number button 4
+ if (bhit=='1') {
+ //add hit code here
+ } else {
+ //add release code here
+ }
+ break;
+ case '5': //button 5 up arrow
+ if (bhit=='1') {
+ //add hit code here
+ //uLCD.locate(0,1);
+ //uLCD.printf("Moving Forward");
+ led1 = 1;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",f);
+ //led3 = 1;
+ }
+
+ } else {
+ //add release code here
+ }
+ break;
+ case '6': //button 6 down arrow
+ if (bhit=='1') {
+ //add hit code here
+ // uLCD.locate(0,1);
+ //uLCD.printf("Moving Back");
+ led1 = 0;
+ led2 = 0;
+ led3 = 1;
+ led4 = 0;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",b);
+ }
+
+ } else {
+ //add release code here
+ }
+ break;
+ case '7': //button 7 left arrow
+ if (bhit=='1') {
+ //add hit code here
+ //uLCD.locate(0,1);
+ //uLCD.printf("Moving Left");
+ led1 = 0;
+ led2 = 1;
+ led3 = 0;
+ led4 = 0;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",l);
+ }
+
+ } else {
+ //add release code here
+ }
+ break;
+ case '8': //button 8 right arrow
+ if (bhit=='1') {
+ //add hit code here
+ //uLCD.locate(0,1);
+ //uLCD.printf("Moving Right");
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 1;
+
+ if(uart.writeable())
+ {
+ uart.printf("%c",r);
+ }
+
+ } else {
+ //add release code here
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ }
+ }
+}
\ No newline at end of file