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@1:0b28ff74c547, 2019-04-26 (annotated)

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