main.cpp

00001 //This will rotate the wheels so that after 5 seconds the robot counterclockwise and then clockwise
00002 #include "mbed.h"
00003 #include "rtos.h"
00004 #include "motordriver.h"
00005 #include "MultiModalRobot.h"
00006 #include "uLCD_4DGL.h"
00007 
00008 uLCD_4DGL uLCD(p9,p10,p11); // serial tx, serial rx, reset pin;
00009 
00010 Motor lw(p26, p29, p30, 1); // pwm, fwd, rev   LEFT WHEEL
00011 Motor rw(p25, p28, p27, 1); // pwm, fwd, rev   RIGHT WHEEL
00012 MultiModalRobot robot(lw, rw);
00013 
00014 bool light = true;
00015 
00016 Serial blue(p13, p14);
00017 Serial py(USBTX, USBRX);
00018 
00019 BusOut leds(LED1,LED2,LED3,LED4);
00020 
00021 
00022 //Class to control an RGB LED using three PWM pins
00023 class RGBLed
00024 {
00025 public:
00026     RGBLed(PinName redpin, PinName greenpin, PinName bluepin);
00027     void write(float red,float green, float blue);
00028 private:
00029     PwmOut _redpin;
00030     PwmOut _greenpin;
00031     PwmOut _bluepin;
00032 };
00033 
00034 RGBLed::RGBLed (PinName redpin, PinName greenpin, PinName bluepin)
00035     : _redpin(redpin), _greenpin(greenpin), _bluepin(bluepin)
00036 {
00037     //50Hz PWM clock default a bit too low, go to 2000Hz (less flicker)
00038     _redpin.period(0.0005);
00039 }
00040 
00041 void RGBLed::write(float red,float green, float blue)
00042 {
00043     _redpin = red;
00044     _greenpin = green;
00045     _bluepin = blue;
00046 }
00047 //class could be moved to include file
00048 
00049 RGBLed myRGBled(p21,p22,p23); //RGB PWM pins
00050 
00051 bool visible;
00052 char lastSeen;
00053 
00054 int getYpos(){
00055     int ypos;
00056     char buffer[4];
00057     char y;
00058     while(1){
00059       char start = 0;
00060       start = py.getc();
00061       if(start == 's' || start == 'v'){
00062 
00063       } else {
00064         uLCD.printf("\nUnexpected char %c\n", start);
00065         continue;
00066       }
00067       if (start=='s') {
00068           if(py.getc()=='t') {
00069             if (py.getc() == 'a') {
00070                 if (py.getc() == 'r') {
00071                     if (py.getc() == 't') {
00072                         if (py.getc() == 'c') {
00073                             for (int count = 0; count < 4; count++) {
00074                                 y = py.getc();
00075                                 buffer[count] = y;
00076                                 if (count % 3 == 0) {
00077                                     //char snum[5];
00078                                     memcpy(&ypos, buffer, 4);
00079                                     if(ypos>120){
00080                                       lastSeen = 'r';
00081                                     } else {
00082                                       lastSeen = 'l';
00083                                     }
00084                                     if (light) { //Cop lights
00085                                       myRGBled.write(1.0,0.0,0.0);
00086                                       light = false;
00087                                     } else {
00088                                       myRGBled.write(0.0,0.0,1.0);
00089                                       light = true;
00090                                     }
00091                                 }
00092                             }
00093                             break;
00094                         }
00095                   }
00096               }
00097           }
00098         }
00099       } else if(start=='v'){
00100           if(py.getc()=='w'){
00101             if(py.getc()=='x'){
00102               if(py.getc()=='y'){
00103                 if(py.getc()=='z'){
00104                   ypos = -1;
00105                   break;
00106                 }
00107               }
00108             }
00109           }
00110       } else {
00111         uLCD.printf("\nUNEXPECTED\n"); //Default Green on black text
00112       }
00113     }
00114     uLCD.cls();
00115     uLCD.printf("\nReceived %d\n", ypos); //Default Green on black text
00116     //uLCD.freeBUFFER();
00117     return ypos;
00118 }
00119 
00120 
00121 int main() {
00122     lastSeen = 'n';
00123     py.baud(9600);
00124     char bnum = 0;
00125     char bhit = 0;
00126     char needToStopRobot = 0;
00127     float leftSpeed = 0;
00128     float rightSpeed = 0;
00129     float DEFAULT_SPEED = 1.0;
00130     float ROTATE_SPEED = 0.25;
00131 
00132     while(1) {
00133         wait(0.1);
00134         if(blue.readable() && blue.getc() == '!') {
00135             if(blue.readable() && blue.getc() == 'B') {
00136                 bnum = blue.getc();
00137                 bhit = blue.getc();
00138                 blue.getc();
00139                 switch(bnum) {
00140                     case '1':
00141                         needToStopRobot = 1;
00142                         break;
00143                     case '2': // become follower
00144                         py.putc('s');
00145                         int ypos;
00146                         while (1) {
00147                             ypos = getYpos();
00148                             if (ypos == 251) { //exit condition since camera is 240 wide
00149                               myRGBled.write(0.0,1.0,0.0);
00150                               robot.driveWheels(0, 0);
00151                               break;
00152                             }
00153                             visible = (ypos>=0);
00154                             if (visible) { // CAMERA CAN SEE A CIRCLE
00155                                 if (light) { //Cop lights
00156                                   myRGBled.write(1.0,0.0,0.0);
00157                                   light = false;
00158                                 } else {
00159                                   myRGBled.write(0.0,0.0,1.0);
00160                                   light = true;
00161                                 }
00162                                 leds[3] = 1;
00163                                 if(ypos<159 && ypos>80) {
00164                                     leftSpeed = DEFAULT_SPEED;
00165                                     rightSpeed = DEFAULT_SPEED;
00166                                     robot.driveWheels(leftSpeed, rightSpeed);
00167                                     leds[0] = 1;
00168                                     leds[1] = 0;
00169                                     leds[2] = 0;
00170                                 } else if(ypos>159) {
00171                                     while(ypos>120){
00172                                         ypos = getYpos();
00173                                         if(ypos<0){
00174                                          break;
00175                                         }
00176                                         leds[0] = 0;
00177                                         leds[1] = 1;
00178                                         leds[2] = 0;
00179                                         leftSpeed = ROTATE_SPEED;
00180                                         rightSpeed = -ROTATE_SPEED;
00181                                         robot.driveWheels(leftSpeed, rightSpeed);
00182                                     }
00183                                     robot.stop(0.5);
00184                                 } else {
00185                                     while(ypos<120){
00186                                         ypos = getYpos();
00187                                         if(ypos<0){
00188                                          break;
00189                                         }
00190                                         leds[0] = 0;
00191                                         leds[1] = 0;
00192                                         leds[2] = 1;
00193                                         leftSpeed = -ROTATE_SPEED;
00194                                         rightSpeed = ROTATE_SPEED;
00195                                         robot.driveWheels(leftSpeed, rightSpeed);
00196                                     }
00197                                     robot.stop(0.5);
00198                                 }
00199 
00200 
00201                             } else { // CIRCLE NOT VISIBLE
00202                                 leds[0] = 0;
00203                                 leds[1] = 0;
00204                                 leds[2] = 0;
00205                                 leds[3] = 0;
00206                                 if(lastSeen=='r'){
00207                                   leftSpeed = ROTATE_SPEED;
00208                                   rightSpeed = -ROTATE_SPEED;
00209                                 } else {
00210                                   leftSpeed = -ROTATE_SPEED;
00211                                   rightSpeed = ROTATE_SPEED;
00212                                 }
00213                                 robot.driveWheels(leftSpeed, rightSpeed);
00214                             }
00215                         }
00216                         // stop thread
00217                     case '5': //up
00218                         if(bhit=='1'){
00219                             leftSpeed = rightSpeed = DEFAULT_SPEED;
00220                         } else {
00221                             needToStopRobot = 1;
00222                         }
00223                         break;
00224                     case '6': //down
00225                         if(bhit=='1') {
00226                             leftSpeed = rightSpeed = -DEFAULT_SPEED;
00227                         } else {
00228                             needToStopRobot = 1;
00229                         }
00230                         break;
00231                     case '7': //left
00232                         if(bhit=='1') {
00233                             leftSpeed = -DEFAULT_SPEED;
00234                             rightSpeed = DEFAULT_SPEED;
00235                         } else {
00236                             needToStopRobot = 1;
00237                         }
00238                         break;
00239                     case '8': //right
00240                         if(bhit=='1') {
00241                             leftSpeed = DEFAULT_SPEED;
00242                             rightSpeed = -DEFAULT_SPEED;
00243                         } else {
00244                             needToStopRobot = 1;
00245                         }
00246                         break;
00247                     default:
00248                         robot.stop(0.5);
00249                         break;
00250                 }
00251             }
00252         }
00253         if(needToStopRobot){
00254             robot.stop(0.5);
00255             needToStopRobot = 0;
00256             leftSpeed = 0;
00257             rightSpeed = 0;
00258             robot.driveWheels(leftSpeed, rightSpeed);
00259         } else {
00260             robot.driveWheels(leftSpeed, rightSpeed);
00261         }
00262     }
00263 }