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?
User | Revision | Line number | New 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 | } |