main.cpp

00001 #include "mbed.h"
00002 #include "SDHCFileSystem.h"
00003 #include "wavfile.h"
00004 #include "Camera_LS_Y201.h"
00005 
00006 Serial pc(USBTX, USBRX);
00007 Serial esp(p28, p27); // tx, rx
00008 Serial device(p9, p10);  // tx, rx
00009 Camera_LS_Y201 cam1(p13, p14);
00010 
00011 //Cam and Mic code
00012 #define DEBMSG      printf
00013 #define NEWLINE()   printf("\r\n")
00014   
00015 #define FILENAME    "/sd/IMG_spy_%01d.jpg"
00016 //#define FILENAME    "/local/IMG_spy%001d.jpg"
00017 #define RECORDNAME    "/sd/rec_%001d.wav"
00018 
00019 SDFileSystem fs(p5, p6, p7, p8, "sd");
00020 
00021 //LocalFileSystem fs("local");
00022 
00023 #define RAM_TOTAL   0x1000
00024 
00025 AnalogOut dacout(p18);
00026 AnalogIn adcin(p20);
00027 //DigitalOut led_play_ok(LED1);
00028 //DigitalOut led_rec_ok(LED2);
00029 
00030 Ticker ticker;
00031 //SDFileSystem sdc(p5, p6, p7, p8, "sdc");
00032 
00033 // Definitions of iRobot Roomba SCI Command Numbers
00034 // See the Roomba SCI manual for a complete list 
00035 //                 Create Command              // Arguments
00036 const char         Start = 128;
00037 const char         Control = 130;
00038 const char         Drive = 137;                // 4:   [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
00039 const char         Sensors = 142;              // 1:    Sensor Packet ID
00040 const char         CoverandDock = 143;         // 0:    Return to Charger
00041 const char         Clean = 135;                 // 0:    Start Cleaning
00042 const char         PlaySong = 141;
00043 const char         Song = 140;
00044                 /* iRobot Roomba Sensor IDs */
00045 const char         BumpsandDrops = 1;
00046  
00047 int speed =  400;
00048 int radius = 0x8000;
00049 void start();
00050 void forward();
00051 void reverse();
00052 void left();
00053 void right();
00054 void stop();
00055 void playsong();
00056 void charger();
00057 
00058 // Standard Mbed LED definitions
00059 DigitalOut  led1(LED1),led2(LED2),led3(LED3),led4(LED4);
00060 
00061 
00062 //-------------------Cam and Mic functions------------------
00063 float buffer[RAM_TOTAL];
00064 int cnt = 0;
00065 int rp = 0;
00066 int wp = 0;
00067 int dropout = 0;
00068 
00069 #define WAVFILE_ERROR_PRINT(RESULT) \
00070     do { \
00071         WavFileResult R = RESULT; \
00072         if (R != WavFileResultOK) { \
00073             char wavfile_error_print_text[BUFSIZ]; \
00074             wavfile_result_string(R, wavfile_error_print_text, sizeof(wavfile_error_print_text)); \
00075             pc.printf("%s (code=%d)\r\n", wavfile_error_print_text, R); \
00076             return 1; \
00077         } \
00078     } while(0)
00079 
00080 //////
00081 typedef struct work {
00082     FILE *fp;
00083 } work_t;
00084  
00085 work_t work;
00086 
00087 void callback_func(int done, int total, uint8_t *buf, size_t siz)
00088 {
00089     fwrite(buf, siz, 1, work.fp);
00090  
00091     static int n = 0;
00092     int tmp = done * 100 / total;
00093     if (n != tmp) {
00094         n = tmp;
00095         DEBMSG("Writing...: %3d%%", n);
00096         NEWLINE();
00097     }
00098 }
00099 
00100 int capture(Camera_LS_Y201 *cam, char *filename)
00101 {
00102     /*
00103      * Take a picture.
00104      */
00105     if (cam->takePicture() != 0) {
00106         return -1;
00107     }
00108     DEBMSG("Captured.");
00109     NEWLINE();
00110  
00111     /*
00112      * Open file.
00113      */
00114     work.fp = fopen(filename, "wb");
00115     if (work.fp == NULL) {
00116         return -2;
00117     }
00118  
00119     /*
00120      * Read the content.
00121      */
00122     DEBMSG("%s", filename);
00123     NEWLINE();
00124     if (cam->readJpegFileContent(callback_func) != 0) {
00125         fclose(work.fp);
00126         return -3;
00127     }
00128     fclose(work.fp);
00129  
00130     /*
00131      * Stop taking pictures.
00132      */
00133     cam->stopTakingPictures();
00134  
00135     return 0;
00136 }
00137 
00138 // things for sending/receiving data over serial
00139 volatile int tx_in=0;
00140 volatile int tx_out=0;
00141 volatile int rx_in=0;
00142 volatile int rx_out=0;
00143 const int buffer_size = 2048;//4095;
00144 char tx_buffer[64], rx_buffer[buffer_size],webdata[buffer_size];
00145 void Tx_interrupt(),Rx_interrupt(),send_line(),read_line();
00146 
00147 volatile int roombaDirection =0;
00148 
00149 int DataRX, update, count, timeout;
00150 char replybuff[buffer_size],  cmdbuff[64], rx_line[buffer_size];
00151 void SendCMD(),getreply(),ReadWebData(),moveRobo(),startserver();
00152 
00153 void tickrec(void)
00154 {
00155     /*
00156      * Check the rec overflow
00157      */
00158     int np = (wp + 1) & (RAM_TOTAL - 1);
00159     if (np != rp) {
00160         buffer[wp] = adcin;
00161         wp = np;
00162     } else {
00163         dropout++;
00164     }
00165     led2 = !led2;
00166     //led_rec_ok = !led_rec_ok;
00167 }
00168 
00169 int rec(const char *filename, const int nsec)
00170 {
00171     led3=1;
00172     WavFileResult result;
00173     wavfile_info_t info;
00174     wavfile_data_t data;
00175 
00176     WAVFILE_INFO_AUDIO_FORMAT(&info)    = 1;
00177     WAVFILE_INFO_NUM_CHANNELS(&info)    = 1;
00178     WAVFILE_INFO_SAMPLE_RATE(&info)     = 11025;
00179     WAVFILE_INFO_BYTE_RATE(&info)       = 22050;
00180     WAVFILE_INFO_BLOCK_ALIGN(&info)     = 2;
00181     WAVFILE_INFO_BITS_PER_SAMPLE(&info) = 16;
00182 
00183     WAVFILE *wf = wavfile_open(filename, WavFileModeWrite, &result);
00184     WAVFILE_ERROR_PRINT(result);
00185     WAVFILE_ERROR_PRINT(wavfile_write_info(wf, &info));
00186 
00187     printf("[REC:%s]\r\n", filename);
00188     printf("\tWAVFILE_INFO_AUDIO_FORMAT(&info)    = %d\r\n", WAVFILE_INFO_AUDIO_FORMAT(&info));
00189     printf("\tWAVFILE_INFO_NUM_CHANNELS(&info)    = %d\r\n", WAVFILE_INFO_NUM_CHANNELS(&info));
00190     printf("\tWAVFILE_INFO_SAMPLE_RATE(&info)     = %d\r\n", WAVFILE_INFO_SAMPLE_RATE(&info));
00191     printf("\tWAVFILE_INFO_BYTE_RATE(&info)       = %d\r\n", WAVFILE_INFO_BYTE_RATE(&info));
00192     printf("\tWAVFILE_INFO_BLOCK_ALIGN(&info)     = %d\r\n", WAVFILE_INFO_BLOCK_ALIGN(&info));
00193     printf("\tWAVFILE_INFO_BITS_PER_SAMPLE(&info) = %d\r\n", WAVFILE_INFO_BITS_PER_SAMPLE(&info));
00194 
00195     const int interval_us =  1000000 / WAVFILE_INFO_SAMPLE_RATE(&info);
00196     const unsigned int samples_for_nsec = WAVFILE_INFO_SAMPLE_RATE(&info) * nsec;
00197 
00198     rp = 0;
00199     wp = 0;
00200     dropout = 0;
00201     unsigned int count = 0;
00202     ticker.attach_us(tickrec, interval_us);
00203     WAVFILE_DATA_NUM_CHANNELS(&data) = 1;
00204     while (1) {
00205         while (rp == wp) {
00206             wait_us(1);
00207         }
00208 
00209         WAVFILE_DATA_CHANNEL_DATA(&data, 0) = buffer[rp];
00210         rp = (rp + 1) & (RAM_TOTAL - 1);
00211         WAVFILE_ERROR_PRINT(wavfile_write_data(wf, &data));
00212 
00213         count++;
00214         if (count > samples_for_nsec) {
00215             break;
00216         }
00217     }
00218     ticker.detach();
00219     //led_rec_ok = 0;
00220     led3 = 0;
00221     pc.printf("\t-- Rec done. (dropout=%d) --\r\n", dropout);
00222 
00223     WAVFILE_ERROR_PRINT(wavfile_close(wf));
00224     return 0;
00225 }
00226 //-------------------------Cam and Mic ends here-----------------
00227 
00228 
00229 int main()
00230 {    
00231     pc.baud(9600);
00232     esp.baud(9600);
00233     device.baud(57600);
00234         
00235     led1=0,led2=0,led3=0, led4=0;
00236     
00237     pc.printf("Welcome!\r\n");
00238 
00239     start();
00240 
00241     pc.printf("Playing sound!\r\n");
00242     playsong();
00243     
00244     for(int i=0;i<8;i++)
00245     {
00246             led1=!led1;wait(0.2);led1=!led1;led2=!led2;
00247             wait(0.2);led2=!led2;led3=!led3;wait(0.2);
00248             led3=!led3;led4=!led4;wait(0.2);led4=!led4;            
00249     }    
00250     led1=0,led2=0,led3=0, led4=0;
00251     
00252     timeout=2;
00253     
00254     esp.attach(&Rx_interrupt, Serial::RxIrq);     // Setup a serial interrupt function to receive data
00255     esp.attach(&Tx_interrupt, Serial::TxIrq);    // Setup a serial interrupt function to transmit data
00256     
00257     startserver();
00258     
00259     DataRX=0;
00260     count=0;
00261  
00262     while(1) {
00263         if(DataRX==1) {
00264              pc.printf("\r\nReading Webdata!\r\n");
00265 
00266             ReadWebData();
00267             esp.attach(&Rx_interrupt, Serial::RxIrq);
00268         }
00269         if(update==1) // update time, hit count, and analog levels in the HUZZAH chip
00270         {
00271             pc.printf("Updating robot position!\r\n");
00272             moveRobo();
00273             count++;
00274             update=0;   
00275         }
00276     }
00277 }
00278 
00279 // Starts webserver
00280 void startserver()
00281 {    
00282     pc.printf("\n++++++++++ Setting up HTTP Server ++++++++++\r\n> ");
00283     
00284     strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
00285     SendCMD();
00286     getreply();
00287     wait(0.2);
00288     strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
00289     SendCMD();
00290     getreply();
00291     wait(0.2);
00292  
00293     //create server
00294     sprintf(cmdbuff, "srv=net.createServer(net.TCP)\r\n");
00295     SendCMD();
00296     getreply();
00297     wait(0.5);
00298     strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
00299     SendCMD();
00300     getreply();
00301     wait(0.3);
00302         strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
00303         SendCMD();
00304         getreply();
00305         wait(0.3);
00306         
00307         //print data to mbed
00308         strcpy(cmdbuff,"print(payload)\r\n");
00309         SendCMD();
00310         getreply();
00311         wait(0.2);
00312        
00313         //web page data
00314         strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><body><h1>IoT Controlled Spy Robot</h1>')\r\n");
00315         SendCMD();
00316         getreply();
00317         wait(0.4);
00318         strcpy(cmdbuff,"conn:send('<form method=\"POST\"')\r\n");
00319         SendCMD();
00320         getreply();
00321         wait(0.3);
00322         strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led1v\"> Forward</p>')\r\n");
00323         SendCMD();
00324         getreply();
00325         wait(0.3);
00326         strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led2v\"> Backward</p>')\r\n");
00327         SendCMD();
00328         getreply();
00329         wait(0.3);
00330         strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led3v\"> Left</p>')\r\n");
00331         SendCMD();
00332         getreply();
00333         wait(0.3);
00334         strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led4v\"> Right</p>')\r\n");
00335         SendCMD();
00336         getreply();
00337         wait(0.3);
00338         strcpy(cmdbuff, "conn:send('<p><input type=\"checkbox\" name=\"check\" value=\"led5v\"> Snap-Rec</p>')\r\n");
00339         SendCMD();
00340         getreply();
00341         wait(0.3);        
00342         strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"SEND\"></p></form>')\r\n");
00343         SendCMD();
00344         getreply();
00345         wait(0.3);
00346         
00347         //strcpy(cmdbuff,"conn:send('<h2>Camera Image</h2>')\r\n");
00348         //SendCMD();
00349         //getreply();
00350         //wait(0.3);
00351         //strcpy(cmdbuff,"conn:send('<img src=\"IMG_0001.jpg\" alt=\"cam-img\" style=\"width:320px;height:240px;\">')\r\n");
00352         //SendCMD();
00353         //getreply();
00354         //wait(0.3);
00355         
00356         strcpy(cmdbuff, "conn:send('<p><h2>Tip: </h2></p><ul><li>Select a checkbox to specify direction</li><li>Click SEND to send data to robot</li></ul></body></html>')\r\n");
00357         SendCMD();
00358         getreply();
00359         wait(0.5); 
00360         // end web page data
00361         strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
00362         SendCMD();
00363         getreply();
00364         wait(0.3);
00365         strcpy(cmdbuff, "end)\r\n");
00366         SendCMD();
00367         getreply();
00368         wait(0.2);
00369     strcpy(cmdbuff, "end)\r\n");
00370     SendCMD();
00371     getreply();
00372     wait(0.2);
00373     
00374     pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n");
00375 }
00376 
00377 // Reads and processes GET and POST web data
00378 void ReadWebData()
00379 {
00380     wait_ms(200);
00381     esp.attach(NULL,Serial::RxIrq);
00382     DataRX=0;
00383     memset(webdata, '\0', sizeof(webdata));
00384     strcpy(webdata, rx_buffer);
00385     memset(rx_buffer, '\0', sizeof(rx_buffer));
00386     rx_in = 0;
00387     rx_out = 0;
00388     // check web data for form information
00389     if( strstr(webdata, "check=led1v") != NULL ) {
00390         //led1=!led1;
00391         roombaDirection = 1;
00392     }
00393     if( strstr(webdata, "check=led2v") != NULL ) {
00394         //led2=!led2;
00395         roombaDirection =2;
00396     }
00397     if( strstr(webdata, "check=led3v") != NULL ) {
00398         //led3=!led3;
00399         roombaDirection =3;
00400     }
00401     if( strstr(webdata, "check=led4v") != NULL ) {
00402         //led4=!led4;
00403         roombaDirection =4;
00404     }
00405     if( strstr(webdata, "check=led5v") != NULL ) {
00406         //led4=!led4;
00407         roombaDirection =5;
00408     }
00409     if((strstr(webdata, "POST")!= NULL ) || ( strstr(webdata, "GET") != NULL)) { // set update flag if POST request
00410         update=1;
00411     }
00412 }
00413 
00414 void moveRobo()
00415 {       
00416   switch (roombaDirection)
00417     {
00418       case 1:
00419       {         
00420         forward();
00421         wait(3);
00422         stop();
00423         roombaDirection =0;
00424         pc.printf("Forward\r\n");
00425         for(int i=0;i<4;i++)       
00426           led1=!led1;wait(0.2);led1=!led1;
00427         break;
00428       }
00429       case 2:
00430       {
00431         reverse();
00432         wait(3);
00433         stop();
00434         roombaDirection =0;
00435         pc.printf("Backward\r\n");
00436         for(int i=0;i<4;i++)       
00437           led2=!led2;wait(0.2);led2=!led2;
00438         break;
00439        }
00440       case 3:
00441       {
00442         left();
00443         wait(0.5);
00444         stop();
00445         wait(0.1);        
00446         forward();
00447         wait(2);
00448         stop();
00449         roombaDirection =0;
00450         pc.printf("Left\r\n");
00451         for(int i=0;i<4;i++)       
00452           led3=!led3;wait(0.2);led3=!led3;
00453         break;
00454       }
00455       case 4:
00456       {
00457         right();        
00458         wait(0.5);
00459         stop();
00460         wait(0.1);        
00461         forward();
00462         wait(2);                
00463         stop();
00464         roombaDirection =0;
00465         pc.printf("Right\r\n");
00466         for(int i=0;i<4;i++)       
00467           led3=!led3;wait(0.2);led3=!led3;
00468         break;
00469       }
00470       case 5:
00471       {
00472        cnt++;
00473        
00474        //char recname[32];
00475        
00476        //snprintf(recname, sizeof(recname) - 1, RECORDNAME, cnt);
00477        
00478        //static const char *target_filename = "/fs/rec-test1.wav"; //recname; //"
00479        //pc.printf(target_filename);
00480     
00481         //int val =rec(target_filename, 10);
00482         
00483         //pc.printf("\rrec value: %d", val);
00484         
00485        //while (1) 
00486        //{
00487             /*
00488              * 30 seconds recording.
00489              */
00490             //if (rec(target_filename, 10) != 0) {
00491               //  break;
00492             //}         
00493             
00494         //WavFileResult result;
00495         //wavfile_info_t info;
00496         //wavfile_data_t data;
00497         //break;
00498         //}        
00499            
00500         ////
00501         DEBMSG("Camera module");
00502         NEWLINE();
00503         DEBMSG("Resetting...");
00504         NEWLINE();
00505         wait(1);
00506      
00507         if (cam1.reset() == 0) {
00508             DEBMSG("Reset OK.");
00509             NEWLINE();
00510         led1=1;led2=0;
00511         } else {
00512             DEBMSG("Reset fail.");
00513             NEWLINE();
00514             error("Reset fail.");
00515             led1=0;led2=0;
00516         }
00517         wait(1);
00518      
00519         char fname[64];
00520         
00521         //led1=1;led2=0;
00522         snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
00523         int r = capture(&cam1, fname);
00524         if (r == 0) 
00525         {
00526             DEBMSG("[%04d]:OK.", cnt);
00527            led1=1;led2=1;
00528             NEWLINE();           
00529         } 
00530         else 
00531         {
00532             DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
00533             NEWLINE();
00534             error("Failure.");
00535             led1=0;led2=0;
00536         }
00537        
00538       }          
00539    }
00540 } 
00541 
00542 // ESP Command data send
00543 void SendCMD()
00544 {
00545     int i;
00546     char temp_char;
00547     bool empty;
00548     i = 0;
00549 // Start Critical Section - don't interrupt while changing global buffer variables
00550     NVIC_DisableIRQ(UART1_IRQn);
00551     empty = (tx_in == tx_out);
00552     while ((i==0) || (cmdbuff[i-1] != '\n')) {
00553 // Wait if buffer full
00554         if (((tx_in + 1) % buffer_size) == tx_out) {
00555 // End Critical Section - need to let interrupt routine empty buffer by sending
00556             NVIC_EnableIRQ(UART1_IRQn);
00557             while (((tx_in + 1) % buffer_size) == tx_out) {
00558             }
00559 // Start Critical Section - don't interrupt while changing global buffer variables
00560             NVIC_DisableIRQ(UART1_IRQn);
00561         }
00562         tx_buffer[tx_in] = cmdbuff[i];
00563         i++;
00564         tx_in = (tx_in + 1) % buffer_size;
00565     }
00566     if (esp.writeable() && (empty)) {
00567         temp_char = tx_buffer[tx_out];
00568         tx_out = (tx_out + 1) % buffer_size;
00569 // Send first character to start tx interrupts, if stopped
00570         esp.putc(temp_char);
00571     }
00572 // End Critical Section
00573     NVIC_EnableIRQ(UART1_IRQn);
00574     return;
00575 }
00576 
00577 // Get Command and ESP status replies
00578 void getreply()
00579 {
00580     read_line();
00581     sscanf(rx_line,replybuff);
00582 }
00583  
00584 // Read a line from the large rx buffer from rx interrupt routine
00585 void read_line() {
00586     int i;
00587     i = 0;
00588 // Start Critical Section - don't interrupt while changing global buffer variables
00589     NVIC_DisableIRQ(UART1_IRQn);
00590 // Loop reading rx buffer characters until end of line character
00591     while ((i==0) || (rx_line[i-1] != '\r')) {
00592 // Wait if buffer empty
00593         if (rx_in == rx_out) {
00594 // End Critical Section - need to allow rx interrupt to get new characters for buffer
00595             NVIC_EnableIRQ(UART1_IRQn);
00596             while (rx_in == rx_out) {
00597             }
00598 // Start Critical Section - don't interrupt while changing global buffer variables
00599             NVIC_DisableIRQ(UART1_IRQn);
00600         }
00601         rx_line[i] = rx_buffer[rx_out];
00602         i++;
00603         rx_out = (rx_out + 1) % buffer_size;
00604     }
00605 // End Critical Section
00606     NVIC_EnableIRQ(UART1_IRQn);
00607     rx_line[i-1] = 0;
00608     return;
00609 }
00610   
00611 // Interupt Routine to read in data from serial port
00612 void Rx_interrupt() {
00613     DataRX=1;
00614     //led3=1;
00615 // Loop just in case more than one character is in UART's receive FIFO buffer
00616 // Stop if buffer full
00617     while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
00618         rx_buffer[rx_in] = esp.getc();
00619 // Uncomment to Echo to USB serial to watch data flow
00620         pc.putc(rx_buffer[rx_in]);
00621         rx_in = (rx_in + 1) % buffer_size;
00622     }
00623     //led3=0;
00624     return;
00625 }
00626  
00627 // Interupt Routine to write out data to serial port
00628 void Tx_interrupt() {
00629     //led2=1;
00630 // Loop to fill more than one character in UART's transmit FIFO buffer
00631 // Stop if buffer empty
00632     while ((esp.writeable()) && (tx_in != tx_out)) {
00633         esp.putc(tx_buffer[tx_out]);
00634         tx_out = (tx_out + 1) % buffer_size;
00635     }
00636     //led2=0;
00637     return;
00638 } 
00639 
00640 
00641 //--------------------Roomba section----------------------------------
00642  
00643 // Start  - send start and safe mode, start streaming sensor data
00644 void start() {
00645    // device.printf("%c%c", Start, SafeMode);
00646     device.putc(Start);
00647     
00648     //device.putc(FullMode);
00649 
00650     wait(.1);
00651     device.putc(Control);
00652     wait(.5);
00653   //  device.printf("%c%c", SensorStream, char(1));
00654     device.putc(Sensors);
00655     device.putc(BumpsandDrops);
00656     wait(.5);
00657 }
00658 // Stop  - turn off drive motors
00659 void stop() {
00660     device.printf("%c%c%c%c%c", Drive, char(0),  char(0),  char(0),  char(0));
00661 }
00662 // Forward  - turn on drive motors
00663 void forward() {
00664     device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF),  char(speed&0xFF),  
00665     char((radius>>8)&0xFF),  char(radius&0xFF));
00666  
00667 }
00668 // Reverse - reverse drive motors
00669 void reverse() {
00670     device.printf("%c%c%c%c%c", Drive, char(((-speed)>>8)&0xFF),  char((-speed)&0xFF),  
00671     char(((radius)>>8)&0xFF),  char((radius)&0xFF));
00672  
00673 }
00674 // Left - drive motors set to rotate to left
00675 void left() {
00676     device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF),  char(speed&0xFF),  
00677     char(((1)>>8)&0xFF),  char((1)&0xFF));
00678 }
00679 // Right - drive motors set to rotate to right
00680 void right() {
00681     device.printf("%c%c%c%c%c", Drive, char(((speed)>>8)&0xFF),  char((speed)&0xFF),  
00682     char((-1>>8)&0xFF),  char(-1&0xFF));
00683  
00684 }
00685 // Charger - search and return to charger using IR beacons (if found)
00686 void charger() {
00687     device.printf("%c", Clean );
00688     wait(.2);
00689     device.printf("%c", CoverandDock );
00690 }
00691 // Play Song  - define and play a song
00692 void playsong() { // Send out notes & duration to define song and then play song
00693  
00694     device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", 
00695                   Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
00696                   char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
00697                   char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
00698                   char(24), char(86), char(12), char(87), char(48));
00699  
00700     wait(.2);
00701     device.printf("%c%c", PlaySong, char(0));
00702 }
00703