Semi-Autonomous-Robot / Semi Autonomous Robot - ECE 4180

Code for controlling mbed hardware (LED's, motors), as well as code for the Raspberry Pi to run a Support Vector Machine that identifies objects using the Pi camera

Dependencies:   mbed Motordriver mbed-rtos PololuLedStrip

mbed/main.cpp@3:a3ed7ff99772, 2019-12-06 (annotated)

Committer:
arogliero3
Date:
Fri Dec 06 00:58:02 2019 -0500
Revision:
3:a3ed7ff99772
Parent:
0:e0dbd261724a 
update img6

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arogliero3 0:e0dbd261724a 1 #include "mbed.h"
arogliero3 0:e0dbd261724a 2 #include "rtos.h"
arogliero3 0:e0dbd261724a 3 #include "motordriver.h"
arogliero3 0:e0dbd261724a 4 #include "PololuLedStrip.h"
arogliero3 0:e0dbd261724a 5 // Connect mbed to Pi USB
arogliero3 0:e0dbd261724a 6 Serial pi(USBTX, USBRX);
arogliero3 0:e0dbd261724a 7
arogliero3 0:e0dbd261724a 8 Serial blue(p9, p10);
arogliero3 0:e0dbd261724a 9
arogliero3 0:e0dbd261724a 10 // Set up motors
arogliero3 0:e0dbd261724a 11 Motor L(p24, p23, p22, 1); // pwmA, fwd, rev, can brake
arogliero3 0:e0dbd261724a 12 Motor R(p21, p8, p7, 1); // pwmB, fwd, rev, can brake
arogliero3 0:e0dbd261724a 13
arogliero3 0:e0dbd261724a 14 PololuLedStrip ledStrip(p5);
arogliero3 0:e0dbd261724a 15
arogliero3 0:e0dbd261724a 16 // for LED strip
arogliero3 0:e0dbd261724a 17 Timer timer;
arogliero3 0:e0dbd261724a 18
arogliero3 0:e0dbd261724a 19 volatile bool disco_mode = false;
arogliero3 0:e0dbd261724a 20 volatile bool tornado_mode = false;
arogliero3 0:e0dbd261724a 21 volatile bool autonomous_mode = false;
arogliero3 0:e0dbd261724a 22
arogliero3 0:e0dbd261724a 23 #define LED_COUNT 82
arogliero3 0:e0dbd261724a 24 rgb_color colors[LED_COUNT];
arogliero3 0:e0dbd261724a 25
arogliero3 0:e0dbd261724a 26
arogliero3 0:e0dbd261724a 27 // Converts a color from the HSV representation to RGB.
arogliero3 0:e0dbd261724a 28 rgb_color hsvToRgb(float h, float s, float v)
arogliero3 0:e0dbd261724a 29 {
arogliero3 0:e0dbd261724a 30 int i = floor(h * 6);
arogliero3 0:e0dbd261724a 31 float f = h * 6 - i;
arogliero3 0:e0dbd261724a 32 float p = v * (1 - s);
arogliero3 0:e0dbd261724a 33 float q = v * (1 - f * s);
arogliero3 0:e0dbd261724a 34 float t = v * (1 - (1 - f) * s);
arogliero3 0:e0dbd261724a 35 float r = 0, g = 0, b = 0;
arogliero3 0:e0dbd261724a 36 switch(i % 6){
arogliero3 0:e0dbd261724a 37 case 0: r = v; g = t; b = p; break;
arogliero3 0:e0dbd261724a 38 case 1: r = q; g = v; b = p; break;
arogliero3 0:e0dbd261724a 39 case 2: r = p; g = v; b = t; break;
arogliero3 0:e0dbd261724a 40 case 3: r = p; g = q; b = v; break;
arogliero3 0:e0dbd261724a 41 case 4: r = t; g = p; b = v; break;
arogliero3 0:e0dbd261724a 42 case 5: r = v; g = p; b = q; break;
arogliero3 0:e0dbd261724a 43 }
arogliero3 0:e0dbd261724a 44 return (rgb_color){r * 255, g * 255, b * 255};
arogliero3 0:e0dbd261724a 45 }
arogliero3 0:e0dbd261724a 46
arogliero3 0:e0dbd261724a 47 #define TIME_SLICE 5.0f
arogliero3 0:e0dbd261724a 48
arogliero3 0:e0dbd261724a 49 // LEDs for debugging
arogliero3 0:e0dbd261724a 50
arogliero3 0:e0dbd261724a 51 // Invalid character was read
arogliero3 0:e0dbd261724a 52 DigitalOut led1(LED1);
arogliero3 0:e0dbd261724a 53
arogliero3 0:e0dbd261724a 54 // Detected tornado
arogliero3 0:e0dbd261724a 55 DigitalOut led2(LED2);
arogliero3 0:e0dbd261724a 56 DigitalOut led3(LED3);
arogliero3 0:e0dbd261724a 57
arogliero3 0:e0dbd261724a 58 // Detected disco_ball
arogliero3 0:e0dbd261724a 59 DigitalOut led4(LED4);
arogliero3 0:e0dbd261724a 60
arogliero3 0:e0dbd261724a 61
arogliero3 0:e0dbd261724a 62 void forward()
arogliero3 0:e0dbd261724a 63 {
arogliero3 0:e0dbd261724a 64 L.speed(0.5);
arogliero3 0:e0dbd261724a 65 R.speed(0.5);
arogliero3 0:e0dbd261724a 66 wait(TIME_SLICE/10);
arogliero3 0:e0dbd261724a 67 L.coast();
arogliero3 0:e0dbd261724a 68 R.coast();
arogliero3 0:e0dbd261724a 69 }
arogliero3 0:e0dbd261724a 70
arogliero3 0:e0dbd261724a 71 void backward()
arogliero3 0:e0dbd261724a 72 {
arogliero3 0:e0dbd261724a 73 L.speed(-0.5);
arogliero3 0:e0dbd261724a 74 R.speed(-0.5);
arogliero3 0:e0dbd261724a 75 wait(TIME_SLICE/10);
arogliero3 0:e0dbd261724a 76 L.coast();
arogliero3 0:e0dbd261724a 77 R.coast();
arogliero3 0:e0dbd261724a 78 }
arogliero3 0:e0dbd261724a 79
arogliero3 0:e0dbd261724a 80 void left()
arogliero3 0:e0dbd261724a 81 {
arogliero3 0:e0dbd261724a 82 L.speed(-0.5);
arogliero3 0:e0dbd261724a 83 R.speed(0.5);
arogliero3 0:e0dbd261724a 84 wait(TIME_SLICE/10);
arogliero3 0:e0dbd261724a 85 L.coast();
arogliero3 0:e0dbd261724a 86 R.coast();
arogliero3 0:e0dbd261724a 87 }
arogliero3 0:e0dbd261724a 88
arogliero3 0:e0dbd261724a 89 void right()
arogliero3 0:e0dbd261724a 90 {
arogliero3 0:e0dbd261724a 91 L.speed(0.5);
arogliero3 0:e0dbd261724a 92 R.speed(-0.5);
arogliero3 0:e0dbd261724a 93 wait(TIME_SLICE/10);
arogliero3 0:e0dbd261724a 94 L.coast();
arogliero3 0:e0dbd261724a 95 R.coast();
arogliero3 0:e0dbd261724a 96 }
arogliero3 0:e0dbd261724a 97
arogliero3 0:e0dbd261724a 98 void stop()
arogliero3 0:e0dbd261724a 99 {
arogliero3 0:e0dbd261724a 100 L.stop(0);
arogliero3 0:e0dbd261724a 101 R.stop(0);
arogliero3 0:e0dbd261724a 102 }
arogliero3 0:e0dbd261724a 103
arogliero3 0:e0dbd261724a 104 void led_strip()
arogliero3 0:e0dbd261724a 105 {
arogliero3 0:e0dbd261724a 106 timer.start();
arogliero3 0:e0dbd261724a 107
arogliero3 0:e0dbd261724a 108 while(1)
arogliero3 0:e0dbd261724a 109 {
arogliero3 0:e0dbd261724a 110 if (disco_mode)
arogliero3 0:e0dbd261724a 111 {
arogliero3 0:e0dbd261724a 112 // Update the colors array.
arogliero3 0:e0dbd261724a 113 uint32_t time = timer.read_ms();
arogliero3 0:e0dbd261724a 114 for(int i = 0; i < LED_COUNT; i++)
arogliero3 0:e0dbd261724a 115 {
arogliero3 0:e0dbd261724a 116 uint8_t phase = (time >> 4) - (i << 2);
arogliero3 0:e0dbd261724a 117 colors[i] = hsvToRgb(phase / 256.0, 1.0, 0.25);
arogliero3 0:e0dbd261724a 118 }
arogliero3 0:e0dbd261724a 119
arogliero3 0:e0dbd261724a 120 // Send the colors to the LED strip.
arogliero3 0:e0dbd261724a 121 ledStrip.write(colors, LED_COUNT);
arogliero3 0:e0dbd261724a 122 }
arogliero3 0:e0dbd261724a 123 else if (tornado_mode)
arogliero3 0:e0dbd261724a 124 {
arogliero3 0:e0dbd261724a 125 rgb_color temp;
arogliero3 0:e0dbd261724a 126 temp.red = 64;
arogliero3 0:e0dbd261724a 127 temp.green = 64;
arogliero3 0:e0dbd261724a 128 temp.blue = 0;
arogliero3 0:e0dbd261724a 129 for (int i = 0; i < LED_COUNT; i++)
arogliero3 0:e0dbd261724a 130 {
arogliero3 0:e0dbd261724a 131 colors[i] = temp;
arogliero3 0:e0dbd261724a 132 }
arogliero3 0:e0dbd261724a 133 ledStrip.write(colors, LED_COUNT);
arogliero3 0:e0dbd261724a 134 }
arogliero3 0:e0dbd261724a 135 else
arogliero3 0:e0dbd261724a 136 {
arogliero3 0:e0dbd261724a 137 rgb_color temp;
arogliero3 0:e0dbd261724a 138 temp.red = 64;
arogliero3 0:e0dbd261724a 139 temp.green = 0;
arogliero3 0:e0dbd261724a 140 temp.blue = 0;
arogliero3 0:e0dbd261724a 141 for (int i = 0; i < LED_COUNT; i++)
arogliero3 0:e0dbd261724a 142 {
arogliero3 0:e0dbd261724a 143 if (i == LED_COUNT/2)
arogliero3 0:e0dbd261724a 144 {
arogliero3 0:e0dbd261724a 145 temp.red = 0;
arogliero3 0:e0dbd261724a 146 temp.green = 64;
arogliero3 0:e0dbd261724a 147 }
arogliero3 0:e0dbd261724a 148 colors[i] = temp;
arogliero3 0:e0dbd261724a 149 }
arogliero3 0:e0dbd261724a 150 ledStrip.write(colors, LED_COUNT);
arogliero3 0:e0dbd261724a 151 }
arogliero3 0:e0dbd261724a 152 }
arogliero3 0:e0dbd261724a 153 }
arogliero3 0:e0dbd261724a 154
arogliero3 0:e0dbd261724a 155 void detected_tornado()
arogliero3 0:e0dbd261724a 156 {
arogliero3 0:e0dbd261724a 157 tornado_mode = true;
arogliero3 0:e0dbd261724a 158 for (int i = 0; i < 10; i++)
arogliero3 0:e0dbd261724a 159 {
arogliero3 0:e0dbd261724a 160 left();
arogliero3 0:e0dbd261724a 161 wait(0.5);
arogliero3 0:e0dbd261724a 162 }
arogliero3 0:e0dbd261724a 163 stop();
arogliero3 0:e0dbd261724a 164 tornado_mode = false;
arogliero3 0:e0dbd261724a 165 }
arogliero3 0:e0dbd261724a 166
arogliero3 0:e0dbd261724a 167 void detected_disco_ball()
arogliero3 0:e0dbd261724a 168 {
arogliero3 0:e0dbd261724a 169 led4 = 1;
arogliero3 0:e0dbd261724a 170 disco_mode = true;
arogliero3 0:e0dbd261724a 171 left();
arogliero3 0:e0dbd261724a 172 left();
arogliero3 0:e0dbd261724a 173 right();
arogliero3 0:e0dbd261724a 174 right();
arogliero3 0:e0dbd261724a 175 left();
arogliero3 0:e0dbd261724a 176 left();
arogliero3 0:e0dbd261724a 177 right();
arogliero3 0:e0dbd261724a 178 right();
arogliero3 0:e0dbd261724a 179 forward();
arogliero3 0:e0dbd261724a 180 backward();
arogliero3 0:e0dbd261724a 181 disco_mode = false;
arogliero3 0:e0dbd261724a 182
arogliero3 0:e0dbd261724a 183 }
arogliero3 0:e0dbd261724a 184
arogliero3 0:e0dbd261724a 185 void execute(char c)
arogliero3 0:e0dbd261724a 186 {
arogliero3 0:e0dbd261724a 187 switch (c)
arogliero3 0:e0dbd261724a 188 {
arogliero3 0:e0dbd261724a 189 case 'f':
arogliero3 0:e0dbd261724a 190 forward();
arogliero3 0:e0dbd261724a 191 break;
arogliero3 0:e0dbd261724a 192 case 'b':
arogliero3 0:e0dbd261724a 193 backward();
arogliero3 0:e0dbd261724a 194 break;
arogliero3 0:e0dbd261724a 195 case 'l':
arogliero3 0:e0dbd261724a 196 left();
arogliero3 0:e0dbd261724a 197 break;
arogliero3 0:e0dbd261724a 198 case 'r':
arogliero3 0:e0dbd261724a 199 right();
arogliero3 0:e0dbd261724a 200 break;
arogliero3 0:e0dbd261724a 201 case 't':
arogliero3 0:e0dbd261724a 202 detected_tornado();
arogliero3 0:e0dbd261724a 203 break;
arogliero3 0:e0dbd261724a 204 case 'd':
arogliero3 0:e0dbd261724a 205 detected_disco_ball();
arogliero3 0:e0dbd261724a 206 break;
arogliero3 0:e0dbd261724a 207 case 's':
arogliero3 0:e0dbd261724a 208 stop();
arogliero3 0:e0dbd261724a 209 break;
arogliero3 0:e0dbd261724a 210 default:
arogliero3 0:e0dbd261724a 211 // invalid character
arogliero3 0:e0dbd261724a 212 led1 = 1;
arogliero3 0:e0dbd261724a 213 break;
arogliero3 0:e0dbd261724a 214 }
arogliero3 0:e0dbd261724a 215 }
arogliero3 0:e0dbd261724a 216
arogliero3 0:e0dbd261724a 217 int main()
arogliero3 0:e0dbd261724a 218 {
arogliero3 0:e0dbd261724a 219 led1 = 0;
arogliero3 0:e0dbd261724a 220 led2 = 0;
arogliero3 0:e0dbd261724a 221 led3 = 0;
arogliero3 0:e0dbd261724a 222 led4 = 0;
arogliero3 0:e0dbd261724a 223 pi.baud(9600);
arogliero3 0:e0dbd261724a 224 char bnum, bhit;
arogliero3 0:e0dbd261724a 225
arogliero3 0:e0dbd261724a 226 Thread t1;
arogliero3 0:e0dbd261724a 227 t1.start(led_strip);
arogliero3 0:e0dbd261724a 228
arogliero3 0:e0dbd261724a 229 while(1) {
arogliero3 0:e0dbd261724a 230 // pi commands are read first
arogliero3 0:e0dbd261724a 231 if(pi.readable()) {
arogliero3 0:e0dbd261724a 232 char c = pi.getc();
arogliero3 0:e0dbd261724a 233 if (c != 'n')
arogliero3 0:e0dbd261724a 234 execute(c);
arogliero3 0:e0dbd261724a 235 switch (c)
arogliero3 0:e0dbd261724a 236 {
arogliero3 0:e0dbd261724a 237 case 'd':
arogliero3 0:e0dbd261724a 238 case 't':
arogliero3 0:e0dbd261724a 239 pi.putc('f');
arogliero3 0:e0dbd261724a 240 autonomous_mode = false;
arogliero3 0:e0dbd261724a 241 break;
arogliero3 0:e0dbd261724a 242 default:
arogliero3 0:e0dbd261724a 243 pi.putc(c);
arogliero3 0:e0dbd261724a 244 break;
arogliero3 0:e0dbd261724a 245 }
arogliero3 0:e0dbd261724a 246 }
arogliero3 0:e0dbd261724a 247
arogliero3 0:e0dbd261724a 248 // bluetooth commands are read later
arogliero3 0:e0dbd261724a 249 if (!blue.readable()) continue;
arogliero3 0:e0dbd261724a 250 if (blue.getc()=='!') {
arogliero3 0:e0dbd261724a 251 if (blue.getc()=='B') { //button data packet
arogliero3 0:e0dbd261724a 252 bnum = blue.getc(); //button number
arogliero3 0:e0dbd261724a 253 bhit = blue.getc(); //1=hit, 0=release
arogliero3 0:e0dbd261724a 254 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
arogliero3 0:e0dbd261724a 255 //myled = bnum - '0'; //current button number will appear on LEDs
arogliero3 0:e0dbd261724a 256 switch (bnum) {
arogliero3 0:e0dbd261724a 257 case '1': //number button 1
arogliero3 0:e0dbd261724a 258 if (bhit=='1') {
arogliero3 0:e0dbd261724a 259 autonomous_mode = !autonomous_mode;
arogliero3 0:e0dbd261724a 260 if (!autonomous_mode)
arogliero3 0:e0dbd261724a 261 {
arogliero3 0:e0dbd261724a 262 pi.putc('f');
arogliero3 0:e0dbd261724a 263 }
arogliero3 0:e0dbd261724a 264 else
arogliero3 0:e0dbd261724a 265 {
arogliero3 0:e0dbd261724a 266 pi.putc('a');
arogliero3 0:e0dbd261724a 267 }
arogliero3 0:e0dbd261724a 268 //add hit code here
arogliero3 0:e0dbd261724a 269 }
arogliero3 0:e0dbd261724a 270 break;
arogliero3 0:e0dbd261724a 271 case '2': //number button 2
arogliero3 0:e0dbd261724a 272 if (bhit=='1') {
arogliero3 0:e0dbd261724a 273 // execute('t');
arogliero3 0:e0dbd261724a 274 // add hit code here
arogliero3 0:e0dbd261724a 275 } else {
arogliero3 0:e0dbd261724a 276 //add release code here
arogliero3 0:e0dbd261724a 277 }
arogliero3 0:e0dbd261724a 278 break;
arogliero3 0:e0dbd261724a 279 case '3': //number button 3
arogliero3 0:e0dbd261724a 280 if (bhit=='1') {
arogliero3 0:e0dbd261724a 281 //add hit code here
arogliero3 0:e0dbd261724a 282 } else {
arogliero3 0:e0dbd261724a 283 //add release code here
arogliero3 0:e0dbd261724a 284 }
arogliero3 0:e0dbd261724a 285 break;
arogliero3 0:e0dbd261724a 286 case '4': //number button 4
arogliero3 0:e0dbd261724a 287 if (bhit=='1') {
arogliero3 0:e0dbd261724a 288 //add hit code here
arogliero3 0:e0dbd261724a 289 } else {
arogliero3 0:e0dbd261724a 290 //add release code here
arogliero3 0:e0dbd261724a 291 }
arogliero3 0:e0dbd261724a 292 break;
arogliero3 0:e0dbd261724a 293 case '5': //button 5 up arrow
arogliero3 0:e0dbd261724a 294 if (bhit=='1') {
arogliero3 0:e0dbd261724a 295 //add hit code here
arogliero3 0:e0dbd261724a 296 forward();
arogliero3 0:e0dbd261724a 297 } else {
arogliero3 0:e0dbd261724a 298 //add release code here
arogliero3 0:e0dbd261724a 299 stop();
arogliero3 0:e0dbd261724a 300 }
arogliero3 0:e0dbd261724a 301 break;
arogliero3 0:e0dbd261724a 302 case '6': //button 6 down arrow
arogliero3 0:e0dbd261724a 303 if (bhit=='1') {
arogliero3 0:e0dbd261724a 304 //add hit code here
arogliero3 0:e0dbd261724a 305 backward();
arogliero3 0:e0dbd261724a 306 } else {
arogliero3 0:e0dbd261724a 307 //add release code here
arogliero3 0:e0dbd261724a 308 stop();
arogliero3 0:e0dbd261724a 309 }
arogliero3 0:e0dbd261724a 310 break;
arogliero3 0:e0dbd261724a 311 case '7': //button 7 left arrow
arogliero3 0:e0dbd261724a 312 if (bhit=='1') {
arogliero3 0:e0dbd261724a 313 //add hit code here
arogliero3 0:e0dbd261724a 314 left();
arogliero3 0:e0dbd261724a 315 } else {
arogliero3 0:e0dbd261724a 316 //add release code here
arogliero3 0:e0dbd261724a 317 stop();
arogliero3 0:e0dbd261724a 318 }
arogliero3 0:e0dbd261724a 319 break;
arogliero3 0:e0dbd261724a 320 case '8': //button 8 right arrow
arogliero3 0:e0dbd261724a 321 if (bhit=='1')
arogliero3 0:e0dbd261724a 322 {
arogliero3 0:e0dbd261724a 323 //add hit code here
arogliero3 0:e0dbd261724a 324 right();
arogliero3 0:e0dbd261724a 325 }
arogliero3 0:e0dbd261724a 326 else
arogliero3 0:e0dbd261724a 327 {
arogliero3 0:e0dbd261724a 328 //add release code here
arogliero3 0:e0dbd261724a 329 stop();
arogliero3 0:e0dbd261724a 330 }
arogliero3 0:e0dbd261724a 331 break;
arogliero3 0:e0dbd261724a 332 default:
arogliero3 0:e0dbd261724a 333 // stop();
arogliero3 0:e0dbd261724a 334 break;
arogliero3 0:e0dbd261724a 335 }
arogliero3 0:e0dbd261724a 336 }
arogliero3 0:e0dbd261724a 337 }
arogliero3 0:e0dbd261724a 338 }
arogliero3 0:e0dbd261724a 339 }
arogliero3 0:e0dbd261724a 340
arogliero3 0:e0dbd261724a 341 }