Vehicle code for Collision Avoidance Robot with Radar Imaging (Co-Developers: Andres Rodriguez and David Prina)
Dependencies: mbed Motordriver mbed-rtos HC_SR04_Ultrasonic_Library
Revision 0:3ec03802c957, committed 2019-04-26
- Comitter:
- Andres013
- Date:
- Fri Apr 26 14:27:52 2019 +0000
- Commit message:
- FINAL COMMIT
Changed in this revision
diff -r 000000000000 -r 3ec03802c957 HC_SR04_Ultrasonic_Library.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC_SR04_Ultrasonic_Library.lib Fri Apr 26 14:27:52 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
diff -r 000000000000 -r 3ec03802c957 Motordriver.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motordriver.lib Fri Apr 26 14:27:52 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/farbodjam/code/Motordriver/#541fb1742c8b
diff -r 000000000000 -r 3ec03802c957 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Apr 26 14:27:52 2019 +0000 @@ -0,0 +1,226 @@ +#include "mbed.h" +#include "ultrasonic.h" +#include "motordriver.h" +#include "rtos.h" + +#define STOP_DISTANCE 250 +#define RADAR_SPEED 0.5 +#define RADAR_WAIT 0.07 +#define pi 3.14159265f + +/* LEDS TO SHOW WHICH MODE ROBOT IS IN */ +DigitalOut led_left(LED1); +DigitalOut led_center_left(LED2); +DigitalOut led_center_right(LED3); +DigitalOut led_right(LED4); + +AnalogIn lightSensor(p20); +DigitalOut headlights(p30); + +Serial hc05_slave(p9,p10); +Serial pc(USBTX, USBRX); + +Motor LeftMotor(p21,p15,p16,1); //left motor +Motor RightMotor(p22,p17,p19,1); //right motor + +volatile int mode = 0; + +volatile float Lspeed_rx = 0; +volatile float Rspeed_rx = 0; +volatile float Fspeed_rx = 0; +volatile float Bspeed_rx = 0; + +void collision_protocol (int distance); + +ultrasonic usSensor(p6, p5, 0.050f, 1, &collision_protocol); + +void collision_protocol (int distance) { + + if ((distance < STOP_DISTANCE) && (distance > 50)) { + mode = 1; + + LeftMotor.speed(0); RightMotor.speed(0); + Timer ack_timer; + /* pc.printf("TOO CLOSE\r\n"); + pc.printf("\r\nListening on Bluetooth...\r\n"); */ + + float angle; + + hc05_slave.printf("rXX1,0.101,0.101"); /* SEND MAGIC COMMAND TO CONTROLLER */ + /* pc.printf("\r\nSENT\r\n"); */ + + LeftMotor.speed(-1); RightMotor.speed(-1); + Thread::wait(400); /* BACK UP TO AVOID BEING SET OFF AGAIN */ + LeftMotor.speed(0); RightMotor.speed(0); + + int x = 0; int dist = 0; + int idx; char *ack; + char response_buffer[25]; + memset(response_buffer, NULL, 25); /* ZERO INIT */ + ack_timer.start(); + while(x < 60) { + + idx = 0; ack = NULL; /* ACK --> NULLPTR */ + memset(response_buffer, NULL, 25); /* RESET BUFFER */ + + while(hc05_slave.readable() && idx < 25){ /* FILL RESPONSE BUFFER */ + response_buffer[idx] = hc05_slave.getc(); + idx++; + } + + /* pc.printf("\r\nRESPONSE: %s\r\n", response_buffer); */ + + ack = strstr(response_buffer, "_ack_"); /* FIND ACK */ + + if(ack != NULL) { /* ACK RECIEVED */ + pc.printf("Acknowledged - Sending reading %d\r\n",x); + angle = ((2*pi)/60) * x; /* add pi/2 */ + dist = usSensor.getCurrentDistance(); + if(dist >= 5000) { dist = 4999; } + /* pc.printf("1,%.3f,%.3f\r\n", (((float) dist)/500), angle); */ + hc05_slave.printf("r%02d1,%02.3f,%.3f", x,(((float) dist)/500), angle); + + LeftMotor.speed(RADAR_SPEED); RightMotor.speed(-1*RADAR_SPEED); + Thread::wait(RADAR_WAIT * 1000); /* TURN TO NEXT READING ANGLE */ + LeftMotor.speed(0); RightMotor.speed(0); + + ack_timer.reset(); x++; + } else { /* WAIT FOR ACK */ + pc.printf("Waiting for ack...\r\n"); + + if(ack_timer.read_ms() > 2000) { /* RETRANSMIT IF ACK NOT RECV'D */ + hc05_slave.printf("r%02d1,%02.3f,%.3f", x,(((float) dist)/500), angle); + ack_timer.reset(); /* pc.printf("Retransmitting...\r\n"); */ + } led_center_left = led_center_right = 1; + } + led_center_left = led_center_right = 1; + Thread::wait(100); + led_center_left = led_center_right = 0; + } mode = 0; /* RESET MODE */ + } +} + +void coll_start () { + + usSensor.startUpdates(); + Thread::wait(2000); + while(1) { + if (!mode) { + usSensor.checkDistance(); + /* pc.printf("%d", dist); */ + Thread::wait(50); + } + } +} + +void headlight_switch () { + + headlights = 0; + + while (1) { + if (lightSensor.read() < 0.09) { + headlights = 1; + } else if (lightSensor > 0.105) { + headlights = 0; + } + } +} + +void read_speeds () { + + char speed_buffer[11]; + char Lspeed_buff[5]; + char Rspeed_buff[5]; + char Fspeed_buff[5]; + char Bspeed_buff[5]; + char c; + + while (1) { + + if (hc05_slave.readable() && !mode) { + + memset(speed_buffer, NULL, 11); + + c = hc05_slave.getc(); + if((c == 'v') || (c == 'h')) { + int i = 0; + while (i < 11){ + if(hc05_slave.readable()) { + speed_buffer[i] = hc05_slave.getc(); + i++; + } + } + + if (c == 'h') { + + for (int j = 0; j < 5; j++) { + Lspeed_buff[j] = speed_buffer[j]; + } + + for (int j = 0; j < 5; j++) { + Rspeed_buff[j] = speed_buffer[j+6]; + } + + Lspeed_rx = atof(Lspeed_buff); + Rspeed_rx = atof(Rspeed_buff); + + } else if (c == 'v') { + + for (int j = 0; j < 5; j++) { + Fspeed_buff[j] = speed_buffer[j]; + } + + for (int j = 0; j < 5; j++) { + Bspeed_buff[j] = speed_buffer[j+6]; + } + + Fspeed_rx = atof(Fspeed_buff); + Bspeed_rx = atof(Bspeed_buff); + + } + + if (Fspeed_rx > 0.0f) { + + //pc.printf("forward\r\n"); + //pc.printf("LF: %f, RF: %f\r\n", (Fspeed_rx * 0.5f + Rspeed_rx * 0.4), Fspeed_rx * 0.5f + Lspeed_rx * 0.4f); + LeftMotor.speed(Fspeed_rx * 0.6f + Rspeed_rx * 0.4f); + RightMotor.speed(Fspeed_rx * 0.6f + Lspeed_rx * 0.4f); + + } else if (Bspeed_rx > 0.0f) { + + //pc.printf("reverse\r\n"); + //pc.printf("LF: %f, RF: %f\r\n", 0-(Bspeed_rx * 0.5f + Rspeed_rx * 0.4), 0-(Bspeed_rx * 0.5f + Lspeed_rx * 0.4f)); + LeftMotor.speed(0-(Bspeed_rx * 0.5f + Rspeed_rx * 0.4f)); + RightMotor.speed(0-(Bspeed_rx * 0.5f + Lspeed_rx * 0.4f)); + + } else { + + //pc.printf("turn only\r\n"); + //pc.printf("LF: %f, RF: %f\r\n", Rspeed_rx * 0.4f, Lspeed_rx * 0.4f); + LeftMotor.speed(Rspeed_rx * 0.4f); + RightMotor.speed(Lspeed_rx * 0.4f); + + } + } + } + } +} + +int main() { + + hc05_slave.baud(115200); + pc.baud(115200); + + Thread thread2(&read_speeds); + Thread thread3(&coll_start); + Thread thread4(&headlight_switch); + + while(1) { + if(!mode) { + led_left = led_right = 1; + wait(0.2); + led_left = led_right = 0; + wait(0.2); + } + } +}
diff -r 000000000000 -r 3ec03802c957 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Fri Apr 26 14:27:52 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706
diff -r 000000000000 -r 3ec03802c957 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Apr 26 14:27:52 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file