It's an obstacle avoider using mbed lpc1768
Dependencies: mbed Motordriver HC_SR04_Ultrasonic_Library
Revision 0:3c1eead7416c, committed 2019-12-13
- Comitter:
- 6221570
- Date:
- Fri Dec 13 02:22:49 2019 +0000
- Commit message:
- Its an obstacle avoider using mbed lpc1768
Changed in this revision
diff -r 000000000000 -r 3c1eead7416c HC_SR04_Ultrasonic_Library.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC_SR04_Ultrasonic_Library.lib Fri Dec 13 02:22:49 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
diff -r 000000000000 -r 3c1eead7416c Motordriver.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motordriver.lib Fri Dec 13 02:22:49 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/littlexc/code/Motordriver/#3110b9209d3c
diff -r 000000000000 -r 3c1eead7416c main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Dec 13 02:22:49 2019 +0000 @@ -0,0 +1,158 @@ +#include "ultrasonic.h" +#include "motordriver.h" + +//ADXL +SPI acc(p11,p12,p13); // set up SPI interface on pins 11,12,13 +DigitalOut cs(p14); // use pin 14 as chip select + +//FRONT MOTORS +Motor FL(p22, p6, p5, 1); // pwm, fwd, rev, can brake +Motor FR(p21, p7, p8, 1); + +//BACK MOTORS +Motor BL(p23, p17, p18, 1); +Motor BR(p24, p15, p16, 1); + +void drivefront(void); +void driveback(void); +void driveleft(void); +void driveright(void); +void dynamicdrivefront(void); + +Serial pc(USBTX, USBRX); + +char buffer[6]; //raw data array type char +int16_t data[3]; // 16-bit twos-complement integer data +float x, y, z; // floating point data, to be displayed on-screen + +int sl,sr; //left sensor and right sensor +void dist(int distance) +{ + + //put code here to execute when the distance has changed + + sl = distance; + pc.printf("sl %d mm\r\n", sl); +} +void gist(int distance) +{ + + //put code here to execute when the distance has changed + + sr = distance; + pc.printf("sr %d mm\r\n", sr); +} + +ultrasonic mu(p27, p28, .1, 1, &gist); //Set the trigger pin to p27 and the echo pin to p28 +ultrasonic lu(p10, p9, .1, 1, &dist); //have updates every .1 seconds and a timeout after 1 + //second, and call dist when the distance changes + +int main() +{ +//ADXL{ +cs=1; //initially ADXL345 is not activated +acc.format(8,3); // 8 bit data, Mode 3 +acc.frequency(2000000); // 2MHz clock rate +cs=0; //select the device +acc.write(0x31); // data format register +acc.write(0x0B); // format +/-16g, 0.004g/LSB +cs=1; //end of transmission +cs=0; //start a new transmission +acc.write(0x2D); // power ctrl register +acc.write(0x08); // measure mode +cs=1; //end of transmission +//}ADXL + lu.startUpdates();//start measuring the distance + mu.startUpdates();//start measuring the distance + while(1) + { + cs=0; //start a transmission + acc.write(0x80|0x40|0x32); // RW bit high, MB bit high, plus address + for (int i = 0;i<=5;i++) { + buffer[i]=acc.write(0x00); // read back 6 data bytes + } + cs=1; //end of transmission + data[0] = buffer[1]<<8 | buffer[0]; // combine MSB and LSB + data[1] = buffer[3]<<8 | buffer[2]; + data[2] = buffer[5]<<8 | buffer[4]; + x=0.004*data[0]; y=0.004*data[1]; z=0.004*data[2]; // convert to float, + //actual g value + + // pc.printf("x = %+1.2fg\t y = %+1.2fg\t z = %+1.2fg\n\r", x, y,z); //print + + lu.checkDistance(); + mu.checkDistance(); + + if (sl>200 && sr>200){ + if(y>=0.26){//o.26=sin(15) to signify elevation above that point + dynamicdrivefront(); + } + else{ + drivefront(); + } + } + else if(sl >150 && sr<=150){ + driveleft(); + } + else if(sr >150 && sl<=150){ + driveright(); + } + else{ + driveback(); + wait(0.6); + if(sl<sr){ + driveright();} + else{ + driveleft(); + } + } + } + + + + } + + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +void drivefront(void) +{ +FL.speed(0.4); +FR.speed(0.4); +BL.speed(0.4); +BR.speed(0.4); +//wait(0.1); +} + +void driveback(void) +{ +FL.speed(-0.4); +FR.speed(-0.4); +BL.speed(-0.4); +BR.speed(-0.4); +//wait(0.1); + } +void driveleft(void) +{ +FL.speed(-0.5); +FR.speed(0.4); +BL.speed(-0.5); +BR.speed(0.3); +//wait(0.1); + } +void driveright(void) +{ +FL.speed(0.4); +FR.speed(-0.5); +BL.speed(0.3); +BR.speed(-0.5); +//wait(0.1); + } +void dynamicdrivefront(void) +{ +FL.speed(0.5+((y-0.1)/2)); +FR.speed(0.5+((y-0.1)/2)); +BL.speed(0.5+((y-0.1)/2)); +BR.speed(0.5+((y-0.1)/2)); +//wait(0.1); +} \ No newline at end of file
diff -r 000000000000 -r 3c1eead7416c mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Dec 13 02:22:49 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file