main.cpp

00001 // ECE 4180 Gesture Sensor Controlled Robot Project: Robot Comm, Heading and Motor Control, Obstacle Detection
00002 
00003 #include "mbed.h"
00004 #include "motordriver.h"
00005 
00006 //#define DEBUG
00007 //************************************************************
00008 
00009 //Declarations
00010 
00011 //************************************************************
00012 DigitalOut led1(LED1);
00013 DigitalOut led2(LED2);
00014 DigitalOut led3(LED3);
00015 DigitalOut led4(LED4);
00016 AnalogIn IR(p20);//Input for IR sensor, 0.60 seems like a good object detect distance
00017 InterruptIn REN(p15);//INTERRUPT IN PIN FOR RIGHT ENCODER
00018 InterruptIn LEN(p16);//INTERRUPT IN PIN FOR LEFT ENCODER
00019 int countR = 0;//global encoder count variables
00020 int countL = 0;
00021 
00022 
00023 //Serial pc(USBTX, USBRX);//PC serial for debug
00024 Serial xbee1(p13, p14);//xbee serial connection
00025 
00026 int count = 0; //counter for reading through debug command array;
00027 char debugcommand[] = "FS";//direction commands for debug sequence, can be used without xbee controll
00028 //int num = 0;
00029 char command = 'W'; // switch case variable to be used for commands from wireless comm, intialize W for wait
00030 bool collision_flag = 0;//flag to set high for collision handling
00031 bool once = 0;//send collision feedback once
00032 
00033 Motor  left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
00034 Motor right(p26, p25, p24, 1);
00035 
00036 bool x = true;//loop variable
00037 //************************************************************
00038 
00039 //Helper Functions
00040 
00041 //************************************************************
00042 //Functions for updating/reading encoder with interrupts
00043 void Rcount() //count Right encoder increments
00044 {
00045     countR++;
00046 }
00047 void Lcount() //count Left encoder increments
00048 {
00049     countL++;
00050 }
00051 void clearEN() //clear the encoder values
00052 {
00053     countR = 0;
00054     countL = 0;
00055 }
00056 //************************************************************
00057 
00058 //Main
00059 
00060 //************************************************************
00061 
00062 int main()
00063 {
00064     float sl = 0.65;//preset wheel speed left
00065     float sr = 0.65*0.92;//preset wheel speed right, account for wobble of axel
00066 
00067     REN.fall(&Rcount);//attach counting functions for encoder interrupts
00068     LEN.fall(&Lcount);
00069 
00070     led1 = 1;led2 = 1;led3 = 1;led4 = 1;
00071     wait(5);//setup period
00072     led1 = 0;led2 = 0;led3 = 0;led4 = 0;
00073 
00074     bool x = true;//loop variable
00075 
00076 
00077     while (x) {//program loop
00078 
00079         //************************************************************
00080         if((IR >=0.60) && (collision_flag == 0)) {//if object sensed and flag not already raised
00081             left.stop(1);//stop if flag not raised
00082             right.stop(1);
00083             collision_flag = 1;//raise flag
00084         }
00085         if(IR < 0.60) {//if no object sensed, reset flags
00086             collision_flag = 0;
00087             once = 0;
00088         }
00089         if((collision_flag && !(once))) {//flag high and feeback not yet sense
00090             xbee1.putc('C');
00091             //pc.printf("C\n\r");
00092             once = 1;
00093         }
00094         if(xbee1.readable()) {//if new command readable
00095             command = xbee1.getc();
00096             //pc.printf("Command: %c\n\r",command);
00097         }
00098         //************************************************************
00099 
00100         switch ( command ) {//switch statement for robot motor control
00101             case 'F': //move forward
00102                 //pc.printf("FORWARD\n\r");
00103                 led2 = 1;led3 = 1;//middle indicator lights
00104                 clearEN();
00105                 while ((countL <= 500)||(countR <=340)) {//loop specified distance, axel wobble led to different encoder increment rates
00106                     if(IR >=0.60) {
00107                         left.stop(1);
00108                         right.stop(1);
00109                         collision_flag = 1;
00110                         break;
00111                     }   //if IR has detected a close object, stop
00112                     if(xbee1.readable()) {// if new command while going forward
00113                         command = xbee1.getc();
00114                         //pc.printf("Command: %c\n\r",command);
00115                         break;
00116                     }
00117                     //pc.printf("Speed set\n\r");
00118                     left.speed(sl);
00119                     right.speed(0.69);//drift factor associated with our build
00120                     //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
00121                     wait(.01);
00122                 }
00123                 led2 = 0;led3 = 0;
00124                 left.stop(1);
00125                 right.stop(1);
00126                 command = 'W';
00127                 break;
00128 
00129             case 'R': //turn ~180 degrees right
00130                 //pc.printf("RIGHT\n\r");
00131                 led1 = 1;led2 = 1;
00132                 clearEN();
00133                 left.speed(sl);right.speed(-sr);
00134                 while ((countL <= 40)) { //countR <= 20)) {//loop turn for specified angle
00135                     //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
00136                     //right.speed(-sr);
00137                     if(xbee1.readable()) {//if new command
00138                         command = xbee1.getc();
00139                         //pc.printf("Command: %c\n\r",command);
00140                         break;
00141                     }
00142                 }
00143                 led1 = 0;led2 = 0;
00144                 left.stop(1);
00145                 right.stop(1);
00146                 command = 'W';
00147                 //pc.printf("END OF COMMAND:: RIGHT: %d, LEFT: %d\n\r", countR, countL);
00148                 break;
00149 
00150             case 'L': //turn 30 deg left
00151                 //pc.printf("LEFT\n\r");
00152                 led3 = 1;led4 = 1;
00153                 clearEN();
00154                 //loop turn for specified angle
00155                 while ((countR<=55)) {
00156                     left.speed(-sl);
00157                     right.speed(sr);
00158                     if(xbee1.readable()) {
00159                         command = xbee1.getc();
00160                         //pc.printf("Command: %c\n\r",command);
00161                         break;
00162                     }
00163                 }
00164                 led3 = 0;led4 = 0;
00165                 left.stop(1);
00166                 right.stop(1);
00167                 command = 'W';
00168                 break;
00169 
00170             case 'B': //bump robot backwards
00171                 pc.printf("BACKWARDS\n\r");
00172                 led1 = 1;led4 = 1;
00173                 left.speed(-sl);
00174                 right.speed(-sr);
00175                 wait(.5);//bump robot backwards by reversing both wheels for half a second
00176                 led1 = 0;led4 = 0;
00177                 left.stop(1);
00178                 right.stop(1);
00179                 command = 'W';
00180                 break;
00181 
00182             case 'W': //wait for a new command
00183                 pc.printf("WAIT\n\r");
00184                 wait(.5);
00185                 // Code
00186 #ifdef DEBUG
00187                 /*num = rand()%3;//randomization code, will run different commands indefinetly
00188                 switch( num ) {
00189                     case 1:
00190                         command = 'f';
00191                         break;
00192                     case 2:
00193                         command = 'r';
00194                         break;
00195                     case 3:
00196                         command = 'l';
00197                         break;
00198                     default:
00199                         command = 'b';
00200                         break;
00201                 }*/
00202                 command = debugcommand[count];//parse through debug command array
00203                 count++;
00204                 pc.printf("NEW COMMAND: %c", command);
00205 #endif
00206                 break;
00207                 
00208             default: //wait and show error
00209                 pc.printf("DEFAULT\n\r");
00210                 left.stop(0);
00211                 right.stop(0);
00212                 led1 = 1;led4 = 1;
00213                 wait(.5);
00214                 led2 = 1;led3 = 1;
00215                 led1 =0;led4  = 0;
00216                 wait(.5);
00217                 led2 = 0;led3 = 0;
00218                 break;
00219         }
00220     }
00221 }