ishaan Agarwal / Mbed 2 deprecated obstacle_mbed

It's an obstacle avoider using mbed lpc1768

Dependencies:   mbed Motordriver HC_SR04_Ultrasonic_Library

main.cpp@0:3c1eead7416c, 2019-12-13 (annotated)

Committer:
6221570
Date:
Fri Dec 13 02:22:49 2019 +0000
Revision:
0:3c1eead7416c
Its an obstacle avoider using mbed lpc1768

Who changed what in which revision?

UserRevisionLine numberNew contents of line
6221570 0:3c1eead7416c 1 #include "ultrasonic.h"
6221570 0:3c1eead7416c 2 #include "motordriver.h"
6221570 0:3c1eead7416c 3
6221570 0:3c1eead7416c 4 //ADXL
6221570 0:3c1eead7416c 5 SPI acc(p11,p12,p13); // set up SPI interface on pins 11,12,13
6221570 0:3c1eead7416c 6 DigitalOut cs(p14); // use pin 14 as chip select
6221570 0:3c1eead7416c 7
6221570 0:3c1eead7416c 8 //FRONT MOTORS
6221570 0:3c1eead7416c 9 Motor FL(p22, p6, p5, 1); // pwm, fwd, rev, can brake
6221570 0:3c1eead7416c 10 Motor FR(p21, p7, p8, 1);
6221570 0:3c1eead7416c 11
6221570 0:3c1eead7416c 12 //BACK MOTORS
6221570 0:3c1eead7416c 13 Motor BL(p23, p17, p18, 1);
6221570 0:3c1eead7416c 14 Motor BR(p24, p15, p16, 1);
6221570 0:3c1eead7416c 15
6221570 0:3c1eead7416c 16 void drivefront(void);
6221570 0:3c1eead7416c 17 void driveback(void);
6221570 0:3c1eead7416c 18 void driveleft(void);
6221570 0:3c1eead7416c 19 void driveright(void);
6221570 0:3c1eead7416c 20 void dynamicdrivefront(void);
6221570 0:3c1eead7416c 21
6221570 0:3c1eead7416c 22 Serial pc(USBTX, USBRX);
6221570 0:3c1eead7416c 23
6221570 0:3c1eead7416c 24 char buffer[6]; //raw data array type char
6221570 0:3c1eead7416c 25 int16_t data[3]; // 16-bit twos-complement integer data
6221570 0:3c1eead7416c 26 float x, y, z; // floating point data, to be displayed on-screen
6221570 0:3c1eead7416c 27
6221570 0:3c1eead7416c 28 int sl,sr; //left sensor and right sensor
6221570 0:3c1eead7416c 29 void dist(int distance)
6221570 0:3c1eead7416c 30 {
6221570 0:3c1eead7416c 31
6221570 0:3c1eead7416c 32 //put code here to execute when the distance has changed
6221570 0:3c1eead7416c 33
6221570 0:3c1eead7416c 34 sl = distance;
6221570 0:3c1eead7416c 35 pc.printf("sl %d mm\r\n", sl);
6221570 0:3c1eead7416c 36 }
6221570 0:3c1eead7416c 37 void gist(int distance)
6221570 0:3c1eead7416c 38 {
6221570 0:3c1eead7416c 39
6221570 0:3c1eead7416c 40 //put code here to execute when the distance has changed
6221570 0:3c1eead7416c 41
6221570 0:3c1eead7416c 42 sr = distance;
6221570 0:3c1eead7416c 43 pc.printf("sr %d mm\r\n", sr);
6221570 0:3c1eead7416c 44 }
6221570 0:3c1eead7416c 45
6221570 0:3c1eead7416c 46 ultrasonic mu(p27, p28, .1, 1, &gist); //Set the trigger pin to p27 and the echo pin to p28
6221570 0:3c1eead7416c 47 ultrasonic lu(p10, p9, .1, 1, &dist); //have updates every .1 seconds and a timeout after 1
6221570 0:3c1eead7416c 48 //second, and call dist when the distance changes
6221570 0:3c1eead7416c 49
6221570 0:3c1eead7416c 50 int main()
6221570 0:3c1eead7416c 51 {
6221570 0:3c1eead7416c 52 //ADXL{
6221570 0:3c1eead7416c 53 cs=1; //initially ADXL345 is not activated
6221570 0:3c1eead7416c 54 acc.format(8,3); // 8 bit data, Mode 3
6221570 0:3c1eead7416c 55 acc.frequency(2000000); // 2MHz clock rate
6221570 0:3c1eead7416c 56 cs=0; //select the device
6221570 0:3c1eead7416c 57 acc.write(0x31); // data format register
6221570 0:3c1eead7416c 58 acc.write(0x0B); // format +/-16g, 0.004g/LSB
6221570 0:3c1eead7416c 59 cs=1; //end of transmission
6221570 0:3c1eead7416c 60 cs=0; //start a new transmission
6221570 0:3c1eead7416c 61 acc.write(0x2D); // power ctrl register
6221570 0:3c1eead7416c 62 acc.write(0x08); // measure mode
6221570 0:3c1eead7416c 63 cs=1; //end of transmission
6221570 0:3c1eead7416c 64 //}ADXL
6221570 0:3c1eead7416c 65 lu.startUpdates();//start measuring the distance
6221570 0:3c1eead7416c 66 mu.startUpdates();//start measuring the distance
6221570 0:3c1eead7416c 67 while(1)
6221570 0:3c1eead7416c 68 {
6221570 0:3c1eead7416c 69 cs=0; //start a transmission
6221570 0:3c1eead7416c 70 acc.write(0x80|0x40|0x32); // RW bit high, MB bit high, plus address
6221570 0:3c1eead7416c 71 for (int i = 0;i<=5;i++) {
6221570 0:3c1eead7416c 72 buffer[i]=acc.write(0x00); // read back 6 data bytes
6221570 0:3c1eead7416c 73 }
6221570 0:3c1eead7416c 74 cs=1; //end of transmission
6221570 0:3c1eead7416c 75 data[0] = buffer[1]<<8 | buffer[0]; // combine MSB and LSB
6221570 0:3c1eead7416c 76 data[1] = buffer[3]<<8 | buffer[2];
6221570 0:3c1eead7416c 77 data[2] = buffer[5]<<8 | buffer[4];
6221570 0:3c1eead7416c 78 x=0.004*data[0]; y=0.004*data[1]; z=0.004*data[2]; // convert to float,
6221570 0:3c1eead7416c 79 //actual g value
6221570 0:3c1eead7416c 80
6221570 0:3c1eead7416c 81 // pc.printf("x = %+1.2fg\t y = %+1.2fg\t z = %+1.2fg\n\r", x, y,z); //print
6221570 0:3c1eead7416c 82
6221570 0:3c1eead7416c 83 lu.checkDistance();
6221570 0:3c1eead7416c 84 mu.checkDistance();
6221570 0:3c1eead7416c 85
6221570 0:3c1eead7416c 86 if (sl>200 && sr>200){
6221570 0:3c1eead7416c 87 if(y>=0.26){//o.26=sin(15) to signify elevation above that point
6221570 0:3c1eead7416c 88 dynamicdrivefront();
6221570 0:3c1eead7416c 89 }
6221570 0:3c1eead7416c 90 else{
6221570 0:3c1eead7416c 91 drivefront();
6221570 0:3c1eead7416c 92 }
6221570 0:3c1eead7416c 93 }
6221570 0:3c1eead7416c 94 else if(sl >150 && sr<=150){
6221570 0:3c1eead7416c 95 driveleft();
6221570 0:3c1eead7416c 96 }
6221570 0:3c1eead7416c 97 else if(sr >150 && sl<=150){
6221570 0:3c1eead7416c 98 driveright();
6221570 0:3c1eead7416c 99 }
6221570 0:3c1eead7416c 100 else{
6221570 0:3c1eead7416c 101 driveback();
6221570 0:3c1eead7416c 102 wait(0.6);
6221570 0:3c1eead7416c 103 if(sl<sr){
6221570 0:3c1eead7416c 104 driveright();}
6221570 0:3c1eead7416c 105 else{
6221570 0:3c1eead7416c 106 driveleft();
6221570 0:3c1eead7416c 107 }
6221570 0:3c1eead7416c 108 }
6221570 0:3c1eead7416c 109 }
6221570 0:3c1eead7416c 110
6221570 0:3c1eead7416c 111
6221570 0:3c1eead7416c 112
6221570 0:3c1eead7416c 113 }
6221570 0:3c1eead7416c 114
6221570 0:3c1eead7416c 115
6221570 0:3c1eead7416c 116 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
6221570 0:3c1eead7416c 117
6221570 0:3c1eead7416c 118 void drivefront(void)
6221570 0:3c1eead7416c 119 {
6221570 0:3c1eead7416c 120 FL.speed(0.4);
6221570 0:3c1eead7416c 121 FR.speed(0.4);
6221570 0:3c1eead7416c 122 BL.speed(0.4);
6221570 0:3c1eead7416c 123 BR.speed(0.4);
6221570 0:3c1eead7416c 124 //wait(0.1);
6221570 0:3c1eead7416c 125 }
6221570 0:3c1eead7416c 126
6221570 0:3c1eead7416c 127 void driveback(void)
6221570 0:3c1eead7416c 128 {
6221570 0:3c1eead7416c 129 FL.speed(-0.4);
6221570 0:3c1eead7416c 130 FR.speed(-0.4);
6221570 0:3c1eead7416c 131 BL.speed(-0.4);
6221570 0:3c1eead7416c 132 BR.speed(-0.4);
6221570 0:3c1eead7416c 133 //wait(0.1);
6221570 0:3c1eead7416c 134 }
6221570 0:3c1eead7416c 135 void driveleft(void)
6221570 0:3c1eead7416c 136 {
6221570 0:3c1eead7416c 137 FL.speed(-0.5);
6221570 0:3c1eead7416c 138 FR.speed(0.4);
6221570 0:3c1eead7416c 139 BL.speed(-0.5);
6221570 0:3c1eead7416c 140 BR.speed(0.3);
6221570 0:3c1eead7416c 141 //wait(0.1);
6221570 0:3c1eead7416c 142 }
6221570 0:3c1eead7416c 143 void driveright(void)
6221570 0:3c1eead7416c 144 {
6221570 0:3c1eead7416c 145 FL.speed(0.4);
6221570 0:3c1eead7416c 146 FR.speed(-0.5);
6221570 0:3c1eead7416c 147 BL.speed(0.3);
6221570 0:3c1eead7416c 148 BR.speed(-0.5);
6221570 0:3c1eead7416c 149 //wait(0.1);
6221570 0:3c1eead7416c 150 }
6221570 0:3c1eead7416c 151 void dynamicdrivefront(void)
6221570 0:3c1eead7416c 152 {
6221570 0:3c1eead7416c 153 FL.speed(0.5+((y-0.1)/2));
6221570 0:3c1eead7416c 154 FR.speed(0.5+((y-0.1)/2));
6221570 0:3c1eead7416c 155 BL.speed(0.5+((y-0.1)/2));
6221570 0:3c1eead7416c 156 BR.speed(0.5+((y-0.1)/2));
6221570 0:3c1eead7416c 157 //wait(0.1);
6221570 0:3c1eead7416c 158 }