Sungwoo Han
ece 4180 final project master mbed code
Dependencies: 4DGL-uLCD-SE HC_SR04_Ultrasonic_Library mbed
main.cpp
- Committer:
- ihansw
- Date:
- 2016-12-09
- Revision:
- 0:92d0f9e69c15
File content as of revision 0:92d0f9e69c15:
#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;
}
}
}
}
}
}