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; } } } } } }