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

Tentative cerise latérales + corrections erreurs Gyro

Browse Source
This commit is contained in:
Samuel 2023-05-18 14:51:16 +02:00
parent c2d12305d9
commit f7bebda9d2
4 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Holonome2023.c
View File

@ -90,12 +90,14 @@ int main() {
Trajet_init(); Trajet_init();
Balise_VL53L1X_init(); Balise_VL53L1X_init();
multicore_launch_core1(Monitoring_display);
set_position_avec_gyroscope(1); set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){ if(get_position_avec_gyroscope()){
Gyro_Init(); Gyro_Init();
} }
multicore_launch_core1(Monitoring_display);
temps_ms = Temps_get_temps_ms(); temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms; temps_ms_old = temps_ms;

9
Monitoring.c
View File

@ -2,6 +2,7 @@
#include <stdio.h> #include <stdio.h>
#include "Monitoring.h" #include "Monitoring.h"
#include "Localisation.h" #include "Localisation.h"
#include "Trajet.h"
#include "Asser_Moteurs.h" #include "Asser_Moteurs.h"
#include "i2c_annexe.h" #include "i2c_annexe.h"
@ -42,17 +43,21 @@ void Monitoring_display(){
temps_cycle_display(); temps_cycle_display();
printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.)); printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.));
printf(">DebugVar:%ld:%d\n", temps, debug_var); printf(">DebugVar:%ld:%d\n", temps, debug_var);
printf(">DebugVarf:%ld:%f\n", temps, debug_varf); printf(">Distance_Obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
printf(">DebugVarf(abscisse):%ld:%f\n", temps, debug_varf);
struct position_t position = Localisation_get(); struct position_t position = Localisation_get();
printf(">pos_x:%ld:%f\n", temps, position.x_mm); printf(">pos_x:%ld:%f\n", temps, position.x_mm);
printf(">P_con_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
printf(">pos_y:%ld:%f\n", temps, position.y_mm); printf(">pos_y:%ld:%f\n", temps, position.y_mm);
printf(">P_con_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
printf(">pos_angle:%ld:%f\n", temps, position.angle_radian);
printf(">P_con_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian);
printf(">V_a:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1)); printf(">V_a:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1));
printf(">V_b:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1)); printf(">V_b:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1));
printf(">V_c:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_C, 1)); printf(">V_c:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_C, 1));
printf(">V_con_a:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A)); printf(">V_con_a:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A));
printf(">V_con_b:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B)); printf(">V_con_b:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B));
printf(">V_con_c:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); printf(">V_con_c:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
} }
} }

6
Strategie.c
View File

@ -87,7 +87,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_GAUCHE}; struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE};
objectifs[0]= objectif_1; objectifs[0]= objectif_1;
objectifs[1]= objectif_2; objectifs[1]= objectif_2;
@ -97,7 +97,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN); Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS}; struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT}; struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
struct objectif_t objectif_3 = { .priorite = 2, .etat = FAIT, .cible = CERISE_DROITE}; struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE};
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE};
objectifs[0]= objectif_1; objectifs[0]= objectif_1;
objectifs[1]= objectif_2; objectifs[1]= objectif_2;
@ -118,6 +118,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
angle_fin = -150. * DEGRE_EN_RADIAN; angle_fin = -150. * DEGRE_EN_RADIAN;
point_x = 2000 - 857; point_x = 2000 - 857;
} }
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 500); Trajet_config(250, 500);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
@ -154,6 +155,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
angle_fin = -150. * DEGRE_EN_RADIAN; angle_fin = -150. * DEGRE_EN_RADIAN;
point_x = 2000 - 857; point_x = 2000 - 857;
} }
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
Trajet_config(250, 500); Trajet_config(250, 500);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,

8
gyro_ADXRS453.c
View File

@ -1,5 +1,6 @@
#include "gyro_ADXRS453.h" #include "gyro_ADXRS453.h"
#include "Monitoring.h" #include "Monitoring.h"
#include "Robot_config.h"
#include "spi_nb.h" #include "spi_nb.h"
#include <stdio.h> #include <stdio.h>
@ -57,14 +58,17 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
gyro_spi_wr_32bits(tampon_envoi, tampon_reception); gyro_spi_wr_32bits(tampon_envoi, tampon_reception);
Gyro_traitementDonnees(tampon_reception); Gyro_traitementDonnees(tampon_reception);
if(Gyro_SensorData.SQ != 0x4){ if(Gyro_SensorData.SQ != 0x4){
//printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ); printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);
//affiche_tampon_32bits(tampon_reception); affiche_tampon_32bits(tampon_reception);
Monitoring_Error("Gyro Failed - SQ bits != 0x4\n"); Monitoring_Error("Gyro Failed - SQ bits != 0x4\n");
set_position_avec_gyroscope(0);
return 1; return 1;
} }
if(Gyro_SensorData.ST != 0x1){ if(Gyro_SensorData.ST != 0x1){
printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST); printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST);
Monitoring_Error("Gyro Failed - Status != 0x1\n");
affiche_tampon_32bits(tampon_reception); affiche_tampon_32bits(tampon_reception);
set_position_avec_gyroscope(0);
return 1; return 1;
} }
return 0; return 0;