Keuronde/PAMI_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Premier deplacement acceptable

Browse Source
This commit is contained in:
Samuel 2024-04-29 22:58:59 +02:00
parent 4d5e6c6253
commit ce8d5cff90
3 changed files with 19 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Asser_Position.c
View File

@ -2,8 +2,8 @@
#include "Commande_vitesse.h" #include "Commande_vitesse.h"
#include "math.h" #include "math.h"
#define GAIN_P_POSITION 10 #define GAIN_P_POSITION 1
#define GAIN_P_ORIENTATION 100 #define GAIN_P_ORIENTATION 10
struct position_t position_maintien; struct position_t position_maintien;
@ -24,6 +24,12 @@ void Asser_Position(struct position_t position_consigne){
delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm; delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm;
delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm); delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm);
delta_orientation_radian = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian; delta_orientation_radian = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian;
delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian);
/*printf(">delta_orientation_radian:%.2f\n>angle_opti:%.2f\n>actuel:%.2f\n",delta_orientation_radian,
Geometrie_get_angle_optimal(atan2f(delta_y_mm, delta_x_mm), position_actuelle.angle_radian),
position_actuelle.angle_radian);
printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
printf(">con_x:%.2f\n>con_y:%.2f\n", position_consigne.x_mm, position_consigne.y_mm);*/
// Asservissement // Asservissement
avance_mm_s = delta_avance_mm * GAIN_P_POSITION; avance_mm_s = delta_avance_mm * GAIN_P_POSITION;

3
QEI.c
View File

@ -17,7 +17,8 @@
// Nombre d'impulsions par tour de roue : 8000 // Nombre d'impulsions par tour de roue : 8000
// Impulsion / mm : 42,44 // Impulsion / mm : 42,44
#define IMPULSION_PAR_MM (12.45f) //#define IMPULSION_PAR_MM (12.45f)
#define IMPULSION_PAR_MM (16.45f)
struct QEI_t QEI_A, QEI_B; struct QEI_t QEI_A, QEI_B;

12
main.c
View File

@ -41,7 +41,7 @@ void main(void)
stdio_init_all(); stdio_init_all();
Temps_init(); Temps_init();
tension_batterie_init(); //tension_batterie_init();
identifiant_init(); identifiant_init();
Localisation_init(); Localisation_init();
Trajet_init(); Trajet_init();
@ -57,14 +57,19 @@ void main(void)
multicore_launch_core1(affichage); multicore_launch_core1(affichage);
Localisation_set(1122, 2000-63, M_PI);
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
Trajectoire_droite(&trajectoire, 0, 0, 500, 0, 0, 0); Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63,
606, 2000-590, 225, 2000-225, M_PI, M_PI);
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajet_debut_trajectoire(trajectoire); Trajet_debut_trajectoire(trajectoire);
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS; enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
sleep_ms(3000); sleep_ms(8000);
while(1){ while(1){
@ -96,6 +101,7 @@ void affichage(void){
printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
printf(">abscisse:%f\n",abscisse); printf(">abscisse:%f\n",abscisse);
printf(">id:%d\n", identifiant_lire());
sleep_ms(100); sleep_ms(100);
} }
} }