probabili errori di lettura da parte del giroscopio
Dependencies: X_NUCLEO_IHM12A1 X_NUCLEO_IKS01A2
Fork of rtos_basic by
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?
User | Revision | Line number | New 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 | } |