DuckieTownHS_FabLab / Mbed 2 deprecated DuckieTownCar

DuckieTownCarHS

Dependencies:   TCS3200 X_NUCLEO_IKS01A2 mbed-rtos mbed

Fork of DuckieTownCar by Domenico Francesco De Angelis

VM_Car/DuckieTownCar.cpp@0:65ecca01ac5c, 2017-02-15 (annotated)

Committer:
deangelis1993
Date:
Wed Feb 15 13:27:01 2017 +0000
Revision:
0:65ecca01ac5c
DuckiTownCar first version for virtualization of machine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
deangelis1993 0:65ecca01ac5c 1 #include "DuckieTownCar.h"
deangelis1993 0:65ecca01ac5c 2
deangelis1993 0:65ecca01ac5c 3 #define sens 70
deangelis1993 0:65ecca01ac5c 4
deangelis1993 0:65ecca01ac5c 5 DuckieTownCar* DuckieTownCar::instance = NULL;//inizializzo l'istanza dell'oggetto a null
deangelis1993 0:65ecca01ac5c 6
deangelis1993 0:65ecca01ac5c 7 DuckieTownCar* DuckieTownCar::getInstance(){
deangelis1993 0:65ecca01ac5c 8 if(instance == NULL) instance = new DuckieTownCar;
deangelis1993 0:65ecca01ac5c 9 return instance;
deangelis1993 0:65ecca01ac5c 10 }
deangelis1993 0:65ecca01ac5c 11
deangelis1993 0:65ecca01ac5c 12 DuckieTownCar::DuckieTownCar(){
deangelis1993 0:65ecca01ac5c 13 srand(us_ticker_read());
deangelis1993 0:65ecca01ac5c 14 //Stavo iniziando a mettere il codice dei ragazzi che hanno scritto la parte di giroscopio, ma non so a cosa sono arrivati quini
deangelis1993 0:65ecca01ac5c 15 //mi sono fermato col prendere il codice dell'accelerometro! Prima di usare il loro codice aspetterei la calibrazione dei 4 angoli
deangelis1993 0:65ecca01ac5c 16 X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
deangelis1993 0:65ecca01ac5c 17
deangelis1993 0:65ecca01ac5c 18 this->acc_gyro = mems_expansion_board->acc_gyro;
deangelis1993 0:65ecca01ac5c 19 /*******************************************************************
deangelis1993 0:65ecca01ac5c 20 * Qui serve inizializzare tutti i componenti che servono all'auto! *
deangelis1993 0:65ecca01ac5c 21 * In particolare: giroscopio, servomotori, sensori di prossimità, *
deangelis1993 0:65ecca01ac5c 22 * e infine i sensori di colore per la strada! *
deangelis1993 0:65ecca01ac5c 23 ********************************************************************/
deangelis1993 0:65ecca01ac5c 24 }
deangelis1993 0:65ecca01ac5c 25
deangelis1993 0:65ecca01ac5c 26 void DuckieTownCar::initCarSensor()
deangelis1993 0:65ecca01ac5c 27 {
deangelis1993 0:65ecca01ac5c 28 /* Enable all sensors */
deangelis1993 0:65ecca01ac5c 29 acc_gyro->Enable_X();
deangelis1993 0:65ecca01ac5c 30 acc_gyro->Enable_G();
deangelis1993 0:65ecca01ac5c 31 #ifdef DEBUG_CAR
deangelis1993 0:65ecca01ac5c 32 uint8_t id;
deangelis1993 0:65ecca01ac5c 33 printf("\r\n--- Starting new run ---\r\n");
deangelis1993 0:65ecca01ac5c 34 acc_gyro->ReadID(&id);
deangelis1993 0:65ecca01ac5c 35 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
deangelis1993 0:65ecca01ac5c 36 wait(1.5);
deangelis1993 0:65ecca01ac5c 37 #endif
deangelis1993 0:65ecca01ac5c 38 acc_gyro->Get_G_Axes(axes);
deangelis1993 0:65ecca01ac5c 39 #ifdef DEBUG_CAR
deangelis1993 0:65ecca01ac5c 40 printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
deangelis1993 0:65ecca01ac5c 41 #endif
deangelis1993 0:65ecca01ac5c 42 for(int i=0;i<3;i++){
deangelis1993 0:65ecca01ac5c 43 parziale_angolo[i] = angolo[i] = 0.0;
deangelis1993 0:65ecca01ac5c 44 off[i]=axes[i];
deangelis1993 0:65ecca01ac5c 45 }
deangelis1993 0:65ecca01ac5c 46 }
deangelis1993 0:65ecca01ac5c 47
deangelis1993 0:65ecca01ac5c 48 void DuckieTownCar::execute() const{
deangelis1993 0:65ecca01ac5c 49 Code* end_code = this->code.code+this->code.dim_code;
deangelis1993 0:65ecca01ac5c 50 bool curva = false;//il gettone permette di capire se ho completato la curva e posso tornare a camminare dritto!
deangelis1993 0:65ecca01ac5c 51 unsigned char cop = 0;
deangelis1993 0:65ecca01ac5c 52 for(register Code* i = this->code.code;i!=end_code;i++)
deangelis1993 0:65ecca01ac5c 53 {
deangelis1993 0:65ecca01ac5c 54 cop = ((*i).cop != RANDOM_CODE) ? (*i).cop : (rand() % N_COP)+1;
deangelis1993 0:65ecca01ac5c 55 switch(cop)
deangelis1993 0:65ecca01ac5c 56 {
deangelis1993 0:65ecca01ac5c 57 case DRITTO_CODE://va dritto finchè può o deve
deangelis1993 0:65ecca01ac5c 58 //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
deangelis1993 0:65ecca01ac5c 59
deangelis1993 0:65ecca01ac5c 60 //qui in mezzo ci sarà un controllo sulla curva e setterà curva=true e viene settata il cop con la curva specifica!
deangelis1993 0:65ecca01ac5c 61 break;
deangelis1993 0:65ecca01ac5c 62 case RIGHT_CODE://gira a destra date delle giuste condizioni iniziali
deangelis1993 0:65ecca01ac5c 63 //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
deangelis1993 0:65ecca01ac5c 64 //utile per le rotatorie la ripetizione
deangelis1993 0:65ecca01ac5c 65 break;
deangelis1993 0:65ecca01ac5c 66 case LEFT_CODE://gira a sinistra nelle giuste condizioni
deangelis1993 0:65ecca01ac5c 67 //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
deangelis1993 0:65ecca01ac5c 68 //utile per le rotatorie la ripetizione
deangelis1993 0:65ecca01ac5c 69 break;
deangelis1993 0:65ecca01ac5c 70 }
deangelis1993 0:65ecca01ac5c 71 while(curva){//finchè c'è una curva lui la eseguirà senza uscire dal ciclo
deangelis1993 0:65ecca01ac5c 72 switch(cop)//viene assegnato nel comando dritto, in un raggionamento "euristico" si è compreso la direzione
deangelis1993 0:65ecca01ac5c 73 {
deangelis1993 0:65ecca01ac5c 74 case CURVA_RIGHT_CODE:
deangelis1993 0:65ecca01ac5c 75
deangelis1993 0:65ecca01ac5c 76 break;
deangelis1993 0:65ecca01ac5c 77 case CURVA_LEFT_CODE:
deangelis1993 0:65ecca01ac5c 78
deangelis1993 0:65ecca01ac5c 79 break;
deangelis1993 0:65ecca01ac5c 80 }
deangelis1993 0:65ecca01ac5c 81 curva = false;//uscito dallo switch l'operazione sarà completata!
deangelis1993 0:65ecca01ac5c 82
deangelis1993 0:65ecca01ac5c 83 /*Qui va messo il codice per farlo andare dritto ora se il codice rileva di nuovo la curva allora curva verrà risettato a true*/
deangelis1993 0:65ecca01ac5c 84 }
deangelis1993 0:65ecca01ac5c 85 }
deangelis1993 0:65ecca01ac5c 86 }
deangelis1993 0:65ecca01ac5c 87
deangelis1993 0:65ecca01ac5c 88 void DuckieTownCar::executeLine(unsigned char cop) const
deangelis1993 0:65ecca01ac5c 89 {
deangelis1993 0:65ecca01ac5c 90 switch(cop)
deangelis1993 0:65ecca01ac5c 91 {
deangelis1993 0:65ecca01ac5c 92 case DRITTO_CODE://va dritto finchè può o deve
deangelis1993 0:65ecca01ac5c 93 //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
deangelis1993 0:65ecca01ac5c 94 //nel caso della curva sarà fatto tutto in questo codice qui!
deangelis1993 0:65ecca01ac5c 95 switch(cop)//viene assegnato nel comando dritto, in un raggionamento "euristico" si è compreso la direzione
deangelis1993 0:65ecca01ac5c 96 {
deangelis1993 0:65ecca01ac5c 97 case CURVA_RIGHT_CODE:
deangelis1993 0:65ecca01ac5c 98 break;
deangelis1993 0:65ecca01ac5c 99 case CURVA_LEFT_CODE:
deangelis1993 0:65ecca01ac5c 100 break;
deangelis1993 0:65ecca01ac5c 101 }
deangelis1993 0:65ecca01ac5c 102 break;
deangelis1993 0:65ecca01ac5c 103 case RIGHT_CODE://gira a destra date delle giuste condizioni iniziali
deangelis1993 0:65ecca01ac5c 104 //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
deangelis1993 0:65ecca01ac5c 105 //utile per le rotatorie la ripetizione
deangelis1993 0:65ecca01ac5c 106 break;
deangelis1993 0:65ecca01ac5c 107 case LEFT_CODE://gira a sinistra nelle giuste condizioni
deangelis1993 0:65ecca01ac5c 108 //per dire quando è finito si fanno i controlli, op qui indica se deve essere eseguito più di una volta
deangelis1993 0:65ecca01ac5c 109 //utile per le rotatorie la ripetizione
deangelis1993 0:65ecca01ac5c 110 break;
deangelis1993 0:65ecca01ac5c 111 }
deangelis1993 0:65ecca01ac5c 112 }
deangelis1993 0:65ecca01ac5c 113
deangelis1993 0:65ecca01ac5c 114 void DuckieTownCar::executeRandom() const
deangelis1993 0:65ecca01ac5c 115 {
deangelis1993 0:65ecca01ac5c 116 unsigned char cop = rand()%N_COP +1;
deangelis1993 0:65ecca01ac5c 117 while(1) this->executeLine(cop);
deangelis1993 0:65ecca01ac5c 118 }
deangelis1993 0:65ecca01ac5c 119
deangelis1993 0:65ecca01ac5c 120 __forceinline void DuckieTownCar::updateStateCar(){
deangelis1993 0:65ecca01ac5c 121 this->infoAxes();//aggiornamento dell'angolo!
deangelis1993 0:65ecca01ac5c 122
deangelis1993 0:65ecca01ac5c 123 //lo metto in commento perchè va prima inizializzato!
deangelis1993 0:65ecca01ac5c 124 //sensor_color.getReading();
deangelis1993 0:65ecca01ac5c 125 }
deangelis1993 0:65ecca01ac5c 126
deangelis1993 0:65ecca01ac5c 127 /**************************************
deangelis1993 0:65ecca01ac5c 128 * Gruppo del giroscopio Accelerometro *
deangelis1993 0:65ecca01ac5c 129 ***************************************/
deangelis1993 0:65ecca01ac5c 130 void DuckieTownCar::infoAxes(){
deangelis1993 0:65ecca01ac5c 131 acc_gyro->Get_G_Axes(axes);
deangelis1993 0:65ecca01ac5c 132 #ifdef DEBUG_CAR
deangelis1993 0:65ecca01ac5c 133 printf("LSM6DSLrow [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
deangelis1993 0:65ecca01ac5c 134 #endif
deangelis1993 0:65ecca01ac5c 135 for(int i=0;i<3;i++)
deangelis1993 0:65ecca01ac5c 136 axes[i]-=off[i];
deangelis1993 0:65ecca01ac5c 137
deangelis1993 0:65ecca01ac5c 138 #ifdef DEBUG_CAR
deangelis1993 0:65ecca01ac5c 139 printf("LSM6DSLfine [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
deangelis1993 0:65ecca01ac5c 140 #endif
deangelis1993 0:65ecca01ac5c 141 wait_ms(1);
deangelis1993 0:65ecca01ac5c 142 // ricavo il parziale dalla velocità angolare
deangelis1993 0:65ecca01ac5c 143 for(int i=0;i<3;i++)
deangelis1993 0:65ecca01ac5c 144 {
deangelis1993 0:65ecca01ac5c 145 parziale_angolo[i]=(float)(axes[i]*sens)/1000.0;// passo da mdps a dpLSB
deangelis1993 0:65ecca01ac5c 146 angolo[i]=(angolo[i]-19.0)/2.84;// levo la correzione per poter sommare i dati parziali
deangelis1993 0:65ecca01ac5c 147 parziale_angolo[i]/= 1000.0;// moltiplico per il dt (1ms)
deangelis1993 0:65ecca01ac5c 148 if (axes[i]>150 || axes[i]<-150)
deangelis1993 0:65ecca01ac5c 149 angolo[i] += parziale_angolo[i];//integro
deangelis1993 0:65ecca01ac5c 150
deangelis1993 0:65ecca01ac5c 151 angolo[i]=(angolo[i]*2.84)+19;//correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta)
deangelis1993 0:65ecca01ac5c 152 }
deangelis1993 0:65ecca01ac5c 153 }