Andres Rodriguez / Mbed 2 deprecated Collision_Avoidance_Robot-Car

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

main.cpp@0:3ec03802c957, 2019-04-26 (annotated)

Committer:
Andres013
Date:
Fri Apr 26 14:27:52 2019 +0000
Revision:
0:3ec03802c957
FINAL COMMIT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Andres013 0:3ec03802c957 1 #include "mbed.h"
Andres013 0:3ec03802c957 2 #include "ultrasonic.h"
Andres013 0:3ec03802c957 3 #include "motordriver.h"
Andres013 0:3ec03802c957 4 #include "rtos.h"
Andres013 0:3ec03802c957 5
Andres013 0:3ec03802c957 6 #define STOP_DISTANCE 250
Andres013 0:3ec03802c957 7 #define RADAR_SPEED 0.5
Andres013 0:3ec03802c957 8 #define RADAR_WAIT 0.07
Andres013 0:3ec03802c957 9 #define pi 3.14159265f
Andres013 0:3ec03802c957 10
Andres013 0:3ec03802c957 11 /* LEDS TO SHOW WHICH MODE ROBOT IS IN */
Andres013 0:3ec03802c957 12 DigitalOut led_left(LED1);
Andres013 0:3ec03802c957 13 DigitalOut led_center_left(LED2);
Andres013 0:3ec03802c957 14 DigitalOut led_center_right(LED3);
Andres013 0:3ec03802c957 15 DigitalOut led_right(LED4);
Andres013 0:3ec03802c957 16
Andres013 0:3ec03802c957 17 AnalogIn lightSensor(p20);
Andres013 0:3ec03802c957 18 DigitalOut headlights(p30);
Andres013 0:3ec03802c957 19
Andres013 0:3ec03802c957 20 Serial hc05_slave(p9,p10);
Andres013 0:3ec03802c957 21 Serial pc(USBTX, USBRX);
Andres013 0:3ec03802c957 22
Andres013 0:3ec03802c957 23 Motor LeftMotor(p21,p15,p16,1); //left motor
Andres013 0:3ec03802c957 24 Motor RightMotor(p22,p17,p19,1); //right motor
Andres013 0:3ec03802c957 25
Andres013 0:3ec03802c957 26 volatile int mode = 0;
Andres013 0:3ec03802c957 27
Andres013 0:3ec03802c957 28 volatile float Lspeed_rx = 0;
Andres013 0:3ec03802c957 29 volatile float Rspeed_rx = 0;
Andres013 0:3ec03802c957 30 volatile float Fspeed_rx = 0;
Andres013 0:3ec03802c957 31 volatile float Bspeed_rx = 0;
Andres013 0:3ec03802c957 32
Andres013 0:3ec03802c957 33 void collision_protocol (int distance);
Andres013 0:3ec03802c957 34
Andres013 0:3ec03802c957 35 ultrasonic usSensor(p6, p5, 0.050f, 1, &collision_protocol);
Andres013 0:3ec03802c957 36
Andres013 0:3ec03802c957 37 void collision_protocol (int distance) {
Andres013 0:3ec03802c957 38
Andres013 0:3ec03802c957 39 if ((distance < STOP_DISTANCE) && (distance > 50)) {
Andres013 0:3ec03802c957 40 mode = 1;
Andres013 0:3ec03802c957 41
Andres013 0:3ec03802c957 42 LeftMotor.speed(0); RightMotor.speed(0);
Andres013 0:3ec03802c957 43 Timer ack_timer;
Andres013 0:3ec03802c957 44 /* pc.printf("TOO CLOSE\r\n");
Andres013 0:3ec03802c957 45 pc.printf("\r\nListening on Bluetooth...\r\n"); */
Andres013 0:3ec03802c957 46
Andres013 0:3ec03802c957 47 float angle;
Andres013 0:3ec03802c957 48
Andres013 0:3ec03802c957 49 hc05_slave.printf("rXX1,0.101,0.101"); /* SEND MAGIC COMMAND TO CONTROLLER */
Andres013 0:3ec03802c957 50 /* pc.printf("\r\nSENT\r\n"); */
Andres013 0:3ec03802c957 51
Andres013 0:3ec03802c957 52 LeftMotor.speed(-1); RightMotor.speed(-1);
Andres013 0:3ec03802c957 53 Thread::wait(400); /* BACK UP TO AVOID BEING SET OFF AGAIN */
Andres013 0:3ec03802c957 54 LeftMotor.speed(0); RightMotor.speed(0);
Andres013 0:3ec03802c957 55
Andres013 0:3ec03802c957 56 int x = 0; int dist = 0;
Andres013 0:3ec03802c957 57 int idx; char *ack;
Andres013 0:3ec03802c957 58 char response_buffer[25];
Andres013 0:3ec03802c957 59 memset(response_buffer, NULL, 25); /* ZERO INIT */
Andres013 0:3ec03802c957 60 ack_timer.start();
Andres013 0:3ec03802c957 61 while(x < 60) {
Andres013 0:3ec03802c957 62
Andres013 0:3ec03802c957 63 idx = 0; ack = NULL; /* ACK --> NULLPTR */
Andres013 0:3ec03802c957 64 memset(response_buffer, NULL, 25); /* RESET BUFFER */
Andres013 0:3ec03802c957 65
Andres013 0:3ec03802c957 66 while(hc05_slave.readable() && idx < 25){ /* FILL RESPONSE BUFFER */
Andres013 0:3ec03802c957 67 response_buffer[idx] = hc05_slave.getc();
Andres013 0:3ec03802c957 68 idx++;
Andres013 0:3ec03802c957 69 }
Andres013 0:3ec03802c957 70
Andres013 0:3ec03802c957 71 /* pc.printf("\r\nRESPONSE: %s\r\n", response_buffer); */
Andres013 0:3ec03802c957 72
Andres013 0:3ec03802c957 73 ack = strstr(response_buffer, "_ack_"); /* FIND ACK */
Andres013 0:3ec03802c957 74
Andres013 0:3ec03802c957 75 if(ack != NULL) { /* ACK RECIEVED */
Andres013 0:3ec03802c957 76 pc.printf("Acknowledged - Sending reading %d\r\n",x);
Andres013 0:3ec03802c957 77 angle = ((2*pi)/60) * x; /* add pi/2 */
Andres013 0:3ec03802c957 78 dist = usSensor.getCurrentDistance();
Andres013 0:3ec03802c957 79 if(dist >= 5000) { dist = 4999; }
Andres013 0:3ec03802c957 80 /* pc.printf("1,%.3f,%.3f\r\n", (((float) dist)/500), angle); */
Andres013 0:3ec03802c957 81 hc05_slave.printf("r%02d1,%02.3f,%.3f", x,(((float) dist)/500), angle);
Andres013 0:3ec03802c957 82
Andres013 0:3ec03802c957 83 LeftMotor.speed(RADAR_SPEED); RightMotor.speed(-1*RADAR_SPEED);
Andres013 0:3ec03802c957 84 Thread::wait(RADAR_WAIT * 1000); /* TURN TO NEXT READING ANGLE */
Andres013 0:3ec03802c957 85 LeftMotor.speed(0); RightMotor.speed(0);
Andres013 0:3ec03802c957 86
Andres013 0:3ec03802c957 87 ack_timer.reset(); x++;
Andres013 0:3ec03802c957 88 } else { /* WAIT FOR ACK */
Andres013 0:3ec03802c957 89 pc.printf("Waiting for ack...\r\n");
Andres013 0:3ec03802c957 90
Andres013 0:3ec03802c957 91 if(ack_timer.read_ms() > 2000) { /* RETRANSMIT IF ACK NOT RECV'D */
Andres013 0:3ec03802c957 92 hc05_slave.printf("r%02d1,%02.3f,%.3f", x,(((float) dist)/500), angle);
Andres013 0:3ec03802c957 93 ack_timer.reset(); /* pc.printf("Retransmitting...\r\n"); */
Andres013 0:3ec03802c957 94 } led_center_left = led_center_right = 1;
Andres013 0:3ec03802c957 95 }
Andres013 0:3ec03802c957 96 led_center_left = led_center_right = 1;
Andres013 0:3ec03802c957 97 Thread::wait(100);
Andres013 0:3ec03802c957 98 led_center_left = led_center_right = 0;
Andres013 0:3ec03802c957 99 } mode = 0; /* RESET MODE */
Andres013 0:3ec03802c957 100 }
Andres013 0:3ec03802c957 101 }
Andres013 0:3ec03802c957 102
Andres013 0:3ec03802c957 103 void coll_start () {
Andres013 0:3ec03802c957 104
Andres013 0:3ec03802c957 105 usSensor.startUpdates();
Andres013 0:3ec03802c957 106 Thread::wait(2000);
Andres013 0:3ec03802c957 107 while(1) {
Andres013 0:3ec03802c957 108 if (!mode) {
Andres013 0:3ec03802c957 109 usSensor.checkDistance();
Andres013 0:3ec03802c957 110 /* pc.printf("%d", dist); */
Andres013 0:3ec03802c957 111 Thread::wait(50);
Andres013 0:3ec03802c957 112 }
Andres013 0:3ec03802c957 113 }
Andres013 0:3ec03802c957 114 }
Andres013 0:3ec03802c957 115
Andres013 0:3ec03802c957 116 void headlight_switch () {
Andres013 0:3ec03802c957 117
Andres013 0:3ec03802c957 118 headlights = 0;
Andres013 0:3ec03802c957 119
Andres013 0:3ec03802c957 120 while (1) {
Andres013 0:3ec03802c957 121 if (lightSensor.read() < 0.09) {
Andres013 0:3ec03802c957 122 headlights = 1;
Andres013 0:3ec03802c957 123 } else if (lightSensor > 0.105) {
Andres013 0:3ec03802c957 124 headlights = 0;
Andres013 0:3ec03802c957 125 }
Andres013 0:3ec03802c957 126 }
Andres013 0:3ec03802c957 127 }
Andres013 0:3ec03802c957 128
Andres013 0:3ec03802c957 129 void read_speeds () {
Andres013 0:3ec03802c957 130
Andres013 0:3ec03802c957 131 char speed_buffer[11];
Andres013 0:3ec03802c957 132 char Lspeed_buff[5];
Andres013 0:3ec03802c957 133 char Rspeed_buff[5];
Andres013 0:3ec03802c957 134 char Fspeed_buff[5];
Andres013 0:3ec03802c957 135 char Bspeed_buff[5];
Andres013 0:3ec03802c957 136 char c;
Andres013 0:3ec03802c957 137
Andres013 0:3ec03802c957 138 while (1) {
Andres013 0:3ec03802c957 139
Andres013 0:3ec03802c957 140 if (hc05_slave.readable() && !mode) {
Andres013 0:3ec03802c957 141
Andres013 0:3ec03802c957 142 memset(speed_buffer, NULL, 11);
Andres013 0:3ec03802c957 143
Andres013 0:3ec03802c957 144 c = hc05_slave.getc();
Andres013 0:3ec03802c957 145 if((c == 'v') || (c == 'h')) {
Andres013 0:3ec03802c957 146 int i = 0;
Andres013 0:3ec03802c957 147 while (i < 11){
Andres013 0:3ec03802c957 148 if(hc05_slave.readable()) {
Andres013 0:3ec03802c957 149 speed_buffer[i] = hc05_slave.getc();
Andres013 0:3ec03802c957 150 i++;
Andres013 0:3ec03802c957 151 }
Andres013 0:3ec03802c957 152 }
Andres013 0:3ec03802c957 153
Andres013 0:3ec03802c957 154 if (c == 'h') {
Andres013 0:3ec03802c957 155
Andres013 0:3ec03802c957 156 for (int j = 0; j < 5; j++) {
Andres013 0:3ec03802c957 157 Lspeed_buff[j] = speed_buffer[j];
Andres013 0:3ec03802c957 158 }
Andres013 0:3ec03802c957 159
Andres013 0:3ec03802c957 160 for (int j = 0; j < 5; j++) {
Andres013 0:3ec03802c957 161 Rspeed_buff[j] = speed_buffer[j+6];
Andres013 0:3ec03802c957 162 }
Andres013 0:3ec03802c957 163
Andres013 0:3ec03802c957 164 Lspeed_rx = atof(Lspeed_buff);
Andres013 0:3ec03802c957 165 Rspeed_rx = atof(Rspeed_buff);
Andres013 0:3ec03802c957 166
Andres013 0:3ec03802c957 167 } else if (c == 'v') {
Andres013 0:3ec03802c957 168
Andres013 0:3ec03802c957 169 for (int j = 0; j < 5; j++) {
Andres013 0:3ec03802c957 170 Fspeed_buff[j] = speed_buffer[j];
Andres013 0:3ec03802c957 171 }
Andres013 0:3ec03802c957 172
Andres013 0:3ec03802c957 173 for (int j = 0; j < 5; j++) {
Andres013 0:3ec03802c957 174 Bspeed_buff[j] = speed_buffer[j+6];
Andres013 0:3ec03802c957 175 }
Andres013 0:3ec03802c957 176
Andres013 0:3ec03802c957 177 Fspeed_rx = atof(Fspeed_buff);
Andres013 0:3ec03802c957 178 Bspeed_rx = atof(Bspeed_buff);
Andres013 0:3ec03802c957 179
Andres013 0:3ec03802c957 180 }
Andres013 0:3ec03802c957 181
Andres013 0:3ec03802c957 182 if (Fspeed_rx > 0.0f) {
Andres013 0:3ec03802c957 183
Andres013 0:3ec03802c957 184 //pc.printf("forward\r\n");
Andres013 0:3ec03802c957 185 //pc.printf("LF: %f, RF: %f\r\n", (Fspeed_rx * 0.5f + Rspeed_rx * 0.4), Fspeed_rx * 0.5f + Lspeed_rx * 0.4f);
Andres013 0:3ec03802c957 186 LeftMotor.speed(Fspeed_rx * 0.6f + Rspeed_rx * 0.4f);
Andres013 0:3ec03802c957 187 RightMotor.speed(Fspeed_rx * 0.6f + Lspeed_rx * 0.4f);
Andres013 0:3ec03802c957 188
Andres013 0:3ec03802c957 189 } else if (Bspeed_rx > 0.0f) {
Andres013 0:3ec03802c957 190
Andres013 0:3ec03802c957 191 //pc.printf("reverse\r\n");
Andres013 0:3ec03802c957 192 //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));
Andres013 0:3ec03802c957 193 LeftMotor.speed(0-(Bspeed_rx * 0.5f + Rspeed_rx * 0.4f));
Andres013 0:3ec03802c957 194 RightMotor.speed(0-(Bspeed_rx * 0.5f + Lspeed_rx * 0.4f));
Andres013 0:3ec03802c957 195
Andres013 0:3ec03802c957 196 } else {
Andres013 0:3ec03802c957 197
Andres013 0:3ec03802c957 198 //pc.printf("turn only\r\n");
Andres013 0:3ec03802c957 199 //pc.printf("LF: %f, RF: %f\r\n", Rspeed_rx * 0.4f, Lspeed_rx * 0.4f);
Andres013 0:3ec03802c957 200 LeftMotor.speed(Rspeed_rx * 0.4f);
Andres013 0:3ec03802c957 201 RightMotor.speed(Lspeed_rx * 0.4f);
Andres013 0:3ec03802c957 202
Andres013 0:3ec03802c957 203 }
Andres013 0:3ec03802c957 204 }
Andres013 0:3ec03802c957 205 }
Andres013 0:3ec03802c957 206 }
Andres013 0:3ec03802c957 207 }
Andres013 0:3ec03802c957 208
Andres013 0:3ec03802c957 209 int main() {
Andres013 0:3ec03802c957 210
Andres013 0:3ec03802c957 211 hc05_slave.baud(115200);
Andres013 0:3ec03802c957 212 pc.baud(115200);
Andres013 0:3ec03802c957 213
Andres013 0:3ec03802c957 214 Thread thread2(&read_speeds);
Andres013 0:3ec03802c957 215 Thread thread3(&coll_start);
Andres013 0:3ec03802c957 216 Thread thread4(&headlight_switch);
Andres013 0:3ec03802c957 217
Andres013 0:3ec03802c957 218 while(1) {
Andres013 0:3ec03802c957 219 if(!mode) {
Andres013 0:3ec03802c957 220 led_left = led_right = 1;
Andres013 0:3ec03802c957 221 wait(0.2);
Andres013 0:3ec03802c957 222 led_left = led_right = 0;
Andres013 0:3ec03802c957 223 wait(0.2);
Andres013 0:3ec03802c957 224 }
Andres013 0:3ec03802c957 225 }
Andres013 0:3ec03802c957 226 }