main.cpp

00001 
00002 /* Includes */
00003 #include "mbed.h"
00004 #include "x_nucleo_iks01a2.h"
00005 #include "stspin240_250_class.h"
00006 
00007 /* Instantiate the expansion board */
00008 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D10, D11);
00009 Serial pc(SERIAL_TX, SERIAL_RX);
00010 
00011 #define  sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato 
00012 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00013 Thread uno;
00014 Timer t;
00015 
00016 /* Simple main function */
00017 
00018 /* Variables -----------------------------------------------------------------*/
00019 
00020 /* Initialization parameters of the motor connected to the expansion board. */
00021 Stspin240_250_Init_t initDeviceParameters = {
00022     100000, /* Frequency of PWM of Input Bridge A in Hz up to 100000Hz             */
00023     100000, /* Frequency of PWM of Input Bridge B in Hz up to 100000Hz             */
00024     100000, /* Frequency of PWM used for Ref pin in Hz up to 100000Hz              */
00025     100,    /* Duty cycle of PWM used for Ref pin (from 0 to 100)                  */
00026     TRUE   /* Dual Bridge configuration  (FALSE for mono, TRUE for dual brush dc) */
00027 };
00028 
00029 /* Motor Control Component. */
00030 STSPIN240_250 *motor;
00031 
00032 
00033 int32_t mdps[3];
00034 uint8_t id;
00035 int32_t acc[3];
00036 int32_t off[3];
00037 float parziale[3];
00038 volatile float angolo[3];
00039 
00040 
00041 void angle()
00042 {
00043     float dt=0;
00044     parziale[2]=0;
00045     angolo [2]=0;
00046     
00047     acc_gyro->Enable_G();                                           //abilito giroscopio
00048     pc.printf("\r\n--- Starting new run ---\r\n");
00049     acc_gyro->ReadID(&id);
00050     pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00051     wait(1.5);                                                       // attendo l' inzializzazione
00052     acc_gyro->Get_G_Axes(mdps);                                       // eseguo una prima acquisizione di velocità angolari
00053     pc.printf("LSM6DSL [gyro/mdps]:    %6ld\r\n",  mdps[2]);
00054     off[2]=mdps[2];                                                   // off[] mi serve per salvare la posizione iniziale
00055     pc.printf("off [gyro/mdps]:  %6ld\r\n",  off[2]);
00056     while(1) {
00057 
00058          t.stop();
00059          dt=t.read();
00060 
00061         acc_gyro->Get_G_Axes(mdps);  //Acquisisco una nuova misura e la confronto con la posizione iniziale
00062         pc.printf("tempo   %6ld\r\n", t.read_ms());
00063         t.reset();
00064         t.start();
00065          
00066         pc.printf("LSM6DSLrow [gyro/mdps]:    %6ld\r\n", mdps[2]);
00067 
00068         // sottraggo l'offset
00069         mdps[2]=mdps[2]-off[2];
00070         pc.printf("LSM6DSLfine [gyro/mdps]:   %6ld\r\n",mdps[2]);
00071 
00072         // ricavo il parziale dalla velocità angolare
00073 
00074         // passo da mdps a dpLSB
00075         parziale[2]=(mdps[2]*sens)/1000;                                                //parziale è una velocità angolare, sens=quanto
00076 
00077         // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
00078         angolo[2]=(angolo[2]-(-7.04451))/0.0163359;
00079         parziale[2]*=(dt);                           //Moltiplico il parziale per il dt così da ottenere una poszione angolare
00080 
00081         if (mdps[2]>70 ||mdps[2]<-70)// ci si puo mettere anche 70 come soglia non cambia molto
00082             angolo[2] += parziale[2];     // integro
00083 
00084        // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line  )
00085         angolo[2]=(angolo[2]*0.0163359)+(-7.04451);
00086         pc.printf(" angolo:  %6f\r\n",angolo[2]);
00087 
00088     }
00089 }
00090 
00091 int main()
00092 {
00093     uno.start(angle);
00094 
00095  //MOTORI
00096 
00097   //  uint8_t demoStep = 0;
00098     /* Initializing Motor Control Component. */
00099     motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0 );           //Chiamo i l costruttore per inizializzare l'oggetto motor
00100 
00101     /* Set dual bridge enabled as two motors are used*/
00102     motor->SetDualFullBridgeConfig(1);
00103 
00104     /* Set PWM Frequency of Ref to 15000 Hz */
00105     motor->SetRefPwmFreq(0, 15000 );       //frequenza clock
00106 
00107     /* Set PWM duty cycle of Ref to 100% */
00108     motor->SetRefPwmDc(0, 100);
00109 
00110     /* Set PWM Frequency of bridge A inputs to 10000 Hz */
00111     motor->SetBridgeInputPwmFreq(0,20000);
00112 
00113     /* Set PWM Frequency of bridge B inputs to 10000 Hz */
00114     motor->SetBridgeInputPwmFreq(1,20000);
00115 
00116 
00117 
00118     //INIZIALIZZAZIONI, s1 e s2 conterranno i valori di velocità da dare ai singoli motori
00119  
00120     int s0=15; //ruota sx
00121     int s1=15;  //ruota dx
00122 
00123    
00124    
00125    /**** SETTO I DUE MOTORI ALLA STESSA VELOCITA, 50%, E ALLA STESSA DIREZIONE ****/
00126     motor->SetSpeed(0,s0);                          //SETTO LA VELOCITà DEL MOTORE 0 AL 50%
00127     motor->SetSpeed(1,s1);                          //RIDUCO LA VELOCITA DEL MOTORE 0
00128     motor->Run(1, BDCMotor::FWD);                   //FACCIO ANDARE AVANTI IL MOTORE 1, per farlo andare indietro basta mettere BWD invece di FWD
00129     motor->Run(0, BDCMotor::FWD);                   //MOTORE 0 IN AVANTI
00130 
00131     
00132      
00133     while(1) {
00134 
00135        // pc.printf("\r\n");
00136  
00137         //L'angolo a sinistra è positivo ed s1 è la ruota di destra
00138         if(angolo[2] >  3) 
00139         {   
00140             
00141             s1=10;                 
00142             s0=20;           
00143        //     pc.printf("  velocita ruota destra   %d\r\n", s1);
00144        //     pc.printf("  velocita ruota sinistra :  %d\r\n", s0);
00145         } 
00146            
00147       if(angolo[2] < - 3)
00148       {
00149                         s1=20;
00150                         s0=10;
00151          //               pc.printf("  velocita ruota destra   %d\r\n", s1);
00152          //               pc.printf("  velocita ruota sinistra :  %d\r\n", s0);
00153       }
00154       
00155         motor->SetSpeed(1,s1);
00156         motor->SetSpeed(0,s0);     
00157   }
00158 }