main.cpp

00001 #include "mbed.h"
00002 #include "Speaker.h"
00003 #include "ultrasonic.h"
00004 #include "Motor.h"
00005 #include "rtos.h"
00006 
00007 RawSerial pc(USBTX, USBRX);
00008 RawSerial bluetooth(p28, p27);
00009 Motor leftWheel(p21, p20, p19);
00010 Motor rightWheel(p22, p15, p14);
00011 Speaker speaker(p23);
00012 
00013 float speed = .5;
00014 
00015 void bluetooth_thread(void const *args) {
00016     char cmd, bhit, bnum;
00017     while(true) {
00018         if (bluetooth.getc() == '!') {
00019             cmd = bluetooth.getc();
00020             switch (cmd) {
00021                 case 'B': // button press
00022                     bnum = bluetooth.getc();
00023                     bhit = bluetooth.getc();
00024                     if (bluetooth.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum               
00025                         switch (bnum) {
00026                             case '1':
00027                                 speed = 1;
00028                                 break;
00029                             case '2':
00030                                 speed = .75;
00031                                 break;     
00032                             case '3':
00033                                 speed = .5;
00034                                 break;
00035                             case '4':
00036                                 speed = .25;
00037                                 break;
00038                             case '5': //up
00039                                 if (bhit == '1') {
00040                                     leftWheel.speed(speed);
00041                                     rightWheel.speed(speed);
00042                                 } else {
00043                                     leftWheel.speed(0);
00044                                     rightWheel.speed(0);
00045                                 }
00046                                 break;
00047                             case '6': //down
00048                                 if (bhit == '1') {
00049                                     leftWheel.speed(-1 * speed);
00050                                     rightWheel.speed(-1 * speed);
00051                                 } else {
00052                                     leftWheel.speed(0);
00053                                     rightWheel.speed(0);
00054                                 }
00055                                 break;
00056                             case '7': //left
00057                                 if (bhit == '1') {
00058                                     leftWheel.speed(-.5);
00059                                     rightWheel.speed(.5);
00060                                 } else {
00061                                     leftWheel.speed(0);
00062                                     rightWheel.speed(0);
00063                                 }
00064                                 break;
00065                             case '8': //right
00066                                 if (bhit == '1') {
00067                                     leftWheel.speed(.5);
00068                                     rightWheel.speed(-.5);
00069                                 } else {
00070                                     leftWheel.speed(0);
00071                                     rightWheel.speed(0);
00072                                 }
00073                                 break;
00074                             default:
00075                                 break;
00076                         }                       
00077                     }                    
00078                     break;
00079                 default:
00080                     break;
00081             }
00082         }
00083     }
00084 }
00085 
00086 void dist(int distance) {
00087     pc.printf("Distance %d mm\r\n", distance);
00088     if (distance < 50) {
00089         speaker.PlayNote(150, 0.15, 0.5);
00090     } else if (distance < 100) {
00091         speaker.PlayNote(150, 0.25, 0.02);
00092     } else if (distance < 200) {
00093         speaker.PlayNote(150, 0.5, 0.02);
00094     } else if (distance < 300) {
00095         speaker.PlayNote(150, 0.75, 0.02);
00096     }
00097     Thread::wait(50);
00098 }
00099 
00100 ultrasonic sonar(p6, p7, .1, 1, &dist);
00101 
00102 int main() {
00103     sonar.startUpdates();
00104     
00105     Thread bt_t(bluetooth_thread);
00106     while(1) {
00107         sonar.checkDistance();
00108     }
00109 }