Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Les essais montrent une derive max de 1°/90s avec 10 secondes de calibration

Browse Source
This commit is contained in:
Samuel 2022-10-29 15:51:52 +02:00
parent ae2034e66c
commit 9f51807d6f
2 changed files with 23 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gyro.c
View File

@ -78,9 +78,9 @@ void Gyro_Init(void){
} }
} }
sleep_ms(150); // Temps d'init du gyroscope sleep_ms(150); // Temps d'init du gyroscope
/*while(1){
gyro_calibration(); gyro_calibration();
}*/
} }
void Gyro_Read(uint16_t step_ms){ void Gyro_Read(uint16_t step_ms){
@ -123,8 +123,8 @@ void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){
} }
void gyro_calibration(void){ void gyro_calibration(void){
uint32_t t_calibration_ms = 40000; uint32_t t_calibration_ms = 10000;
uint32_t nb_ech = t_calibration_ms/5; uint32_t nb_ech = t_calibration_ms/2;
uint32_t m_temps_ms = Temps_get_temps_ms(); uint32_t m_temps_ms = Temps_get_temps_ms();
uint32_t temps_500ms = m_temps_ms; uint32_t temps_500ms = m_temps_ms;
int16_t temperature; int16_t temperature;
@ -145,19 +145,20 @@ void gyro_calibration(void){
vitesse_calibration->rot_x += vitesse_grute_gyro.rot_x; vitesse_calibration->rot_x += vitesse_grute_gyro.rot_x;
vitesse_calibration->rot_y += vitesse_grute_gyro.rot_y; vitesse_calibration->rot_y += vitesse_grute_gyro.rot_y;
vitesse_calibration->rot_z += vitesse_grute_gyro.rot_z; vitesse_calibration->rot_z += vitesse_grute_gyro.rot_z;
if(m_temps_ms > temps_500ms){ /*if(m_temps_ms > temps_500ms){
printf("."); printf(".");
gyro_get_temp(); gyro_get_temp();
temps_500ms += 500; temps_500ms += 500;
} }*/
sleep_ms(5); sleep_ms(2);
} }
vitesse_calibration->rot_x = vitesse_calibration->rot_x / (int32_t)nb_ech; vitesse_calibration->rot_x = vitesse_calibration->rot_x / (int32_t)nb_ech;
vitesse_calibration->rot_y = vitesse_calibration->rot_y / (int32_t)nb_ech; vitesse_calibration->rot_y = vitesse_calibration->rot_y / (int32_t)nb_ech;
vitesse_calibration->rot_z = vitesse_calibration->rot_z / (int32_t)nb_ech; vitesse_calibration->rot_z = vitesse_calibration->rot_z / (int32_t)nb_ech;
temperature = gyro_get_temp(); //temperature = gyro_get_temp();
printf("fin calibration, %d, %d, %d, %d\n", vitesse_calibration->rot_x, vitesse_calibration->rot_y ,vitesse_calibration->rot_z, temperature); //printf("fin calibration, %d, %d, %d, %d\n", vitesse_calibration->rot_x, vitesse_calibration->rot_y ,vitesse_calibration->rot_z, temperature);
printf("fin calibration, %d\n", vitesse_calibration->rot_z);
} }

14
test.c
View File

@ -46,7 +46,11 @@ int main() {
// Tous les pas de step_ms // Tous les pas de step_ms
if(!(temps_ms % step_ms)){ if(!(temps_ms % step_ms)){
uint64_t _time;
uint32_t time_after_read, time_after_filter, time_after_printf;
_time = time_us_64();
Gyro_Read(step_ms); Gyro_Read(step_ms);
time_after_read = time_us_64() - _time;
//gyro_affiche(gyro_get_vitesse(), "Angle :"); //gyro_affiche(gyro_get_vitesse(), "Angle :");
// Filtre // Filtre
@ -60,15 +64,21 @@ int main() {
vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre; vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre;
vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre; vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre;
} }
time_after_filter = time_us_64() - _time;
printf("%f, %f\n", (double)temps_ms_old / 1000, vitesse_filtre_z); //printf("%#x, %#x\n", (double)temps_ms_old / 1000, vitesse_filtre_z);
//printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000));
time_after_printf = time_us_64() - _time;
//gyro_affiche(angle_gyro, "Vitesse (°/s),"); //gyro_affiche(angle_gyro, "Vitesse (°/s),");
} }
// Toutes les 50 ms // Toutes les 50 ms
if((Temps_get_temps_ms() % 50) == 0){ if((Temps_get_temps_ms() % 50) == 0){
struct t_angle_gyro_double m_gyro;
m_gyro = gyro_get_angle();
printf("%f, %f\n", (double)temps_ms / 1000, m_gyro.rot_z);
} }
// Toutes les 500 ms // Toutes les 500 ms