Localisation fonctionnelle

.vscode/settings.json

@@ -1,5 +1,6 @@
 {
     "files.associations": {
-
+        "geometrie.h": "c",
+        "geometrie_robot.h": "c"
     }
 }

CMakeLists.txt

@@ -12,7 +12,9 @@ pico_sdk_init()
 
 add_executable(Mon_Projet
   Asser_Moteurs.c
+  Geometrie.c
   Moteurs.c
+  Localisation.c
   main.c
   Temps.c
   QEI.c

Geometrie.c

@@ -0,0 +1,82 @@
+#include "Geometrie.h"
+#include "math.h"
+
+
+/// @brief Retourne l'angle entre -PI et +PI
+/// @param angle 
+/// @return 
+float Geometrie_get_angle_normalisee(float angle){
+    while(angle > M_PI){
+        angle -= 2* M_PI;
+    }
+    while(angle < -M_PI){
+        angle += 2* M_PI;
+    }
+    return angle;
+}
+
+/// @brief Indique si un angle est compris entre deux angles. Les angles doivent être entre -PI et PI.
+/// @param angle : angle à comparer
+/// @param angle_min : début de la fourchette
+/// @param angle_max : fin de la fourchette
+/// @return 1 si l'angle est compris entre min et max, 0 sinon
+unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max){
+    if(angle_min > angle_max){
+        // cas où la fourchette comprend -PI.
+        if( (angle > angle_min) || (angle < angle_max)){
+            return 1;
+        }
+        return 0;
+    }else{
+        // Cas normal
+        if( (angle > angle_min) && (angle < angle_max)){
+            return 1;
+        }
+        return 0;
+    }
+}
+
+/// @brief A partir de l'orientation actuelle du robot et de l'orientation souhaitée, 
+/// donne l'angle consigne pour limiter les rotations inutiles.
+/// Tous les angles sont en radian
+/// @param angle_depart 
+/// @param angle_souhaite 
+/// @return angle_optimal en radian
+float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){
+    while((angle_depart - angle_souhaite) > M_PI){
+        angle_souhaite += 2* M_PI;
+    }
+    while((angle_depart - angle_souhaite) < -M_PI){
+        angle_souhaite -= 2* M_PI;
+    }
+    return angle_souhaite;
+}
+
+/// @brief Indique si les deux plages d'angle se recoupent
+/// @param angle1_min Début de la première plage
+/// @param angle1_max Fin de la première plage
+/// @param angle2_min Début de la seconde plage
+/// @param angle2_max Fin de la seconde plage
+/// @return 1 si les deux plages s'intersectent, 0 sinon
+unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max){
+    // Pour que les plages s'intersectent, soit :
+    // * angle1_min est compris entre angle2_min et angle2_max
+    // * angle1_max est compris entre angle2_min et angle2_max
+    // * angle2_min et angle2_max sont compris entre angle1_min et angle1_max (tester angle2_min ou angle2_max est suffisant)
+    if(Geometrie_compare_angle(angle1_min, angle2_min, angle2_max) ||
+        Geometrie_compare_angle(angle1_max, angle2_min, angle2_max) ||
+        Geometrie_compare_angle(angle2_min, angle1_min, angle1_max)){
+            return 1;
+    }
+    return 0;
+}
+
+/// @brief Déplace un point de la distance indiquée en se servant de l'angle de la position donnée.
+struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){
+    struct position_t position_arrivée;
+    position_arrivée.angle_radian = position_depart.angle_radian;
+    position_arrivée.x_mm = position_depart.x_mm + cosf(position_depart.angle_radian) * distance_mm;
+    position_arrivée.y_mm = position_depart.y_mm + sinf(position_depart.angle_radian) * distance_mm;
+    
+    return position_arrivée;
+}

Geometrie.h

@@ -0,0 +1,22 @@
+#ifndef GEOMETRIE_H
+#define GEOMETRIE_H
+
+#ifndef M_PI
+#define M_PI (3.14159265358979323846)
+#endif
+
+#define DEGRE_EN_RADIAN  (M_PI / 180.)
+#define DISTANCE_INVALIDE (-1.)
+
+struct position_t{
+    float x_mm, y_mm;
+    float angle_radian;
+};
+
+float Geometrie_get_angle_normalisee(float angle);
+unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max);
+unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max);
+float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite);
+struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm);
+
+#endif

Geometrie_robot.h

@@ -0,0 +1,4 @@
+#include "Geometrie.h"
+
+#define DISTANCE_ROUES_CENTRE_MM 52.
+

Localisation.c

@@ -0,0 +1,54 @@
+#include "Localisation.h"
+#include "Temps.h"
+#include "QEI.h"
+#include "math.h"
+#include "Geometrie_robot.h"
+
+struct position_t position;
+
+void Localisation_init(){
+    Temps_init();
+    QEI_init();
+    position.x_mm = 0;
+    position.y_mm = 0;
+    position.angle_radian = 0;
+}
+
+void Localisation_set(float x_mm, float y_mm, float angle_radian){
+    position.x_mm = x_mm;
+    position.y_mm = y_mm;
+    position.angle_radian = angle_radian;
+}
+
+void Localisation_set_x(float x_mm){
+    position.x_mm = x_mm;
+}
+
+void Localisation_set_y(float y_mm){
+    position.y_mm = y_mm;
+}
+
+void Localisation_set_angle(float angle_radian){
+    position.angle_radian = angle_radian;
+}
+
+void Localisation_gestion(){
+    float distance_roue_gauche_mm = QEI_get_mm(QEI_B_NAME);
+    float distance_roue_droite_mm = QEI_get_mm(QEI_A_NAME);
+    
+    float delta_avance_mm, delta_orientation_rad;
+
+    delta_avance_mm = (distance_roue_droite_mm + distance_roue_gauche_mm)/2;
+    delta_orientation_rad = (distance_roue_droite_mm - distance_roue_gauche_mm) / (DISTANCE_ROUES_CENTRE_MM*2);
+
+    position.angle_radian += delta_orientation_rad;
+
+    // Projection dans le référentiel de la table
+    position.x_mm += delta_avance_mm * cosf(position.angle_radian);
+    position.y_mm += delta_avance_mm * sinf(position.angle_radian);
+
+}
+
+struct position_t Localisation_get(void){
+    return position;
+}

Localisation.h

@@ -0,0 +1,10 @@
+#include "Geometrie.h"
+
+struct position_t Localisation_get(void);
+void Localisation_gestion();
+void Localisation_init();
+
+void Localisation_set(float x_mm, float y_mm, float angle_radian);
+void Localisation_set_x(float x_mm);
+void Localisation_set_y(float y_mm);
+void Localisation_set_angle(float angle_radian);

main.c

@@ -8,6 +8,7 @@
 #include "hardware/adc.h"
 #include "hardware/pwm.h"
 #include "Asser_Moteurs.h"
+#include "Localisation.h"
 #include "Moteurs.h"
 #include "Temps.h"
 #include <stdio.h>
@@ -36,19 +37,16 @@ void main(void)
 	Temps_init();
 	tension_batterie_init();
 	identifiant_init();
+	Localisation_init();
 	uint32_t temps_ms = Temps_get_temps_ms();
 	
 
 	gpio_init(LED1PIN);
 	gpio_set_dir(LED1PIN, GPIO_OUT );
 	gpio_put(LED1PIN, 1);
-	AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100.);
-	AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100.);
 
 	
 	multicore_launch_core1(affichage);
-	Moteur_SetVitesse(MOTEUR_A, 000);
-	Moteur_SetVitesse(MOTEUR_B, 000);
 
 
 	while(1){
@@ -58,6 +56,7 @@ void main(void)
 			if(temps_ms % step_ms == 0){
 				QEI_update();
 				AsserMoteur_Gestion(step_ms);
+				Localisation_gestion();
 			}
 			if(temps_ms % 100 == 0){
 				identifiant_lire();
@@ -76,6 +75,7 @@ void affichage(void){
 		
 		/*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);
 		sleep_ms(100);
 	}
 }