File content as of revision 0:65ecca01ac5c:

#include "DuckieTownCar.h"

#define sens 70

DuckieTownCar* DuckieTownCar::instance = NULL;//inizializzo l'istanza dell'oggetto a null

DuckieTownCar* DuckieTownCar::getInstance(){
    if(instance == NULL) instance = new DuckieTownCar;
    return instance;
}

DuckieTownCar::DuckieTownCar(){
    srand(us_ticker_read());
    //Stavo iniziando a mettere il codice dei ragazzi che hanno scritto la parte di giroscopio, ma non so a cosa sono arrivati quini
    //mi sono fermato col prendere il codice dell'accelerometro! Prima di usare il loro codice aspetterei la calibrazione dei 4 angoli
    X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
    
    this->acc_gyro = mems_expansion_board->acc_gyro;
    /*******************************************************************
    * Qui serve inizializzare tutti i componenti che servono all'auto! *
    * In particolare: giroscopio, servomotori, sensori di prossimità,  *
    * e infine i sensori di colore per la strada!                      *
    ********************************************************************/
}

void DuckieTownCar::initCarSensor()
{
    /* Enable all sensors */
    acc_gyro->Enable_X();
    acc_gyro->Enable_G();
#ifdef DEBUG_CAR 
    uint8_t id;
    printf("\r\n--- Starting new run ---\r\n");
    acc_gyro->ReadID(&id);
    printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
    wait(1.5);
#endif
    acc_gyro->Get_G_Axes(axes);
#ifdef DEBUG_CAR 
    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
#endif
    for(int i=0;i<3;i++){
        parziale_angolo[i] = angolo[i] = 0.0;
        off[i]=axes[i];
    }
}

void DuckieTownCar::execute() const{
    Code* end_code = this->code.code+this->code.dim_code;
    bool curva = false;//il gettone permette di capire se ho completato la curva e posso tornare a camminare dritto!
    unsigned char cop = 0;
    for(register Code* i = this->code.code;i!=end_code;i++)
    {
        cop = ((*i).cop != RANDOM_CODE) ? (*i).cop : (rand() % N_COP)+1;
        switch(cop)
        {
        case DRITTO_CODE://va dritto finchè può o deve
            //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
                
            //qui in mezzo ci sarà un controllo sulla curva e setterà curva=true e viene settata il cop con la curva specifica!
                break;
        case RIGHT_CODE://gira a destra date delle giuste condizioni iniziali
           //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
           //utile per le rotatorie la ripetizione
                 break;
        case LEFT_CODE://gira a sinistra nelle giuste condizioni
           //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
           //utile per le rotatorie la ripetizione
                 break;
        }
        while(curva){//finchè c'è una curva lui la eseguirà senza uscire dal ciclo
            switch(cop)//viene assegnato nel comando dritto, in un raggionamento "euristico" si è compreso la direzione
            {
            case CURVA_RIGHT_CODE:
                
                break;
            case CURVA_LEFT_CODE:
            
                break;
            }
            curva = false;//uscito dallo switch l'operazione sarà completata!
            
            /*Qui va messo il codice per farlo andare dritto ora se il codice rileva di nuovo la curva allora curva verrà risettato a true*/
        }
    }
}

void DuckieTownCar::executeLine(unsigned char cop) const
{
    switch(cop)
    {
    case DRITTO_CODE://va dritto finchè può o deve
    //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta    
    //nel caso della curva sarà fatto tutto in questo codice qui!
       switch(cop)//viene assegnato nel comando dritto, in un raggionamento "euristico" si è compreso la direzione
       {
       case CURVA_RIGHT_CODE:          
            break;
       case CURVA_LEFT_CODE:   
            break;
       }
       break;
    case RIGHT_CODE://gira a destra date delle giuste condizioni iniziali
    //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
    //utile per le rotatorie la ripetizione
        break;
    case LEFT_CODE://gira a sinistra nelle giuste condizioni
    //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
    //utile per le rotatorie la ripetizione
        break;
    }
}

void DuckieTownCar::executeRandom() const
{
    unsigned char cop = rand()%N_COP +1;
    while(1)  this->executeLine(cop);
}

__forceinline void DuckieTownCar::updateStateCar(){
    this->infoAxes();//aggiornamento dell'angolo!
    
    //lo metto in commento perchè va prima inizializzato!
    //sensor_color.getReading();
}

/**************************************
* Gruppo del giroscopio Accelerometro *
***************************************/
void DuckieTownCar::infoAxes(){
    acc_gyro->Get_G_Axes(axes);
#ifdef DEBUG_CAR
    printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
#endif
    for(int i=0;i<3;i++)
        axes[i]-=off[i];
    
#ifdef DEBUG_CAR
   printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
#endif
    wait_ms(1);
    // ricavo il parziale dalla velocità angolare
    for(int i=0;i<3;i++)
    {
        parziale_angolo[i]=(float)(axes[i]*sens)/1000.0;// passo da mdps a dpLSB
        angolo[i]=(angolo[i]-19.0)/2.84;// levo la correzione per poter sommare i dati parziali
        parziale_angolo[i]/= 1000.0;// moltiplico per il dt (1ms)
        if (axes[i]>150 || axes[i]<-150)
            angolo[i] += parziale_angolo[i];//integro 
    
        angolo[i]=(angolo[i]*2.84)+19;//correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta)
    }
}