duckietownhsunina / Mbed OS giro_motore_2

probabili errori di lettura da parte del giroscopio

Dependencies:   X_NUCLEO_IHM12A1 X_NUCLEO_IKS01A2

Fork of rtos_basic by mbed_example

main.cpp@11:a7f3f554886c, 2017-03-04 (annotated)

Committer:
Giuliove
Date:
Sat Mar 04 22:56:07 2017 +0000
Revision:
11:a7f3f554886c
Parent:
10:dc33cd3f4eb9 
motore + giroscopio, prima soluzione

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Giuliove 11:a7f3f554886c 1
Giuliove 11:a7f3f554886c 2 /* Includes */
Giuliove 11:a7f3f554886c 3 #include "mbed.h"
Giuliove 11:a7f3f554886c 4 #include "x_nucleo_iks01a2.h"
Giuliove 11:a7f3f554886c 5 #include "stspin240_250_class.h"
Giuliove 11:a7f3f554886c 6
Giuliove 11:a7f3f554886c 7 /* Instantiate the expansion board */
Giuliove 11:a7f3f554886c 8 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D10, D11);
Giuliove 11:a7f3f554886c 9 Serial pc(SERIAL_TX, SERIAL_RX);
Giuliove 11:a7f3f554886c 10
Giuliove 11:a7f3f554886c 11 #define sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato
Giuliove 11:a7f3f554886c 12 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
Giuliove 11:a7f3f554886c 13 Thread uno;
Giuliove 11:a7f3f554886c 14 Timer t;
Giuliove 11:a7f3f554886c 15
Giuliove 11:a7f3f554886c 16 /* Simple main function */
Giuliove 11:a7f3f554886c 17
Giuliove 11:a7f3f554886c 18 /* Variables -----------------------------------------------------------------*/
Giuliove 11:a7f3f554886c 19
Giuliove 11:a7f3f554886c 20 /* Initialization parameters of the motor connected to the expansion board. */
Giuliove 11:a7f3f554886c 21 Stspin240_250_Init_t initDeviceParameters = {
Giuliove 11:a7f3f554886c 22 100000, /* Frequency of PWM of Input Bridge A in Hz up to 100000Hz */
Giuliove 11:a7f3f554886c 23 100000, /* Frequency of PWM of Input Bridge B in Hz up to 100000Hz */
Giuliove 11:a7f3f554886c 24 100000, /* Frequency of PWM used for Ref pin in Hz up to 100000Hz */
Giuliove 11:a7f3f554886c 25 100, /* Duty cycle of PWM used for Ref pin (from 0 to 100) */
Giuliove 11:a7f3f554886c 26 TRUE /* Dual Bridge configuration (FALSE for mono, TRUE for dual brush dc) */
Giuliove 11:a7f3f554886c 27 };
Giuliove 11:a7f3f554886c 28
Giuliove 11:a7f3f554886c 29 /* Motor Control Component. */
Giuliove 11:a7f3f554886c 30 STSPIN240_250 *motor;
Giuliove 11:a7f3f554886c 31
Giuliove 11:a7f3f554886c 32
Giuliove 11:a7f3f554886c 33 int32_t mdps[3];
Giuliove 11:a7f3f554886c 34 uint8_t id;
Giuliove 11:a7f3f554886c 35 int32_t acc[3];
Giuliove 11:a7f3f554886c 36 int32_t off[3];
Giuliove 11:a7f3f554886c 37 float parziale[3];
Giuliove 11:a7f3f554886c 38 volatile float angolo[3];
Giuliove 11:a7f3f554886c 39
Giuliove 11:a7f3f554886c 40
Giuliove 11:a7f3f554886c 41 void angle()
Giuliove 11:a7f3f554886c 42 {
Giuliove 11:a7f3f554886c 43 float dt=0;
Giuliove 11:a7f3f554886c 44 parziale[2]=0;
Giuliove 11:a7f3f554886c 45 angolo [2]=0;
Giuliove 11:a7f3f554886c 46
Giuliove 11:a7f3f554886c 47 acc_gyro->Enable_G(); //abilito giroscopio
Giuliove 11:a7f3f554886c 48 pc.printf("\r\n--- Starting new run ---\r\n");
Giuliove 11:a7f3f554886c 49 acc_gyro->ReadID(&id);
Giuliove 11:a7f3f554886c 50 pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
Giuliove 11:a7f3f554886c 51 wait(1.5); // attendo l' inzializzazione
Giuliove 11:a7f3f554886c 52 acc_gyro->Get_G_Axes(mdps); // eseguo una prima acquisizione di velocità angolari
Giuliove 11:a7f3f554886c 53 pc.printf("LSM6DSL [gyro/mdps]: %6ld\r\n", mdps[2]);
Giuliove 11:a7f3f554886c 54 off[2]=mdps[2]; // off[] mi serve per salvare la posizione iniziale
Giuliove 11:a7f3f554886c 55 pc.printf("off [gyro/mdps]: %6ld\r\n", off[2]);
Giuliove 11:a7f3f554886c 56 while(1) {
Giuliove 11:a7f3f554886c 57
Giuliove 11:a7f3f554886c 58 t.stop();
Giuliove 11:a7f3f554886c 59 dt=t.read();
Giuliove 11:a7f3f554886c 60
Giuliove 11:a7f3f554886c 61 acc_gyro->Get_G_Axes(mdps); //Acquisisco una nuova misura e la confronto con la posizione iniziale
Giuliove 11:a7f3f554886c 62 pc.printf("tempo %6ld\r\n", t.read_ms());
Giuliove 11:a7f3f554886c 63 t.reset();
Giuliove 11:a7f3f554886c 64 t.start();
Giuliove 11:a7f3f554886c 65
Giuliove 11:a7f3f554886c 66 pc.printf("LSM6DSLrow [gyro/mdps]: %6ld\r\n", mdps[2]);
Giuliove 11:a7f3f554886c 67
Giuliove 11:a7f3f554886c 68 // sottraggo l'offset
Giuliove 11:a7f3f554886c 69 mdps[2]=mdps[2]-off[2];
Giuliove 11:a7f3f554886c 70 pc.printf("LSM6DSLfine [gyro/mdps]: %6ld\r\n",mdps[2]);
Giuliove 11:a7f3f554886c 71
Giuliove 11:a7f3f554886c 72 // ricavo il parziale dalla velocità angolare
Giuliove 11:a7f3f554886c 73
Giuliove 11:a7f3f554886c 74 // passo da mdps a dpLSB
Giuliove 11:a7f3f554886c 75 parziale[2]=(mdps[2]*sens)/1000; //parziale è una velocità angolare, sens=quanto
Giuliove 11:a7f3f554886c 76
Giuliove 11:a7f3f554886c 77 // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
Giuliove 11:a7f3f554886c 78 angolo[2]=(angolo[2]-(-7.04451))/0.0163359;
Giuliove 11:a7f3f554886c 79 parziale[2]*=(dt); //Moltiplico il parziale per il dt così da ottenere una poszione angolare
Giuliove 11:a7f3f554886c 80
Giuliove 11:a7f3f554886c 81 if (mdps[2]>70 ||mdps[2]<-70)// ci si puo mettere anche 70 come soglia non cambia molto
Giuliove 11:a7f3f554886c 82 angolo[2] += parziale[2]; // integro
Giuliove 11:a7f3f554886c 83
Giuliove 11:a7f3f554886c 84 // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line )
Giuliove 11:a7f3f554886c 85 angolo[2]=(angolo[2]*0.0163359)+(-7.04451);
Giuliove 11:a7f3f554886c 86 pc.printf(" angolo: %6f\r\n",angolo[2]);
Giuliove 11:a7f3f554886c 87
Giuliove 11:a7f3f554886c 88 }
Giuliove 11:a7f3f554886c 89 }
Giuliove 11:a7f3f554886c 90
Giuliove 11:a7f3f554886c 91 int main()
Giuliove 11:a7f3f554886c 92 {
Giuliove 11:a7f3f554886c 93 uno.start(angle);
Giuliove 11:a7f3f554886c 94
Giuliove 11:a7f3f554886c 95 //MOTORI
Giuliove 11:a7f3f554886c 96
Giuliove 11:a7f3f554886c 97 // uint8_t demoStep = 0;
Giuliove 11:a7f3f554886c 98 /* Initializing Motor Control Component. */
Giuliove 11:a7f3f554886c 99 motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0 ); //Chiamo i l costruttore per inizializzare l'oggetto motor
Giuliove 11:a7f3f554886c 100
Giuliove 11:a7f3f554886c 101 /* Set dual bridge enabled as two motors are used*/
Giuliove 11:a7f3f554886c 102 motor->SetDualFullBridgeConfig(1);
Giuliove 11:a7f3f554886c 103
Giuliove 11:a7f3f554886c 104 /* Set PWM Frequency of Ref to 15000 Hz */
Giuliove 11:a7f3f554886c 105 motor->SetRefPwmFreq(0, 15000 ); //frequenza clock
Giuliove 11:a7f3f554886c 106
Giuliove 11:a7f3f554886c 107 /* Set PWM duty cycle of Ref to 100% */
Giuliove 11:a7f3f554886c 108 motor->SetRefPwmDc(0, 100);
Giuliove 11:a7f3f554886c 109
Giuliove 11:a7f3f554886c 110 /* Set PWM Frequency of bridge A inputs to 10000 Hz */
Giuliove 11:a7f3f554886c 111 motor->SetBridgeInputPwmFreq(0,20000);
Giuliove 11:a7f3f554886c 112
Giuliove 11:a7f3f554886c 113 /* Set PWM Frequency of bridge B inputs to 10000 Hz */
Giuliove 11:a7f3f554886c 114 motor->SetBridgeInputPwmFreq(1,20000);
Giuliove 11:a7f3f554886c 115
Giuliove 11:a7f3f554886c 116
Giuliove 11:a7f3f554886c 117
Giuliove 11:a7f3f554886c 118 //INIZIALIZZAZIONI, s1 e s2 conterranno i valori di velocità da dare ai singoli motori
Giuliove 11:a7f3f554886c 119
Giuliove 11:a7f3f554886c 120 int s0=15; //ruota sx
Giuliove 11:a7f3f554886c 121 int s1=15; //ruota dx
Giuliove 11:a7f3f554886c 122
Giuliove 11:a7f3f554886c 123
Giuliove 11:a7f3f554886c 124
Giuliove 11:a7f3f554886c 125 /**** SETTO I DUE MOTORI ALLA STESSA VELOCITA, 50%, E ALLA STESSA DIREZIONE ****/
Giuliove 11:a7f3f554886c 126 motor->SetSpeed(0,s0); //SETTO LA VELOCITà DEL MOTORE 0 AL 50%
Giuliove 11:a7f3f554886c 127 motor->SetSpeed(1,s1); //RIDUCO LA VELOCITA DEL MOTORE 0
Giuliove 11:a7f3f554886c 128 motor->Run(1, BDCMotor::FWD); //FACCIO ANDARE AVANTI IL MOTORE 1, per farlo andare indietro basta mettere BWD invece di FWD
Giuliove 11:a7f3f554886c 129 motor->Run(0, BDCMotor::FWD); //MOTORE 0 IN AVANTI
Giuliove 11:a7f3f554886c 130
Giuliove 11:a7f3f554886c 131
Giuliove 11:a7f3f554886c 132
Giuliove 11:a7f3f554886c 133 while(1) {
Giuliove 11:a7f3f554886c 134
Giuliove 11:a7f3f554886c 135 // pc.printf("\r\n");
Giuliove 11:a7f3f554886c 136
Giuliove 11:a7f3f554886c 137 //L'angolo a sinistra è positivo ed s1 è la ruota di destra
Giuliove 11:a7f3f554886c 138 if(angolo[2] > 3)
Giuliove 11:a7f3f554886c 139 {
Giuliove 11:a7f3f554886c 140
Giuliove 11:a7f3f554886c 141 s1=10;
Giuliove 11:a7f3f554886c 142 s0=20;
Giuliove 11:a7f3f554886c 143 // pc.printf(" velocita ruota destra %d\r\n", s1);
Giuliove 11:a7f3f554886c 144 // pc.printf(" velocita ruota sinistra : %d\r\n", s0);
Giuliove 11:a7f3f554886c 145 }
Giuliove 11:a7f3f554886c 146
Giuliove 11:a7f3f554886c 147 if(angolo[2] < - 3)
Giuliove 11:a7f3f554886c 148 {
Giuliove 11:a7f3f554886c 149 s1=20;
Giuliove 11:a7f3f554886c 150 s0=10;
Giuliove 11:a7f3f554886c 151 // pc.printf(" velocita ruota destra %d\r\n", s1);
Giuliove 11:a7f3f554886c 152 // pc.printf(" velocita ruota sinistra : %d\r\n", s0);
Giuliove 11:a7f3f554886c 153 }
Giuliove 11:a7f3f554886c 154
Giuliove 11:a7f3f554886c 155 motor->SetSpeed(1,s1);
Giuliove 11:a7f3f554886c 156 motor->SetSpeed(0,s0);
Giuliove 11:a7f3f554886c 157 }
Giuliove 11:a7f3f554886c 158 }