main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "rtos.h"
00004 #include "SDFileSystem.h"
00005 #include "SongPlayer.h"
00006 #include "motordriver.h"
00007 
00008 #define PI 3.14159
00009 
00010 //Serial Debug
00011 Serial rfLink(p9, p10);
00012 Serial pc(USBTX, USBRX);
00013 
00014 
00015 //Threads
00016 Thread audioThread, motorThread, thread3, gameDisplayThread;
00017 
00018 //Mutexes
00019 Mutex commMutex;
00020 
00021 //Motors
00022 Motor left(p22, p16, p15, 1); // pwm, fwd, rev, can brake
00023 Motor right(p21, p17, p18, 1); // pwm, fwd, rev, can brake
00024 
00025 //Command Byte
00026 char commByte = 0;
00027 
00028 DigitalOut ledup(p20);
00029 DigitalOut leddown(p19);
00030 DigitalOut ledleft(p18);
00031 DigitalOut ledright(p17);
00032 
00033 //Music & Sounds
00034 float note[18]= {1568.0,1396.9,1244.5,1244.5,1396.9,1568.0,1568.0,1568.0,1396.9,
00035                  1244.5,1396.9,1568.0,1396.9,1244.5,1174.7,1244.5,1244.5, 0.0
00036                 };
00037 float duration[18]= {0.48,0.24,0.72,0.48,0.24,0.48,0.24,0.24,0.24,
00038                      0.24,0.24,0.24,0.24,0.48,0.24,0.48,0.48, 0.0
00039                     };
00040                     
00041 float note2[] = {
00042     2637.0, 2637.0, 0, 2637.0,
00043     0, 2093.0, 2637.0, 0,
00044     3136.0, 0, 0,  0,
00045     1568.0, 0, 0, 0,
00046     
00047     2093.0, 0, 0, 1568.0,
00048     0, 0, 1319.0, 0,
00049     0, 1760.0, 0, 1976.0,
00050     0, 1865.0, 1760.0, 0,
00051      
00052     1568.0, 2637.0, 3136.0,
00053     3520.0, 0, 2794.0, 3136.0,
00054     0, 2637.0, 0, 2093.0,
00055     2349.0, 1976.0, 0, 0,
00056      
00057     2093.0, 0, 0, 1568.0,
00058     0, 0, 1319.0, 0,
00059     0, 1760.0, 0, 1976.0,
00060     0, 1865.0, 1760.0, 0,
00061      
00062     1568.0, 2637.0, 3136.0,
00063     3520.0, 0, 2794.0, 3136.0,
00064     0, 2637.0, 0, 2093.0,
00065     2349.0, 1976.0, 0, 0
00066 };
00067 
00068 float duration2[] = {
00069   0.0833, 0.0833, 0.0833, 0.0833,
00070   0.0833, 0.0833, 0.0833, 0.0833,
00071   0.0833, 0.0833, 0.0833, 0.0833,
00072   0.0833, 0.0833, 0.0833, 0.0833,
00073  
00074   0.0833, 0.0833, 0.0833, 0.0833,
00075   0.0833, 0.0833, 0.0833, 0.0833,
00076   0.0833, 0.0833, 0.0833, 0.0833,
00077   0.0833, 0.0833, 0.0833, 0.0833,
00078  
00079   0.1111, 0.1111, 0.1111,
00080   0.0833, 0.0833, 0.0833, 0.0833,
00081   0.0833, 0.0833, 0.0833, 0.0833,
00082   0.0833, 0.0833, 0.0833, 0.0833,
00083  
00084   0.0833, 0.0833, 0.0833, 0.0833,
00085   0.0833, 0.0833, 0.0833, 0.0833,
00086   0.0833, 0.0833, 0.0833, 0.0833,
00087   0.0833, 0.0833, 0.0833, 0.0833,
00088  
00089   0.1111, 0.1111, 0.1111,
00090   0.0833, 0.0833, 0.0833, 0.0833,
00091   0.0833, 0.0833, 0.0833, 0.0833,
00092   0.0833, 0.0833, 0.0833, 0.0833, 0.0
00093 };
00094 
00095 float note3[] = {440.0, 440.0, 440.0, 349.0, 523.0, 440.0, 349.0, 523.0, 440.0, 
00096                  0.0, 659.0, 659.0, 659.0, 698.0, 523.0, 415.0, 349.0, 523.0, 
00097                  440.0, 0.0};
00098 
00099 float duration3[] = {0.5, 0.5, 0.5, 0.35, 0.15, 0.5, 0.35, 0.15, 0.65, 0.5,
00100                      0.5, 0.5, 0.5, 0.35, 0.15, 0.5, 0.35, 0.15, 0.65, 0.5,
00101                      0.0}; 
00102 SongPlayer mySpeaker(p26);
00103 
00104 void playSound() {
00105     //Setup PWM hardware for a Class D style audio output
00106     int songSelect = 0;
00107     while (true) {
00108         commMutex.lock();
00109         if (commByte == 0x60) {
00110             commMutex.unlock();
00111             songSelect++; 
00112             if (songSelect % 3 == 0)
00113                 mySpeaker.PlaySong(note2,duration2);
00114             else if (songSelect % 3 == 1)
00115                 mySpeaker.PlaySong(note3,duration3);
00116             else
00117                 mySpeaker.PlaySong(note,duration);
00118         } else {
00119             commMutex.unlock();
00120             Thread::yield();
00121         }
00122     }
00123 }
00124 
00125 void runMotors(float l, float r, int wait_time) {
00126         left.speed(l);
00127         right.speed(r);
00128         Thread::wait(wait_time);
00129         left.stop(1);
00130         right.stop(1);
00131         Thread::wait(100);    
00132 }
00133 
00134 void moveRobot() {
00135     while (true) {
00136         commMutex.lock();
00137         if (commByte == 0x55) {
00138             commMutex.unlock();
00139             runMotors(0.75, 0.75, 2000);
00140             //pc.printf("Forward\n\r");
00141             ledup = 1;
00142         } else if (commByte == 0x56) {
00143             commMutex.unlock();
00144             runMotors(-0.75, -0.75, 2000);
00145             //pc.printf("Backward\n\r");
00146             leddown = 1;
00147         } else if (commByte == 0x57) {
00148             commMutex.unlock();
00149             runMotors(-0.75, 0.75, 500);
00150             //pc.printf("Left\n\r");
00151             ledleft = 1;
00152         } else if (commByte == 0x58) {
00153             commMutex.unlock();
00154             runMotors(0.75, -0.75, 500);
00155             //pc.printf("Right\n\r");
00156             ledright = 1;
00157         } else if (commByte == 0x59) {
00158             commMutex.unlock();
00159             runMotors(1.0, -1.0, 1000);
00160             runMotors(-1.0, 1.0, 1000);
00161         } else {
00162             commMutex.unlock();
00163             //pc.printf("Stop\n\r");
00164         }
00165         ledup = 0;
00166         leddown = 0;
00167         ledleft = 0;
00168         ledright = 0;
00169     }
00170 }
00171 
00172 int main() {
00173     rfLink.baud(600);
00174     pc.printf("Started\n");
00175     motorThread.start(moveRobot);
00176     audioThread.start(playSound);
00177     while (true) {         
00178         //RF Receive Code
00179         if (rfLink.readable()) {
00180             commMutex.lock();
00181             commByte = rfLink.getc();
00182             commMutex.unlock();
00183         } else {
00184             Thread::wait(10);
00185         }
00186     }
00187 }