Andres Rodriguez / Mbed 2 deprecated Collision_Avoidance_Robot-Controller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos 4DGL-uLCD-SE

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

Committer:
Andres013
Date:
Fri Apr 26 14:24:41 2019 +0000
Revision:
0:a0902c8e1f7b
Child:
1:0b28ff74c547
FINAL COMMIT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Andres013 0:a0902c8e1f7b 1 #include "mbed.h"
Andres013 0:a0902c8e1f7b 2 #include "uLCD_4DGL.h"
Andres013 0:a0902c8e1f7b 3 #include "rtos.h"
Andres013 0:a0902c8e1f7b 4 #include "stdio.h"
Andres013 0:a0902c8e1f7b 5 #include <cstdlib>
Andres013 0:a0902c8e1f7b 6 #include "math.h"
Andres013 0:a0902c8e1f7b 7
Andres013 0:a0902c8e1f7b 8 //#define MIN_DIST = ?????
Andres013 0:a0902c8e1f7b 9
Andres013 0:a0902c8e1f7b 10 Mutex uLCD_mutex;
Andres013 0:a0902c8e1f7b 11
Andres013 0:a0902c8e1f7b 12 DigitalOut myled(LED1);
Andres013 0:a0902c8e1f7b 13
Andres013 0:a0902c8e1f7b 14 Serial hc05(p9, p10); //master HC05 bluetooth, *CHECK PINS*
Andres013 0:a0902c8e1f7b 15 Serial pc(USBTX, USBRX); //serial pc connection for testing, *ONLY USE WHEN TESTING*
Andres013 0:a0902c8e1f7b 16
Andres013 0:a0902c8e1f7b 17 uLCD_4DGL uLCD(p28, p27, p11); //uLCD for lidar imaging, *CHECK PINS*
Andres013 0:a0902c8e1f7b 18 AnalogOut speaker(p18); //speaker for collision detect noise
Andres013 0:a0902c8e1f7b 19
Andres013 0:a0902c8e1f7b 20 AnalogIn horz(p16); //left/right input from joystick, *CHECK PINS*
Andres013 0:a0902c8e1f7b 21 AnalogIn forward(p15); //forward input from joystick, *CHECK PINS*
Andres013 0:a0902c8e1f7b 22 //DigitalIn stop(p17); //stop input from joystick, *CHECK PINS*
Andres013 0:a0902c8e1f7b 23
Andres013 0:a0902c8e1f7b 24 volatile int mode = 0; //drive mode = 0 or collision detect mode = 1
Andres013 0:a0902c8e1f7b 25
Andres013 0:a0902c8e1f7b 26 volatile float Lspeed_tx = 0; //left speed for transmission
Andres013 0:a0902c8e1f7b 27 volatile float Rspeed_tx = 0; //right speed for transmission
Andres013 0:a0902c8e1f7b 28 volatile float Fspeed_tx = 0; //forward speed for transmission
Andres013 0:a0902c8e1f7b 29 volatile float Bspeed_tx = 0; //reverse speed for transmission
Andres013 0:a0902c8e1f7b 30
Andres013 0:a0902c8e1f7b 31 volatile float collision_dist; //collision distance read from lidar sensor on robot
Andres013 0:a0902c8e1f7b 32 volatile float collision_angle; //collision angle read from robot as input on servo controlling the lidar
Andres013 0:a0902c8e1f7b 33
Andres013 0:a0902c8e1f7b 34 // transmission of speeds from the master BT to the slave BT
Andres013 0:a0902c8e1f7b 35 void transmit() {
Andres013 0:a0902c8e1f7b 36
Andres013 0:a0902c8e1f7b 37 while (1) {
Andres013 0:a0902c8e1f7b 38 if (!mode) {
Andres013 0:a0902c8e1f7b 39
Andres013 0:a0902c8e1f7b 40 hc05.printf("h%.3f,%.3f", Lspeed_tx, Rspeed_tx);
Andres013 0:a0902c8e1f7b 41 Thread::wait(50);
Andres013 0:a0902c8e1f7b 42 hc05.printf("v%.3f,%.3f", Fspeed_tx, Bspeed_tx);
Andres013 0:a0902c8e1f7b 43 /* pc.printf("H: %.3f,%.3f, V: %.3f,%.3f\r\n", Lspeed_tx, Rspeed_tx, Fspeed_tx, Bspeed_tx); */
Andres013 0:a0902c8e1f7b 44 }
Andres013 0:a0902c8e1f7b 45
Andres013 0:a0902c8e1f7b 46 Thread::wait(50);
Andres013 0:a0902c8e1f7b 47 }
Andres013 0:a0902c8e1f7b 48
Andres013 0:a0902c8e1f7b 49 }
Andres013 0:a0902c8e1f7b 50
Andres013 0:a0902c8e1f7b 51 //reading of data from the joystick and storing it into the values transmitted
Andres013 0:a0902c8e1f7b 52 //by the HC05 on the controller
Andres013 0:a0902c8e1f7b 53 void joystick_read() {
Andres013 0:a0902c8e1f7b 54
Andres013 0:a0902c8e1f7b 55 while (1) {
Andres013 0:a0902c8e1f7b 56
Andres013 0:a0902c8e1f7b 57 if (!mode) {
Andres013 0:a0902c8e1f7b 58
Andres013 0:a0902c8e1f7b 59 if (horz.read() > 0.65f) {
Andres013 0:a0902c8e1f7b 60 Lspeed_tx = (horz.read() - 0.5f) * 2;
Andres013 0:a0902c8e1f7b 61 Rspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 62 } else if (horz.read() < 0.45f) {
Andres013 0:a0902c8e1f7b 63 Rspeed_tx = (0.60f - horz.read()) * 2;
Andres013 0:a0902c8e1f7b 64 Lspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 65 } else {
Andres013 0:a0902c8e1f7b 66 Lspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 67 Rspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 68 }
Andres013 0:a0902c8e1f7b 69
Andres013 0:a0902c8e1f7b 70 if (forward.read() > 0.65f) {
Andres013 0:a0902c8e1f7b 71 Fspeed_tx = (forward.read() - 0.5f) * 2;
Andres013 0:a0902c8e1f7b 72 Bspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 73 } else if (forward < 0.45f) {
Andres013 0:a0902c8e1f7b 74 Bspeed_tx = (0.55f - forward.read()) * 2;
Andres013 0:a0902c8e1f7b 75 Fspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 76 } else {
Andres013 0:a0902c8e1f7b 77 Fspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 78 Bspeed_tx = 0.0f;
Andres013 0:a0902c8e1f7b 79 }
Andres013 0:a0902c8e1f7b 80
Andres013 0:a0902c8e1f7b 81 } else {
Andres013 0:a0902c8e1f7b 82
Andres013 0:a0902c8e1f7b 83 Lspeed_tx = 0;
Andres013 0:a0902c8e1f7b 84 Rspeed_tx = 0;
Andres013 0:a0902c8e1f7b 85 Fspeed_tx = 0;
Andres013 0:a0902c8e1f7b 86 Bspeed_tx = 0;
Andres013 0:a0902c8e1f7b 87
Andres013 0:a0902c8e1f7b 88 }
Andres013 0:a0902c8e1f7b 89
Andres013 0:a0902c8e1f7b 90 Thread::wait(100);
Andres013 0:a0902c8e1f7b 91 }
Andres013 0:a0902c8e1f7b 92
Andres013 0:a0902c8e1f7b 93 }
Andres013 0:a0902c8e1f7b 94
Andres013 0:a0902c8e1f7b 95 //collision avoidance protocol
Andres013 0:a0902c8e1f7b 96 void collision_protocol() {
Andres013 0:a0902c8e1f7b 97
Andres013 0:a0902c8e1f7b 98 char collision_buffer[16];
Andres013 0:a0902c8e1f7b 99 char dist_buffer[6];
Andres013 0:a0902c8e1f7b 100 char angle_buffer[5];
Andres013 0:a0902c8e1f7b 101 int a;
Andres013 0:a0902c8e1f7b 102 bool good_buff;
Andres013 0:a0902c8e1f7b 103 Timer ack_timer;
Andres013 0:a0902c8e1f7b 104
Andres013 0:a0902c8e1f7b 105 while (1) {
Andres013 0:a0902c8e1f7b 106 if (hc05.readable()) {
Andres013 0:a0902c8e1f7b 107 /* pc.printf("Reading HC05\r\n"); */
Andres013 0:a0902c8e1f7b 108 memset(collision_buffer, NULL, 16); /* ZERO INIT */
Andres013 0:a0902c8e1f7b 109 a = 0;
Andres013 0:a0902c8e1f7b 110 while(a < 16) {
Andres013 0:a0902c8e1f7b 111 if(hc05.readable()) {
Andres013 0:a0902c8e1f7b 112 collision_buffer[a] = hc05.getc();
Andres013 0:a0902c8e1f7b 113 if(collision_buffer[0] == 'r') { a++; } /* START AT 'R' */
Andres013 0:a0902c8e1f7b 114 }
Andres013 0:a0902c8e1f7b 115 } pc.printf("%s\r\n", collision_buffer);
Andres013 0:a0902c8e1f7b 116
Andres013 0:a0902c8e1f7b 117 /* PSEUDO-CHECKSUM: CHECK ALL KNOWN POSITIONS TO VERIFY BUFFER */
Andres013 0:a0902c8e1f7b 118 good_buff = ( (collision_buffer[0] == 'r') && \
Andres013 0:a0902c8e1f7b 119 (collision_buffer[3] == '1') && \
Andres013 0:a0902c8e1f7b 120 (collision_buffer[4] == ',') && \
Andres013 0:a0902c8e1f7b 121 (collision_buffer[6] == '.') && \
Andres013 0:a0902c8e1f7b 122 (collision_buffer[10] == ',') && \
Andres013 0:a0902c8e1f7b 123 (collision_buffer[12] == '.') );
Andres013 0:a0902c8e1f7b 124
Andres013 0:a0902c8e1f7b 125 if(good_buff){ mode = 1; } /* SET MODE */
Andres013 0:a0902c8e1f7b 126
Andres013 0:a0902c8e1f7b 127
Andres013 0:a0902c8e1f7b 128 if (mode) {
Andres013 0:a0902c8e1f7b 129 /* pc.printf("Collision Avoidance Engaged\r\n"); */
Andres013 0:a0902c8e1f7b 130 uLCD.cls(); /* INIT RADAR SCREEN */
Andres013 0:a0902c8e1f7b 131 uLCD.filled_circle(64, 64, 5, (RED+GREEN));
Andres013 0:a0902c8e1f7b 132 Thread::wait(2*1000);
Andres013 0:a0902c8e1f7b 133 ack_timer.start();
Andres013 0:a0902c8e1f7b 134 int i = 0;
Andres013 0:a0902c8e1f7b 135 while(i < 60) {
Andres013 0:a0902c8e1f7b 136 good_buff = ( (collision_buffer[0] == 'r') && \
Andres013 0:a0902c8e1f7b 137 (collision_buffer[3] == '1') && \
Andres013 0:a0902c8e1f7b 138 (collision_buffer[4] == ',') );
Andres013 0:a0902c8e1f7b 139
Andres013 0:a0902c8e1f7b 140 if(good_buff) {
Andres013 0:a0902c8e1f7b 141 i++;
Andres013 0:a0902c8e1f7b 142 hc05.printf("_ack_%02d", (i-1)); /* ACKNOWLEDGE */
Andres013 0:a0902c8e1f7b 143 /* pc.printf("_ack_%02d\r\n", i); */
Andres013 0:a0902c8e1f7b 144 ack_timer.reset();
Andres013 0:a0902c8e1f7b 145 }
Andres013 0:a0902c8e1f7b 146
Andres013 0:a0902c8e1f7b 147 while(!hc05.readable()) { } /* WAIT FOR NEW DATA */
Andres013 0:a0902c8e1f7b 148 memset(collision_buffer, NULL, 16); /* RESET BUFFER */
Andres013 0:a0902c8e1f7b 149 /* pc.printf("Buffer reset\r\n"); */
Andres013 0:a0902c8e1f7b 150 a = 0;
Andres013 0:a0902c8e1f7b 151 while(a < 16) { /* FILL BUFFER */
Andres013 0:a0902c8e1f7b 152 if(hc05.readable()) {
Andres013 0:a0902c8e1f7b 153 collision_buffer[a] = hc05.getc();
Andres013 0:a0902c8e1f7b 154 if(collision_buffer[0] == 'r') { a++; }
Andres013 0:a0902c8e1f7b 155 }
Andres013 0:a0902c8e1f7b 156 }
Andres013 0:a0902c8e1f7b 157 /* PSEUDO-CHECKSUM: CHECK ALL KNOWN POSITIONS TO VERIFY BUFFER */
Andres013 0:a0902c8e1f7b 158 good_buff = ( (collision_buffer[0] == 'r') && \
Andres013 0:a0902c8e1f7b 159 (collision_buffer[3] == '1') && \
Andres013 0:a0902c8e1f7b 160 (collision_buffer[4] == ',') );
Andres013 0:a0902c8e1f7b 161
Andres013 0:a0902c8e1f7b 162 if(good_buff) { /* ONLY USE IF GOOD BUFFER */
Andres013 0:a0902c8e1f7b 163 /* pc.printf("Buffer: %s\r\n", collision_buffer); */
Andres013 0:a0902c8e1f7b 164
Andres013 0:a0902c8e1f7b 165 for (int j = 0; j < 5; j++) {
Andres013 0:a0902c8e1f7b 166 dist_buffer[j] = collision_buffer[j+6];
Andres013 0:a0902c8e1f7b 167 }
Andres013 0:a0902c8e1f7b 168
Andres013 0:a0902c8e1f7b 169 for (int j = 0; j < 5; j++) {
Andres013 0:a0902c8e1f7b 170 angle_buffer[j] = collision_buffer[j+11];
Andres013 0:a0902c8e1f7b 171 }
Andres013 0:a0902c8e1f7b 172
Andres013 0:a0902c8e1f7b 173 collision_dist = (float) atof(dist_buffer);
Andres013 0:a0902c8e1f7b 174 collision_angle = (float) atof(angle_buffer);
Andres013 0:a0902c8e1f7b 175
Andres013 0:a0902c8e1f7b 176
Andres013 0:a0902c8e1f7b 177 int x = (int) (64.0f + (collision_dist * 60 * cos(collision_angle)));
Andres013 0:a0902c8e1f7b 178 int y = (int) (64.0f + (collision_dist * 60 * sin(collision_angle)));
Andres013 0:a0902c8e1f7b 179
Andres013 0:a0902c8e1f7b 180 if (x >= 0 && y >= 0) {
Andres013 0:a0902c8e1f7b 181 uLCD_mutex.lock();
Andres013 0:a0902c8e1f7b 182 uLCD.filled_circle(x, y, 2, GREEN);
Andres013 0:a0902c8e1f7b 183 uLCD_mutex.unlock();
Andres013 0:a0902c8e1f7b 184 }
Andres013 0:a0902c8e1f7b 185 } else if(ack_timer.read_ms() > 2000) {
Andres013 0:a0902c8e1f7b 186 hc05.printf("_ack_%02d", (i-1)); /* ACKNOWLEDGE */
Andres013 0:a0902c8e1f7b 187 /* pc.printf("Resending: _ack_%02d\r\n", i); */
Andres013 0:a0902c8e1f7b 188 ack_timer.reset();
Andres013 0:a0902c8e1f7b 189 }
Andres013 0:a0902c8e1f7b 190 } hc05.printf("_ack_59"); /* SEND FINAL ACKNOWLEDGEMENT */
Andres013 0:a0902c8e1f7b 191 Thread::wait(5000);
Andres013 0:a0902c8e1f7b 192 mode = 0; /* RESET MODE */
Andres013 0:a0902c8e1f7b 193 }
Andres013 0:a0902c8e1f7b 194 }
Andres013 0:a0902c8e1f7b 195
Andres013 0:a0902c8e1f7b 196 Thread::wait(100);
Andres013 0:a0902c8e1f7b 197
Andres013 0:a0902c8e1f7b 198 }
Andres013 0:a0902c8e1f7b 199 }
Andres013 0:a0902c8e1f7b 200
Andres013 0:a0902c8e1f7b 201
Andres013 0:a0902c8e1f7b 202
Andres013 0:a0902c8e1f7b 203 int main() {
Andres013 0:a0902c8e1f7b 204
Andres013 0:a0902c8e1f7b 205 hc05.baud(115200);
Andres013 0:a0902c8e1f7b 206 pc.baud(115200);
Andres013 0:a0902c8e1f7b 207 uLCD.baudrate(115200);
Andres013 0:a0902c8e1f7b 208 uLCD.background_color(BLACK);
Andres013 0:a0902c8e1f7b 209
Andres013 0:a0902c8e1f7b 210 Thread thread2(&transmit);
Andres013 0:a0902c8e1f7b 211 Thread thread3(&joystick_read);
Andres013 0:a0902c8e1f7b 212 Thread thread4(&collision_protocol);
Andres013 0:a0902c8e1f7b 213
Andres013 0:a0902c8e1f7b 214 while(1) {
Andres013 0:a0902c8e1f7b 215 myled = 1;
Andres013 0:a0902c8e1f7b 216 wait(0.2);
Andres013 0:a0902c8e1f7b 217 myled = 0;
Andres013 0:a0902c8e1f7b 218 wait(0.2);
Andres013 0:a0902c8e1f7b 219 }
Andres013 0:a0902c8e1f7b 220 }
Andres013 0:a0902c8e1f7b 221
Andres013 0:a0902c8e1f7b 222
Andres013 0:a0902c8e1f7b 223
Andres013 0:a0902c8e1f7b 224
Andres013 0:a0902c8e1f7b 225
Andres013 0:a0902c8e1f7b 226
Andres013 0:a0902c8e1f7b 227
Andres013 0:a0902c8e1f7b 228
Andres013 0:a0902c8e1f7b 229
Andres013 0:a0902c8e1f7b 230
Andres013 0:a0902c8e1f7b 231
Andres013 0:a0902c8e1f7b 232