diff --git a/.vscode/settings.json b/.vscode/settings.json index 87f6ef0..d2f1e26 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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" } } \ No newline at end of file diff --git a/Asser_Position.c b/Asser_Position.c index aca2a44..aacb514 100644 --- a/Asser_Position.c +++ b/Asser_Position.c @@ -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,17 +26,28 @@ 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); diff --git a/Trajet.c b/Trajet.c index 4f129cd..4dc9ef0 100644 --- a/Trajet.c +++ b/Trajet.c @@ -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 diff --git a/Trajet.h b/Trajet.h index 8fd2be8..c0453ec 100644 --- a/Trajet.h +++ b/Trajet.h @@ -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 diff --git a/main.c b/main.c index 3bd2b0b..63dffc0 100644 --- a/main.c +++ b/main.c @@ -16,9 +16,12 @@ #include "Trajectoire.h" #include "Trajet.h" #include +#include #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,34 +67,54 @@ 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(); - + 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); } } @@ -92,17 +122,21 @@ void main(void) } } - +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); + } \ No newline at end of file