Revision 0:65ecca01ac5c, committed 2017-02-15

Comitter:

deangelis1993

Date:

Wed Feb 15 13:27:01 2017 +0000

Commit message:

DuckiTownCar first version for virtualization of machine

Changed in this revision

diff -r 000000000000 -r 65ecca01ac5c TCS3200.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TCS3200.lib	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/deangelis1993/code/TCS3200/#977d78c9d58a

diff -r 000000000000 -r 65ecca01ac5c VM_Car/ByteCode.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VM_Car/ByteCode.h	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,28 @@
+#ifndef _ByteCode_h
+#define _ByteCode_h
+
+struct Code{
+    unsigned int cop : 4;//cop-> codice operativo, op-> operando di 4 bit ciascuno    
+    unsigned int op : 4;
+};
+
+struct ByteCode{
+    Code* code;
+    unsigned int dim_code; 
+};
+
+#define N_COP           3/*Per N_COP si intende solo il numero di cop che si possono usare, quindi non si contano quelli straordinari(NOPE e Random)*/
+
+/********************************
+* Definizione codici operativi  *
+*********************************/
+#define NOPE_CODE       0x0
+
+#define RANDOM_CODE         0xF
+#define DRITTO_CODE         0x1
+#define RIGHT_CODE          0x2
+#define LEFT_CODE           0x3
+#define CURVA_RIGHT_CODE    0x4
+#define CURVA_LEFT_CODE     0x5
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 65ecca01ac5c VM_Car/DuckieTownCar.cpp
--- /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

diff -r 000000000000 -r 65ecca01ac5c VM_Car/DuckieTownCar.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VM_Car/DuckieTownCar.h	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,55 @@
+#ifndef _DUCKIETOWNCAR_H
+#define _DUCKIETOWNCAR_H
+
+#include "ByteCode.h"
+//#include "color.h"
+#include "x_nucleo_iks01a2.h"
+
+/***********************************************
+*  DuckieTownCarHS     macchina Virtuale!      *
+*  by De Angelis Domenico Francesco            *
+*-------->      UNINA FabLAB      <------------*
+************************************************/
+
+//Se con le classi siamo nel torno, è meglio sbagliare alla grande! :D
+
+#define DEBUG_CAR
+
+//Singleton
+class DuckieTownCar
+{
+private:
+    static DuckieTownCar* instance;//istanza della macchina
+    ByteCode code;
+
+    LSM6DSLSensor *acc_gyro;//giroscopio
+    int32_t axes[3],off[3];
+    float parziale_angolo[3],angolo[3];//angolo indica di quanto sia ruotato rispetto ad offset iniziale in [gyro/d]
+    //sceda per i motori
+    
+    //scheda dei Colori TCS3200
+    //ColorSensor sensor_color;
+    
+    DuckieTownCar();
+    
+    void infoAxes();//aggiorna i valori di angolo!
+    
+    void dritto();
+    void gira();
+public:
+    ~DuckieTownCar(){error("Ora mi devi spiegare come ci sei arrivato qui...\n");}
+    static DuckieTownCar* getInstance();
+    
+    void initCarSensor();
+    __forceinline void updateStateCar();//aggiorno lo stato della macchina attraverso i sensori!
+    __forceinline void readMission(const ByteCode& codice){this->code.code = codice.code; this->code.dim_code = codice.dim_code;}
+    void execute() const;
+    void executeLine(unsigned char code) const;
+    void executeRandom() const;
+
+    //nel caso vogliamo permettere di configurare l'auto
+    //void setVel(float);
+    //float getVel() const;
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 65ecca01ac5c X_NUCLEO_IKS01A2.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X_NUCLEO_IKS01A2.lib	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X_NUCLEO_IKS01A2/#63b2b4c21092

diff -r 000000000000 -r 65ecca01ac5c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,28 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "DuckieTownCar.h"
+
+/*
+* Utilizzo il RTOS-MBED! perchè?
+* Perchè ho la necessità di avere le info dai sensori senza essere bloccante
+*/
+
+DuckieTownCar* car = DuckieTownCar::getInstance();
+
+Mutex car_mutex; 
+ 
+void update_car(void const *args){
+    car_mutex.lock();
+    car->updateStateCar();
+    car_mutex.unlock();
+}
+
+int main(){
+    Thread th_update(update_car);
+    car_mutex.lock();
+    car->initCarSensor();
+    car_mutex.unlock();
+    
+    car->executeRandom();
+    return 0;    
+}
\ No newline at end of file

diff -r 000000000000 -r 65ecca01ac5c mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/mbed_official/code/mbed-rtos/#58563e6cba1e

diff -r 000000000000 -r 65ecca01ac5c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Feb 15 13:27:01 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/176b8275d35d
\ No newline at end of file