Revision 11:a7f3f554886c, committed 2017-03-04

Comitter:

Giuliove

Date:

Sat Mar 04 22:56:07 2017 +0000

Parent:

10:dc33cd3f4eb9

Commit message:

motore + giroscopio, prima soluzione

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X-NUCLEO-IHM12A1.lib	Sat Mar 04 22:56:07 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM12A1/#232e0c730f59

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/X_NUCLEO_IKS01A2.lib	Sat Mar 04 22:56:07 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/X_NUCLEO_IKS01A2/#63b2b4c21092

--- a/main.cpp	Thu Jan 12 23:35:40 2017 +0000
+++ b/main.cpp	Sat Mar 04 22:56:07 2017 +0000
@@ -1,21 +1,158 @@
-#include "mbed.h"
- 
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-Thread thread;
- 
-void led2_thread() {
-    while (true) {
-        led2 = !led2;
-        wait(1);
-    }
-}
- 
-int main() {
-    thread.start(led2_thread);
-    
-    while (true) {
-        led1 = !led1;
-        wait(0.5);
-    }
-}
+
+/* Includes */
+#include "mbed.h"
+#include "x_nucleo_iks01a2.h"
+#include "stspin240_250_class.h"
+
+/* Instantiate the expansion board */
+static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D10, D11);
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+#define  sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato 
+static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
+Thread uno;
+Timer t;
+
+/* Simple main function */
+
+/* Variables -----------------------------------------------------------------*/
+
+/* Initialization parameters of the motor connected to the expansion board. */
+Stspin240_250_Init_t initDeviceParameters = {
+    100000, /* Frequency of PWM of Input Bridge A in Hz up to 100000Hz             */
+    100000, /* Frequency of PWM of Input Bridge B in Hz up to 100000Hz             */
+    100000, /* Frequency of PWM used for Ref pin in Hz up to 100000Hz              */
+    100,    /* Duty cycle of PWM used for Ref pin (from 0 to 100)                  */
+    TRUE   /* Dual Bridge configuration  (FALSE for mono, TRUE for dual brush dc) */
+};
+
+/* Motor Control Component. */
+STSPIN240_250 *motor;
+
+
+int32_t mdps[3];
+uint8_t id;
+int32_t acc[3];
+int32_t off[3];
+float parziale[3];
+volatile float angolo[3];
+
+
+void angle()
+{
+    float dt=0;
+    parziale[2]=0;
+    angolo [2]=0;
+    
+    acc_gyro->Enable_G();                                           //abilito giroscopio
+    pc.printf("\r\n--- Starting new run ---\r\n");
+    acc_gyro->ReadID(&id);
+    pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
+    wait(1.5);                                                       // attendo l' inzializzazione
+    acc_gyro->Get_G_Axes(mdps);                                       // eseguo una prima acquisizione di velocità angolari
+    pc.printf("LSM6DSL [gyro/mdps]:    %6ld\r\n",  mdps[2]);
+    off[2]=mdps[2];                                                   // off[] mi serve per salvare la posizione iniziale
+    pc.printf("off [gyro/mdps]:  %6ld\r\n",  off[2]);
+    while(1) {
+
+         t.stop();
+         dt=t.read();
+
+        acc_gyro->Get_G_Axes(mdps);  //Acquisisco una nuova misura e la confronto con la posizione iniziale
+        pc.printf("tempo   %6ld\r\n", t.read_ms());
+        t.reset();
+        t.start();
+         
+        pc.printf("LSM6DSLrow [gyro/mdps]:    %6ld\r\n", mdps[2]);
+
+        // sottraggo l'offset
+        mdps[2]=mdps[2]-off[2];
+        pc.printf("LSM6DSLfine [gyro/mdps]:   %6ld\r\n",mdps[2]);
+
+        // ricavo il parziale dalla velocità angolare
+
+        // passo da mdps a dpLSB
+        parziale[2]=(mdps[2]*sens)/1000;                                                //parziale è una velocità angolare, sens=quanto
+
+        // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
+        angolo[2]=(angolo[2]-(-7.04451))/0.0163359;
+        parziale[2]*=(dt);                           //Moltiplico il parziale per il dt così da ottenere una poszione angolare
+
+        if (mdps[2]>70 ||mdps[2]<-70)// ci si puo mettere anche 70 come soglia non cambia molto
+            angolo[2] += parziale[2];     // integro
+
+       // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line  )
+        angolo[2]=(angolo[2]*0.0163359)+(-7.04451);
+        pc.printf(" angolo:  %6f\r\n",angolo[2]);
+
+    }
+}
+
+int main()
+{
+    uno.start(angle);
+
+ //MOTORI
+
+  //  uint8_t demoStep = 0;
+    /* Initializing Motor Control Component. */
+    motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0 );           //Chiamo i l costruttore per inizializzare l'oggetto motor
+
+    /* Set dual bridge enabled as two motors are used*/
+    motor->SetDualFullBridgeConfig(1);
+
+    /* Set PWM Frequency of Ref to 15000 Hz */
+    motor->SetRefPwmFreq(0, 15000 );       //frequenza clock
+
+    /* Set PWM duty cycle of Ref to 100% */
+    motor->SetRefPwmDc(0, 100);
+
+    /* Set PWM Frequency of bridge A inputs to 10000 Hz */
+    motor->SetBridgeInputPwmFreq(0,20000);
+
+    /* Set PWM Frequency of bridge B inputs to 10000 Hz */
+    motor->SetBridgeInputPwmFreq(1,20000);
+
+
+
+    //INIZIALIZZAZIONI, s1 e s2 conterranno i valori di velocità da dare ai singoli motori
+ 
+    int s0=15; //ruota sx
+    int s1=15;  //ruota dx
+
+   
+   
+   /**** SETTO I DUE MOTORI ALLA STESSA VELOCITA, 50%, E ALLA STESSA DIREZIONE ****/
+    motor->SetSpeed(0,s0);                          //SETTO LA VELOCITà DEL MOTORE 0 AL 50%
+    motor->SetSpeed(1,s1);                          //RIDUCO LA VELOCITA DEL MOTORE 0
+    motor->Run(1, BDCMotor::FWD);                   //FACCIO ANDARE AVANTI IL MOTORE 1, per farlo andare indietro basta mettere BWD invece di FWD
+    motor->Run(0, BDCMotor::FWD);                   //MOTORE 0 IN AVANTI
+
+    
+     
+    while(1) {
+
+       // pc.printf("\r\n");
+ 
+        //L'angolo a sinistra è positivo ed s1 è la ruota di destra
+        if(angolo[2] >  3) 
+        {   
+            
+            s1=10;                 
+            s0=20;           
+       //     pc.printf("  velocita ruota destra   %d\r\n", s1);
+       //     pc.printf("  velocita ruota sinistra :  %d\r\n", s0);
+        } 
+           
+      if(angolo[2] < - 3)
+      {
+                        s1=20;
+                        s0=10;
+         //               pc.printf("  velocita ruota destra   %d\r\n", s1);
+         //               pc.printf("  velocita ruota sinistra :  %d\r\n", s0);
+      }
+      
+        motor->SetSpeed(1,s1);
+        motor->SetSpeed(0,s0);     
+  }
+}