main.cpp

00001 /*
00002  *   OvniBot
00003  *   
00004  */
00005  
00006 #include "XNucleo6180XA1.h"
00007 #undef printf
00008 #include "Crealab.h"
00009 
00010 Serial bt_uart(PA_9, PA_10);  // PA9 = Tx, PA10 = Rx
00011 Serial pc_uart(USBTX, USBRX); // USBTX = PA2, USBRX = PA3
00012 
00013 // ---------------- PIN DEFINITIONS ---------------------
00014 InterruptIn  buttonBox(PC_13);
00015 Motor motorRG(PB_13, PB_14, PB_15, PB_1);
00016 Motor motorRD(PA_0, PA_1, PA_4, PB_0);
00017 
00018 // insert the motors and indicate wheel diameter and distance between wheels
00019 Creabot mybot(&motorRG, &motorRD, 9.0f,13.2f); // proto CréaLab
00020 
00021 // PIN & number of LEDS. Available color ==> BLUE, ORANGE, RED, GREEN, BLACK, WHITE, PURPLE, PINK, YELLOW
00022 LED_WS2812 ledBand(PB_3,4);
00023 
00024 bool receivedCOMMAND;
00025 char commandRECEIVED;
00026 int parameterRECEIVED;
00027 int state;
00028 char commandLine[256];
00029 int commandPosition;
00030 uint32_t dist;
00031 
00032 // ----------- PIN Definitions 6180 -------------
00033 #define VL6180X_I2C_SDA   D14 
00034 #define VL6180X_I2C_SCL   D15 
00035 static XNucleo6180XA1 *board = NULL;
00036 // penser à modifier dans vl6180x_platform.h -> commenter la ligne // #define MY_LOG 1
00037 
00038 void help() // Display list of Commands
00039 {
00040     DEBUG("List of commands:\n\r");
00041     DEBUG(" h --> Help, display list of commands\n\r");
00042 }
00043 
00044 void callback() {
00045     switch(state){
00046      case 0:
00047        break;
00048      case 1:
00049        break;
00050      case 2:
00051        break;
00052      case 3:
00053        break;
00054      default:
00055      }
00056     state=state+1;
00057 }
00058 
00059 void detect(int parameter)
00060 {
00061     mybot.move(FORWARD,300);
00062     dist=250;
00063     while (dist>parameter)
00064        {wait (0.2); 
00065        board->sensor_bottom->get_distance(&dist) ; }
00066 //    DEBUG ("Distance: %d \n\r", dist);
00067     mybot.stopMove();
00068 }
00069 
00070 void routine1() 
00071 {
00072      DEBUG("routine1\n\r");
00073      wait(1);
00074      mybot.move(FORWARD,200);
00075      ledBand.SetColor(BLUE,0) ;
00076      ledBand.SetColor(BLACK,1) ;
00077      ledBand.SetColor(BLACK,2) ;
00078      ledBand.SetColor(BLACK,3) ;
00079      ledBand.StartRotation(0.5) ;
00080      wait(2);
00081       ledBand.SetColor(WHITE,0) ;
00082      ledBand.SetColor(BLACK,1) ;
00083      ledBand.SetColor(BLACK,2) ;
00084      ledBand.SetColor(BLACK,3) ;
00085      ledBand.StartRotation(0.5) ;    
00086      state=0;  
00087 /*     mybot.move(FORWARD,9);
00088       dist=200;
00089      while (dist>120)
00090            {wait (0.3); 
00091            board->sensor_bottom->get_distance(&dist) ; }
00092      DEBUG ("Distance: %d \n\r", dist);
00093      mybot.stopMove();
00094           ledBand.SetColor(WHITE,0) ;
00095      ledBand.SetColor(RED,1) ;
00096      ledBand.SetColor(BLUE,2) ;
00097      ledBand.SetColor(GREEN,3) ; */                  
00098 } 
00099 
00100 void routine2() 
00101 {
00102      DEBUG("routine2\n\r");
00103      wait(1);
00104      ledBand.SetColor(WHITE,0) ;
00105      ledBand.SetColor(RED,1) ;
00106      ledBand.SetColor(BLUE,2) ;
00107      ledBand.SetColor(GREEN,3) ;
00108      ledBand.StartRotation(0.6) ;
00109      mybot.move(BACKWARD,200);
00110      state=0;  
00111 /*     mybot.move(FORWARD,9);
00112       dist=200;
00113      while (dist>120)
00114            {wait (0.3); 
00115            board->sensor_bottom->get_distance(&dist) ; }
00116      DEBUG ("Distance: %d \n\r", dist);
00117      mybot.stopMove(); */                  
00118 } 
00119 
00120 void routines() 
00121 {
00122      DEBUG("routines\n\r");
00123      wait(1);
00124      ledBand.StopRotation();
00125      ledBand.SetColor(BLACK,0) ;
00126      ledBand.SetColor(BLACK,1) ;
00127      ledBand.SetColor(BLACK,2) ;
00128      ledBand.SetColor(BLACK,3) ; 
00129      mybot.stopMove();
00130      state=0;                 
00131 } 
00132 
00133 
00134 void executeCommand(char c) {
00135         bool flaghelp = false;
00136         switch (c) {
00137             case 'h':
00138                 help();
00139                 state=0;
00140                 flaghelp=true;
00141                 CASE('l', "lance la routine 1", routine1();  )
00142                 CASE('m', "lance la routine 2", routine2();  )
00143                 CASE('s', "stoppe les routines", routines();  )
00144                 CASE('t', "accelere", routines();  )                          
00145             default :
00146                 DEBUG("invalid command; use: 'h' for help()\n\r");
00147                 state=0;
00148         }}
00149 
00150 void analyseCommand(char *command) {
00151     switch(command[0]) {
00152         case 'l':
00153             commandRECEIVED = 'l';
00154              break;
00155         case 's':
00156             commandRECEIVED = 's';
00157              break;
00158         case 'm':
00159             commandRECEIVED = 'm';
00160              break;
00161         case 't':
00162             commandRECEIVED = 't';
00163              break;
00164        default:
00165             commandRECEIVED = 'h';
00166     } }
00167 
00168 void checkCommand(int result, char *command) {
00169     if(result==1) {
00170       analyseCommand(command);        
00171 //    DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED,state);
00172       receivedCOMMAND = true;
00173     } }
00174 
00175 void split(char *line, int length) {
00176     char command[256];
00177     int parameter=0;
00178     int result = 1;
00179     int i=0;
00180     int j=0;
00181     while(i<length && line[i]==' ') {
00182         i++;}
00183     while(i<length && line[i]!=' ') {
00184          command[j]=line[i];
00185          i++;
00186          j++;}
00187     command[j]=0;
00188     i++;
00189     j=0;
00190      while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
00191        i++;
00192        j++;}
00193     if(j>0) {
00194         result++;
00195            i--;
00196         int k=1;
00197         while(j>0) {
00198            parameter += (line[i]-'0')*k;
00199             j--;
00200            i--;
00201            k=k*10;}
00202      }
00203   checkCommand(result, command);
00204 }
00205 
00206 void storeC(char c) {
00207     if(c==10 || c==13|| commandPosition >= 255) {
00208        split(commandLine, commandPosition);   
00209        commandPosition=0;}
00210     else {
00211       commandLine[commandPosition++]=c;
00212       commandLine[commandPosition]=0;}   
00213 }
00214 
00215 /*void getBT() {
00216     char c = bt_uart.getc();
00217     storeC(c);
00218 }*/
00219 
00220 void getPC() {
00221     char c = pc_uart.getc();
00222     storeC(c);
00223 }
00224 
00225 /* Stop all processes */
00226 void stop_all()
00227 {
00228     mybot.stopMove();
00229 }
00230 
00231 /* Interrupt routine, switch of end of course */
00232 void clicked()
00233 {
00234 //    DEBUG("Labyrinthe\n\r");
00235     commandRECEIVED = 'l';
00236     receivedCOMMAND = true;
00237 }
00238 
00239 void endOfMove(int status) {
00240     DEBUG("ENDOFMOVE\n\r");
00241     state=0;
00242 }
00243 
00244 /* Main Routine */
00245 int main()
00246 {
00247   wait(1);
00248     
00249     /* Connect EoC button */
00250     CATCH_BUTTON(buttonBox,clicked);
00251     
00252      /* Init 6180 X-Nucleo  */
00253     int status;
00254     DevI2C *device_i2c = new DevI2C(VL6180X_I2C_SDA, VL6180X_I2C_SCL);
00255     board = XNucleo6180XA1::instance(device_i2c, A5, A4, D13, D4);
00256     status = board->init_board();
00257     if (status) {DEBUG("Failed to init board!\n\r"); } 
00258      
00259     commandPosition=0;
00260 //    bt_uart.attach(getBT);
00261     pc_uart.attach(getPC);
00262     mybot.setCallBack(endOfMove);
00263     mybot.setSpeed(5); // max 8 cm.s, average 5 cm.s
00264     ledBand.SetColor(BLACK);
00265     state=0;
00266     receivedCOMMAND = false;
00267     DEBUG("OvniBot\n\r");
00268     while(1) {
00269        if(state==0 && receivedCOMMAND) {
00270             receivedCOMMAND = false;
00271             state=1;
00272             executeCommand(commandRECEIVED);
00273         }
00274        wait (0.3); 
00275        board->sensor_bottom->get_distance(&dist) ;  
00276        DEBUG ("Distance bottom: %d \n\r", dist);
00277        if (dist<100) {
00278            commandRECEIVED = 'l';
00279            executeCommand(commandRECEIVED);}
00280        wait (0.3); 
00281        board->sensor_top->get_distance(&dist) ;  
00282        DEBUG ("Distance top: %d \n\r", dist);
00283        if (dist<100) {
00284            commandRECEIVED = 's';
00285            executeCommand(commandRECEIVED);}
00286        wait(0.1);
00287     } }