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

Trajectoires sans à-coup

Browse Source
This commit is contained in:
Samuel 2024-04-30 17:59:41 +02:00
parent ce8d5cff90
commit 16a45b620d
5 changed files with 131 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -3,6 +3,7 @@
"geometrie.h": "c",
"geometrie_robot.h": "c",
"commande_vitesse.h": "c",
"asser_moteurs.h": "c"
"asser_moteurs.h": "c",
"localisation.h": "c"
}
}

30
Asser_Position.c
View File

@ -11,11 +11,14 @@ struct position_t position_maintien;
/// C'est à la consigne d'être défini avant pour être atteignable.
/// Nécessite l'appel des fonctions QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms);
/// @param position_consigne : position à atteindre dans le référentiel de la table.
float delta_x_mm, delta_y_mm, delta_orientation_radian;
float delta_orientation_radian_tmp;
void Asser_Position(struct position_t position_consigne){
float delta_avance_mm;
float avance_mm_s, rotation_radian_s;
float delta_x_mm, delta_y_mm, delta_orientation_radian;
//float delta_x_mm, delta_y_mm, delta_orientation_radian;
struct position_t position_actuelle;
float delta_orientation_radian_tmp;
position_actuelle = Localisation_get();
@ -23,18 +26,29 @@ void Asser_Position(struct position_t position_consigne){
delta_x_mm = position_consigne.x_mm - position_actuelle.x_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_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);*/
delta_orientation_radian_tmp = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian;
delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp);
// On asservi sur +PI/2 / -PI/2
/*if(delta_orientation_radian > (M_PI/2)){
delta_orientation_radian -= M_PI;
delta_avance_mm = -delta_avance_mm;
}
if(delta_orientation_radian < -(M_PI/2)){
delta_orientation_radian += M_PI;
delta_avance_mm = -delta_avance_mm;
}*/
// Asservissement
avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
if(delta_avance_mm < 10){
rotation_radian_s=0;
}
// Commande en vitesse
commande_vitesse(avance_mm_s, rotation_radian_s);

3
Trajet.c
View File

@ -53,10 +53,11 @@ void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
/// @brief Avance la consigne de position sur la trajectoire
/// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde
/// @return TRAJET_EN_COURS ou TRAJET_TERMINE
struct point_xyo_t point;
enum etat_trajet_t Trajet_avance(float pas_de_temps_s){
float distance_mm;
enum etat_trajet_t trajet_etat = TRAJET_EN_COURS;
struct point_xyo_t point;
struct position_t position;
// Calcul de la vitesse

4
Trajet.h
View File

@ -10,11 +10,11 @@ enum etat_trajet_t{
};
// Vitesse et acceleration pour translation pure (en mm/s et mm/s²)
#define TRAJECT_CONFIG_AVANCE_DROIT 1000, 500
#define TRAJECT_CONFIG_RAPIDE 300, 600
// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
// Vitesse et acceleration - standard (en mm et mm/s²)
#define TRAJECT_CONFIG_STD 500, 500
#define TRAJECT_CONFIG_STD 300, 300
// Vitesse et acceleration pour une rotation (rad/s et rad/s²)
#define TRAJECT_CONFIG_ROTATION_PURE 2, 2

122
main.c
View File

@ -16,9 +16,12 @@
#include "Trajectoire.h"
#include "Trajet.h"
#include <stdio.h>
#include <math.h>
#include "QEI.h"
#define LED1PIN 20
#define TIRETTE_PIN 6
#define COULEUR_PIN 4
void affichage(void);
void tension_batterie_init(void);
@ -27,11 +30,17 @@ uint16_t tension_batterie_lire(void);
void identifiant_init(void);
uint identifiant_lire(void);
int get_tirette(void);
int get_couleur(void);
void configure_trajet(int identifiant, int couleur);
uint32_t step_ms=1;
float distance1_mm=0, distance2_mm=0;
// DEBUG
extern float abscisse;
extern struct point_xyo_t point;
float vitesse;
void main(void)
{
@ -47,6 +56,7 @@ void main(void)
Trajet_init();
uint32_t temps_ms = Temps_get_temps_ms();
uint32_t temps_depart_ms;
struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
float vitesse_mm_s=100;
@ -57,52 +67,76 @@ void main(void)
multicore_launch_core1(affichage);
Localisation_set(1122, 2000-63, M_PI);
/*Localisation_set(1130, 2000-63, M_PI);
struct trajectoire_t trajectoire;
Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63,
606, 2000-590, 225, 2000-225, M_PI, M_PI);
Trajet_config(TRAJECT_CONFIG_RAPIDE);
Trajet_debut_trajectoire(trajectoire);*/
/*Localisation_set(0, 0, 0);
struct trajectoire_t trajectoire;
Trajectoire_droite(&trajectoire, 0,0,1000,0, M_PI, M_PI);
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajet_debut_trajectoire(trajectoire);
Trajet_debut_trajectoire(trajectoire);*/
configure_trajet(identifiant_lire(), 0);
float vitesse_init =300;
vitesse = vitesse_init;
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
sleep_ms(8000);
while(get_tirette());
// sleep_ms(90000);
temps_depart_ms = Temps_get_temps_ms();
while(1){
// Fin du match
if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
Moteur_Stop();
while(1);
}
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
if(temps_ms % step_ms == 0){
QEI_update();
Localisation_gestion();
if(etat_trajet != TRAJET_TERMINE){
AsserMoteur_Gestion(step_ms);
etat_trajet = Trajet_avance((float)step_ms/1000.);
}else{
Moteur_Stop();
Asser_Position_maintien();
}
AsserMoteur_Gestion(step_ms);
}
}
}
}
}
}
extern float delta_x_mm, delta_y_mm, delta_orientation_radian;
void affichage(void){
while(1){
printf(">c1_mm:%f\n>c2_mm:%f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_NAME) );
/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
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(">abscisse:%f\n",abscisse);
printf(">id:%d\n", identifiant_lire());
sleep_ms(100);
//printf(">abscisse:%f\n",abscisse);
struct position_t position_actuelle;
position_actuelle = Localisation_get();
printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
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", point.point_xy.x, point.point_xy.y);
//printf(">couleur:%d\n>id:%d\n", get_couleur(), identifiant_lire());
}
}
@ -131,11 +165,67 @@ void identifiant_init(){
gpio_set_dir(21, GPIO_IN);
gpio_set_dir(22, GPIO_IN);
gpio_set_dir(26, GPIO_IN);
// Tirette
gpio_init(TIRETTE_PIN);
gpio_pull_up(TIRETTE_PIN);
gpio_set_dir(TIRETTE_PIN, GPIO_IN);
// Couleur
gpio_init(COULEUR_PIN);
gpio_pull_up(COULEUR_PIN);
gpio_set_dir(COULEUR_PIN, GPIO_IN);
}
int get_tirette(void){
return !gpio_get(TIRETTE_PIN);
}
int get_couleur(void){
return !gpio_get(COULEUR_PIN);
}
/// @brief !! Arg la GPIO 26 ne répond pas !
/// @return
uint identifiant_lire(){
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
}
void configure_trajet(int identifiant, int couleur){
struct trajectoire_t trajectoire;
switch (identifiant)
{
case 0:
break;
case 1:
Localisation_set(1465, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1465, 2000-63, 1260, 2000-63,
430, 1240, 430, 2000, M_PI, M_PI);
break;
case 2:
break;
case 3:
Localisation_set(1130, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63,
606, 2000-590, 225, 2000-225, M_PI, M_PI);
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
default:
break;
}
Trajet_config(TRAJECT_CONFIG_RAPIDE);
Trajet_debut_trajectoire(trajectoire);
}