duckietownhsunina
probabili errori di lettura da parte del giroscopio
Dependencies: X_NUCLEO_IHM12A1 X_NUCLEO_IKS01A2
Fork of
rtos_basic
by 
mbed_example
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);
+ }
+}