Trajectoires sans à-coup

.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"
     }
 }

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_tmp = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian;
-    delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian);
+    delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp);
-    /*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),
+    // On asservi sur +PI/2 / -PI/2
-        position_actuelle.angle_radian);
+    /*if(delta_orientation_radian > (M_PI/2)){
-    printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
+        delta_orientation_radian -= M_PI;
-    printf(">con_x:%.2f\n>con_y:%.2f\n", position_consigne.x_mm, position_consigne.y_mm);*/
+        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);

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 

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
 

main.c

@@ -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,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(">abscisse:%f\n",abscisse);
-		printf(">id:%d\n", identifiant_lire());
+		
-		sleep_ms(100);
+		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);
+
 }