Diff: main.cpp

Revision:

12:42d63181dbdb

Parent:

11:0309bef74ba8

--- a/main.cpp	Wed Feb 15 14:04:02 2017 -0600
+++ b/main.cpp	Mon Mar 13 18:35:56 2017 +0000
@@ -1,22 +1,109 @@
 #include "mbed.h"
+#include "Speaker.h"
+#include "ultrasonic.h"
+#include "Motor.h"
 #include "rtos.h"
- 
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-Thread thread;
- 
-void led2_thread() {
-    while (true) {
-        led2 = !led2;
-        Thread::wait(1000);
+
+RawSerial pc(USBTX, USBRX);
+RawSerial bluetooth(p28, p27);
+Motor leftWheel(p21, p20, p19);
+Motor rightWheel(p22, p15, p14);
+Speaker speaker(p23);
+
+float speed = .5;
+
+void bluetooth_thread(void const *args) {
+    char cmd, bhit, bnum;
+    while(true) {
+        if (bluetooth.getc() == '!') {
+            cmd = bluetooth.getc();
+            switch (cmd) {
+                case 'B': // button press
+                    bnum = bluetooth.getc();
+                    bhit = bluetooth.getc();
+                    if (bluetooth.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum               
+                        switch (bnum) {
+                            case '1':
+                                speed = 1;
+                                break;
+                            case '2':
+                                speed = .75;
+                                break;     
+                            case '3':
+                                speed = .5;
+                                break;
+                            case '4':
+                                speed = .25;
+                                break;
+                            case '5': //up
+                                if (bhit == '1') {
+                                    leftWheel.speed(speed);
+                                    rightWheel.speed(speed);
+                                } else {
+                                    leftWheel.speed(0);
+                                    rightWheel.speed(0);
+                                }
+                                break;
+                            case '6': //down
+                                if (bhit == '1') {
+                                    leftWheel.speed(-1 * speed);
+                                    rightWheel.speed(-1 * speed);
+                                } else {
+                                    leftWheel.speed(0);
+                                    rightWheel.speed(0);
+                                }
+                                break;
+                            case '7': //left
+                                if (bhit == '1') {
+                                    leftWheel.speed(-.5);
+                                    rightWheel.speed(.5);
+                                } else {
+                                    leftWheel.speed(0);
+                                    rightWheel.speed(0);
+                                }
+                                break;
+                            case '8': //right
+                                if (bhit == '1') {
+                                    leftWheel.speed(.5);
+                                    rightWheel.speed(-.5);
+                                } else {
+                                    leftWheel.speed(0);
+                                    rightWheel.speed(0);
+                                }
+                                break;
+                            default:
+                                break;
+                        }                       
+                    }                    
+                    break;
+                default:
+                    break;
+            }
+        }
     }
 }
- 
+
+void dist(int distance) {
+    pc.printf("Distance %d mm\r\n", distance);
+    if (distance < 50) {
+        speaker.PlayNote(150, 0.15, 0.5);
+    } else if (distance < 100) {
+        speaker.PlayNote(150, 0.25, 0.02);
+    } else if (distance < 200) {
+        speaker.PlayNote(150, 0.5, 0.02);
+    } else if (distance < 300) {
+        speaker.PlayNote(150, 0.75, 0.02);
+    }
+    Thread::wait(50);
+}
+
+ultrasonic sonar(p6, p7, .1, 1, &dist);
+
 int main() {
-    thread.start(led2_thread);
+    sonar.startUpdates();
     
-    while (true) {
-        led1 = !led1;
-        Thread::wait(500);
+    Thread bt_t(bluetooth_thread);
+    while(1) {
+        sonar.checkDistance();
     }
 }