DuckieTownCar.cpp

00001 #include "DuckieTownCar.h"
00002 
00003 #define sens 70
00004 
00005 DuckieTownCar* DuckieTownCar::instance = NULL;//inizializzo l'istanza dell'oggetto a null
00006 
00007 DuckieTownCar* DuckieTownCar::getInstance(){
00008     if(instance == NULL) instance = new DuckieTownCar;
00009     return instance;
00010 }
00011 
00012 DuckieTownCar::DuckieTownCar(){
00013     srand(us_ticker_read());
00014     //Stavo iniziando a mettere il codice dei ragazzi che hanno scritto la parte di giroscopio, ma non so a cosa sono arrivati quini
00015     //mi sono fermato col prendere il codice dell'accelerometro! Prima di usare il loro codice aspetterei la calibrazione dei 4 angoli
00016     X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
00017     
00018     this->acc_gyro = mems_expansion_board->acc_gyro;
00019     /*******************************************************************
00020     * Qui serve inizializzare tutti i componenti che servono all'auto! *
00021     * In particolare: giroscopio, servomotori, sensori di prossimità,  *
00022     * e infine i sensori di colore per la strada!                      *
00023     ********************************************************************/
00024 }
00025 
00026 void DuckieTownCar::initCarSensor()
00027 {
00028     /* Enable all sensors */
00029     acc_gyro->Enable_X();
00030     acc_gyro->Enable_G();
00031 #ifdef DEBUG_CAR 
00032     uint8_t id;
00033     printf("\r\n--- Starting new run ---\r\n");
00034     acc_gyro->ReadID(&id);
00035     printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00036     wait(1.5);
00037 #endif
00038     acc_gyro->Get_G_Axes(axes);
00039 #ifdef DEBUG_CAR 
00040     printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
00041 #endif
00042     for(int i=0;i<3;i++){
00043         parziale_angolo[i] = angolo[i] = 0.0;
00044         off[i]=axes[i];
00045     }
00046 }
00047 
00048 void DuckieTownCar::execute() const{
00049     Code* end_code = this->code.code+this->code.dim_code;
00050     bool curva = false;//il gettone permette di capire se ho completato la curva e posso tornare a camminare dritto!
00051     unsigned char cop = 0;
00052     for(register Code* i = this->code.code;i!=end_code;i++)
00053     {
00054         cop = ((*i).cop != RANDOM_CODE) ? (*i).cop : (rand() % N_COP)+1;
00055         switch(cop)
00056         {
00057         case DRITTO_CODE://va dritto finchè può o deve
00058             //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
00059                 
00060             //qui in mezzo ci sarà un controllo sulla curva e setterà curva=true e viene settata il cop con la curva specifica!
00061                 break;
00062         case RIGHT_CODE://gira a destra date delle giuste condizioni iniziali
00063            //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
00064            //utile per le rotatorie la ripetizione
00065                  break;
00066         case LEFT_CODE://gira a sinistra nelle giuste condizioni
00067            //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
00068            //utile per le rotatorie la ripetizione
00069                  break;
00070         }
00071         while(curva){//finchè c'è una curva lui la eseguirà senza uscire dal ciclo
00072             switch(cop)//viene assegnato nel comando dritto, in un raggionamento "euristico" si è compreso la direzione
00073             {
00074             case CURVA_RIGHT_CODE:
00075                 
00076                 break;
00077             case CURVA_LEFT_CODE:
00078             
00079                 break;
00080             }
00081             curva = false;//uscito dallo switch l'operazione sarà completata!
00082             
00083             /*Qui va messo il codice per farlo andare dritto ora se il codice rileva di nuovo la curva allora curva verrà risettato a true*/
00084         }
00085     }
00086 }
00087 
00088 void DuckieTownCar::executeLine(unsigned char cop) const
00089 {
00090     switch(cop)
00091     {
00092     case DRITTO_CODE://va dritto finchè può o deve
00093     //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta    
00094     //nel caso della curva sarà fatto tutto in questo codice qui!
00095        switch(cop)//viene assegnato nel comando dritto, in un raggionamento "euristico" si è compreso la direzione
00096        {
00097        case CURVA_RIGHT_CODE:          
00098             break;
00099        case CURVA_LEFT_CODE:   
00100             break;
00101        }
00102        break;
00103     case RIGHT_CODE://gira a destra date delle giuste condizioni iniziali
00104     //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
00105     //utile per le rotatorie la ripetizione
00106         break;
00107     case LEFT_CODE://gira a sinistra nelle giuste condizioni
00108     //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
00109     //utile per le rotatorie la ripetizione
00110         break;
00111     }
00112 }
00113 
00114 void DuckieTownCar::executeRandom() const
00115 {
00116     unsigned char cop = rand()%N_COP +1;
00117     while(1)  this->executeLine(cop);
00118 }
00119 
00120 __forceinline void DuckieTownCar::updateStateCar(){
00121     this->infoAxes();//aggiornamento dell'angolo!
00122     
00123     //lo metto in commento perchè va prima inizializzato!
00124     //sensor_color.getReading();
00125 }
00126 
00127 /**************************************
00128 * Gruppo del giroscopio Accelerometro *
00129 ***************************************/
00130 void DuckieTownCar::infoAxes(){
00131     acc_gyro->Get_G_Axes(axes);
00132 #ifdef DEBUG_CAR
00133     printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
00134 #endif
00135     for(int i=0;i<3;i++)
00136         axes[i]-=off[i];
00137     
00138 #ifdef DEBUG_CAR
00139    printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
00140 #endif
00141     wait_ms(1);
00142     // ricavo il parziale dalla velocità angolare
00143     for(int i=0;i<3;i++)
00144     {
00145         parziale_angolo[i]=(float)(axes[i]*sens)/1000.0;// passo da mdps a dpLSB
00146         angolo[i]=(angolo[i]-19.0)/2.84;// levo la correzione per poter sommare i dati parziali
00147         parziale_angolo[i]/= 1000.0;// moltiplico per il dt (1ms)
00148         if (axes[i]>150 || axes[i]<-150)
00149             angolo[i] += parziale_angolo[i];//integro 
00150     
00151         angolo[i]=(angolo[i]*2.84)+19;//correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta)
00152     }
00153 }