main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "motordriver.h"
00004 #include "PololuLedStrip.h"
00005 // Connect mbed to Pi USB
00006 Serial  pi(USBTX, USBRX);
00007 
00008 Serial blue(p9, p10);
00009 
00010 // Set up motors
00011 Motor L(p24, p23, p22, 1); // pwmA, fwd, rev, can brake 
00012 Motor R(p21, p8, p7, 1); // pwmB, fwd, rev, can brake
00013 
00014 PololuLedStrip ledStrip(p5);
00015 
00016 // for LED strip
00017 Timer timer;
00018 
00019 volatile bool disco_mode = false;
00020 volatile bool tornado_mode = false;
00021 volatile bool autonomous_mode = false;
00022 
00023 #define LED_COUNT 82
00024 rgb_color colors[LED_COUNT];
00025 
00026 
00027 // Converts a color from the HSV representation to RGB.
00028 rgb_color hsvToRgb(float h, float s, float v)
00029 {
00030     int i = floor(h * 6);
00031     float f = h * 6 - i;
00032     float p = v * (1 - s);
00033     float q = v * (1 - f * s);
00034     float t = v * (1 - (1 - f) * s);
00035     float r = 0, g = 0, b = 0;
00036     switch(i % 6){
00037         case 0: r = v; g = t; b = p; break;
00038         case 1: r = q; g = v; b = p; break;
00039         case 2: r = p; g = v; b = t; break;
00040         case 3: r = p; g = q; b = v; break;
00041         case 4: r = t; g = p; b = v; break;
00042         case 5: r = v; g = p; b = q; break;
00043     }
00044     return (rgb_color){r * 255, g * 255, b * 255};
00045 }
00046 
00047 #define TIME_SLICE 5.0f
00048 
00049 // LEDs for debugging
00050 
00051 // Invalid character was read
00052 DigitalOut led1(LED1);
00053 
00054 // Detected tornado
00055 DigitalOut led2(LED2);
00056 DigitalOut led3(LED3);
00057 
00058 // Detected disco_ball
00059 DigitalOut led4(LED4);
00060 
00061 
00062 void forward()
00063 {
00064     L.speed(0.5);
00065     R.speed(0.5);
00066     wait(TIME_SLICE/10);
00067     L.coast();
00068     R.coast();
00069 }
00070 
00071 void backward()
00072 {
00073     L.speed(-0.5);
00074     R.speed(-0.5);
00075     wait(TIME_SLICE/10);
00076     L.coast();
00077     R.coast();
00078 }
00079 
00080 void left()
00081 {
00082     L.speed(-0.5);
00083     R.speed(0.5);
00084     wait(TIME_SLICE/10);
00085     L.coast();
00086     R.coast();
00087 }
00088 
00089 void right()
00090 {
00091     L.speed(0.5);
00092     R.speed(-0.5);
00093     wait(TIME_SLICE/10);
00094     L.coast();
00095     R.coast();
00096 }
00097 
00098 void stop()
00099 {
00100     L.stop(0);
00101     R.stop(0);
00102 }
00103 
00104 void led_strip()
00105 {
00106     timer.start();
00107     
00108     while(1)
00109     {
00110         if (disco_mode)
00111         {
00112             // Update the colors array.
00113             uint32_t time = timer.read_ms();       
00114             for(int i = 0; i < LED_COUNT; i++)
00115             {
00116                 uint8_t phase = (time >> 4) - (i << 2);
00117                 colors[i] = hsvToRgb(phase / 256.0, 1.0, 0.25);
00118             }
00119         
00120             // Send the colors to the LED strip.
00121             ledStrip.write(colors, LED_COUNT);
00122         }
00123         else if (tornado_mode)
00124         {
00125             rgb_color temp;
00126             temp.red  = 64;
00127             temp.green = 64;
00128             temp.blue = 0;
00129             for (int i = 0; i < LED_COUNT; i++)
00130             {
00131                 colors[i] = temp;
00132             }
00133             ledStrip.write(colors, LED_COUNT);
00134         }
00135         else
00136         {
00137             rgb_color temp;
00138             temp.red  = 64;
00139             temp.green = 0;
00140             temp.blue = 0;
00141             for (int i = 0; i < LED_COUNT; i++)
00142             {
00143                 if (i == LED_COUNT/2)
00144                 {
00145                     temp.red = 0;
00146                     temp.green = 64;
00147                 }
00148                 colors[i] = temp;
00149             }
00150             ledStrip.write(colors, LED_COUNT);
00151         }
00152     }
00153 }
00154 
00155 void detected_tornado()
00156 {
00157     tornado_mode = true;
00158     for (int i = 0; i < 10; i++)
00159     {
00160         left();
00161         wait(0.5);
00162     }
00163     stop();
00164     tornado_mode = false;
00165 }
00166 
00167 void detected_disco_ball()
00168 {
00169     led4 = 1;
00170     disco_mode = true;
00171     left();
00172     left();
00173     right();
00174     right();
00175     left();
00176     left();
00177     right();
00178     right();
00179     forward();
00180     backward();
00181     disco_mode = false;
00182     
00183 }
00184 
00185 void execute(char c)
00186 {
00187     switch (c) 
00188     {
00189         case 'f':
00190             forward();
00191             break;
00192         case 'b':
00193             backward();
00194             break;
00195         case 'l':
00196             left();
00197             break; 
00198         case 'r':
00199             right();
00200             break; 
00201         case 't':
00202             detected_tornado();
00203             break;        
00204         case 'd':
00205             detected_disco_ball();
00206             break;
00207         case 's':
00208             stop();
00209             break; 
00210         default:
00211             // invalid character
00212             led1 = 1;
00213             break;
00214     }       
00215 }   
00216 
00217 int main()
00218 {
00219     led1 = 0;
00220     led2 = 0;
00221     led3 = 0;
00222     led4 = 0;
00223     pi.baud(9600);
00224     char bnum, bhit;
00225     
00226     Thread t1;
00227     t1.start(led_strip);
00228     
00229     while(1) {
00230         // pi commands are read first
00231         if(pi.readable()) {
00232             char c = pi.getc();
00233             if (c != 'n')
00234                 execute(c);
00235                 switch (c)
00236                 {
00237                     case 'd':
00238                     case 't':
00239                         pi.putc('f');
00240                         autonomous_mode = false;
00241                         break;
00242                     default:
00243                         pi.putc(c);
00244                         break;
00245                 }
00246         }
00247 
00248         // bluetooth commands are read later
00249         if (!blue.readable()) continue;
00250         if (blue.getc()=='!') {
00251             if (blue.getc()=='B') { //button data packet
00252                 bnum = blue.getc(); //button number
00253                 bhit = blue.getc(); //1=hit, 0=release
00254                 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
00255                     //myled = bnum - '0'; //current button number will appear on LEDs
00256                     switch (bnum) {
00257                         case '1': //number button 1
00258                             if (bhit=='1') {
00259                                 autonomous_mode = !autonomous_mode;
00260                                 if (!autonomous_mode)
00261                                 {
00262                                     pi.putc('f');
00263                                 }
00264                                 else
00265                                 {
00266                                     pi.putc('a');
00267                                 }
00268                                 //add hit code here
00269                             }
00270                             break;
00271                         case '2': //number button 2
00272                             if (bhit=='1') {
00273                                 // execute('t');
00274                                 // add hit code here
00275                             } else {
00276                                 //add release code here
00277                             }
00278                             break;
00279                         case '3': //number button 3
00280                             if (bhit=='1') {
00281                                 //add hit code here
00282                             } else {
00283                                 //add release code here
00284                             }
00285                             break;
00286                         case '4': //number button 4
00287                             if (bhit=='1') {
00288                                 //add hit code here
00289                             } else {
00290                                 //add release code here
00291                             }
00292                             break;
00293                         case '5': //button 5 up arrow
00294                             if (bhit=='1') {
00295                                 //add hit code here
00296                                 forward();
00297                             } else {
00298                                 //add release code here
00299                                 stop();
00300                             }
00301                             break;
00302                         case '6': //button 6 down arrow
00303                             if (bhit=='1') {
00304                                 //add hit code here
00305                                 backward();
00306                             } else {
00307                                 //add release code here
00308                                 stop();
00309                             }
00310                             break;
00311                         case '7': //button 7 left arrow
00312                             if (bhit=='1') {
00313                                 //add hit code here
00314                                 left();
00315                             } else {
00316                                 //add release code here
00317                                 stop();
00318                             }
00319                             break;
00320                         case '8': //button 8 right arrow
00321                             if (bhit=='1') 
00322                             {
00323                                 //add hit code here
00324                                 right();
00325                             } 
00326                             else 
00327                             {
00328                                 //add release code here
00329                                 stop();
00330                             }
00331                             break;
00332                         default:
00333                             // stop();
00334                             break;
00335                     }
00336                 }
00337             }
00338         }
00339     }
00340    
00341 }