Diff: VM_Car/DuckieTownCar.cpp

Revision:

0:65ecca01ac5c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VM_Car/DuckieTownCar.cpp	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,153 @@
+#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)
+    }
+}
\ No newline at end of file