main.cpp

00001 #include "mbed.h"
00002 #include <stdio.h>
00003 #include "Motor.h"
00004 #include "rgbled.h"
00005 #include "rgbSensor.h"
00006 #include "universal.h"
00007 #include <ctime>
00008 
00009 // Define threads and mutexes
00010 Thread thread1;
00011 Thread thread2;
00012 Thread thread3;
00013 
00014 // Global game actions
00015 bool paused = false;
00016 void check_unpause() {
00017     char bnum=0;
00018     char bhit=0;
00019     while (paused) {
00020         if (blue.getc()=='B') { //button data packet
00021             bnum = blue.getc(); //button number
00022             bhit = blue.getc(); //1=hit, 0=release
00023             if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
00024                 if (bnum == '1') { //number button 1, pause
00025                     if (bhit=='1') paused = false;
00026                 }
00027             }
00028         }
00029     }
00030 }
00031 
00032 void game_paused() {
00033     // Cycle through LEDs
00034     sstate = pause;
00035     Thread pause_thread;
00036     pause_thread.start(check_unpause);
00037     while (paused) {
00038         int num = 1;
00039         for (int i = 0; i < 3; i++) {
00040             num *= 2;
00041             myled = num;
00042             ThisThread::sleep_for(100);
00043         }
00044         ThisThread::sleep_for(500);
00045         for (int i = 3; i > 0; i--) {
00046             num /= 2;
00047             myled = num;
00048             ThisThread::sleep_for(100);
00049         }
00050         ThisThread::sleep_for(500);
00051     }
00052     pause_thread.terminate();
00053 }
00054 
00055 void win() {
00056     while(1) {
00057         unsigned int rgbColor[3];
00058         
00059         // Start off with red.
00060         rgbColor[0] = 255;
00061         rgbColor[1] = 0;
00062         rgbColor[2] = 0;  
00063         
00064         // Choose the colors to increment and decrement.
00065         for (int decColor = 0; decColor < 3; decColor += 1) {
00066             int incColor = decColor == 2 ? 0 : decColor + 1;
00067             
00068             // cross-fade the two colors.
00069             for(int i = 0; i < 255; i += 1) {
00070                 rgbColor[decColor] -= 1;
00071                 rgbColor[incColor] += 1;
00072                 
00073                 myRGBled.write(rgbColor[0]/255.0, rgbColor[1]/255.0, rgbColor[2]/255.0);
00074                 ThisThread::sleep_for(1);
00075             }
00076         }
00077     }
00078 }
00079 
00080 // Thread to control speed
00081 void speed_control() {
00082     // The kart has to be in one of three states at all times
00083     while(1) {
00084         while(sstate == coasting) {
00085             if (speed_cmd > 0.0) speed_cmd -= coast_rate;
00086             else speed_cmd = 0.0;
00087             left.speed(speed_cmd * left_multiplier);
00088             right.speed(speed_cmd * right_multiplier);
00089             ThisThread::sleep_for(200);
00090         }
00091         while(sstate == accelerating) {
00092             if (speed_cmd < max_speed) speed_cmd += acceleration_rate;
00093             else speed_cmd = max_speed;
00094             left.speed(speed_cmd * left_multiplier);
00095             right.speed(speed_cmd * right_multiplier);
00096             ThisThread::sleep_for(200);
00097         }
00098         while(sstate == braking) {
00099             if (speed_cmd > -1.0) speed_cmd -= brake_rate;
00100             else speed_cmd = -1.0;
00101             left.speed(speed_cmd * left_multiplier);
00102             right.speed(speed_cmd * right_multiplier);
00103             ThisThread::sleep_for(200);
00104         }
00105         while(sstate == pause) {
00106             left.speed(0.0);
00107             right.speed(0.0);
00108         }
00109     }
00110 }
00111 
00112 void powerupthread() {
00113     while(1){
00114         if(powerup){
00115             max_speed = 1.0;
00116             ThisThread::sleep_for(5000);
00117             max_speed = 0.8;
00118             powerup = false;
00119         }
00120         ThisThread::sleep_for(300);
00121     }
00122 }
00123 
00124 // Thread for checking rgb sensor values and updating game variables
00125 // Change later depending on behavior of RGB sensor and colors used
00126 int t = 8000;   //change this depending on RGB values
00127 void check_RGB() {
00128     while(true){
00129         rgbsensor.update();
00130         int C_value = rgbsensor.get_C();
00131         int R_value = rgbsensor.get_R();
00132         int G_value = rgbsensor.get_G();
00133         int B_value = rgbsensor.get_B();
00134         if(R_value < 3000 && B_value > 6500) {     //Check for if cart runs over a speed boost panel
00135             pc.printf("Found blue");
00136             powerup = true;      //change this depending on speed up panel algs
00137             cstate = 0;
00138         }
00139         else if(R_value > 3000 && G_value < 2000 && B_value < 2000) {     //Check if cart reaches next checkpoint. cstate is set to 1 upon seeing first color of checkpoint
00140             pc.printf("Found red");
00141             paused = true;
00142             win();
00143         }
00144         pc.printf("[C: %d, R: %d, G: %d, B: %d]\r\n", C_value, R_value, G_value, B_value); 
00145         ThisThread::sleep_for(500);
00146     }
00147 }
00148 
00149 int main() {
00150     pc.printf("Initializing ");
00151     
00152     // Start threads
00153     thread1.start(check_RGB);
00154     thread2.start(speed_control); // Since we're stopped, this won't do anything
00155     thread3.start(powerupthread);
00156     
00157     std::clock_t start;
00158     double duration;
00159     start = std::clock();
00160     
00161     // Bluetooth controller code
00162     char bnum=0;
00163     char bhit=0;
00164     pc.printf("running");
00165     while(running) {
00166         if (blue.getc()=='!') {
00167             if (blue.getc()=='B') { //button data packet
00168                 bnum = blue.getc(); //button number
00169                 bhit = blue.getc(); //1=hit, 0=release
00170                 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
00171                     switch (bnum) {
00172                         case '1': //number button 1, pause
00173                             if (bhit=='1') {
00174                                 paused = true;
00175                                 game_paused();
00176                             }
00177                             break;
00178                         case '2': //number button 2, accelerate
00179                             if (bhit=='1') {
00180                                 sstate = accelerating;
00181                                 pc.printf("accelerating");
00182                             } else {
00183                                 sstate = coasting;
00184                             }
00185                             break;
00186                         case '3': //number button 3
00187                             if (bhit=='1') {
00188                             } else {
00189                             }
00190                             break;
00191                         case '4': //number button 4, brakes
00192                             if (bhit=='1') {
00193                                 sstate = braking;
00194                             } else {
00195                                 sstate = coasting;
00196                             }
00197                             break;
00198                         case '5': //button 5 up arrow
00199                             if (bhit=='1') {
00200                                 pc.printf("up");
00201                             } else {
00202                             }
00203                             break;
00204                         case '6': //button 6 down arrow
00205                             if (bhit=='1') {
00206                             } else {
00207                             }
00208                             break;
00209                         case '7': //button 7 left arrow
00210                             if (bhit=='1') {
00211                                 pc.printf("left");
00212                                 left_multiplier = 0.5;
00213                                 right_multiplier = 1.0;
00214                             } else {
00215                                 left_multiplier = 1.0;
00216                                 right_multiplier = 1.0;
00217                             }
00218                             break;
00219                         case '8': //button 8 right arrow
00220                             if (bhit=='1') {
00221                                 pc.printf("right");
00222                                 left_multiplier = 1.0;
00223                                 right_multiplier = 0.5;
00224                             } else {
00225                                 left_multiplier = 1.0;
00226                                 right_multiplier = 1.0;
00227                             }
00228                             break;
00229                         default:
00230                             break;
00231                     }
00232                 }
00233             }
00234         }
00235     }
00236     // Clean up
00237     duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
00238     pc.printf("%d",duration);
00239     thread1.terminate();
00240     thread2.terminate();
00241     thread3.terminate();
00242 }