Ajout des fonctions pour travailler avec les angles

Geometrie.c

@@ -0,0 +1,40 @@
+#include "Geometrie.h"
+
+
+/// @brief Retourne l'angle entre -PI et +PI
+/// @param angle 
+/// @return 
+double Geometrie_get_angle_normalisee(double angle){
+    while(angle > M_PI){
+        angle -= 2* M_PI;
+    }
+    while(angle < 2* -M_PI){
+        angle += 2* M_PI;
+    }
+    return angle;
+}
+
+/// @brief Indique si un angle est compris entre deux angles
+/// @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(double angle, double angle_min, double angle_max){
+    angle = Geometrie_get_angle_normalisee(angle);
+    angle_min = Geometrie_get_angle_normalisee(angle_min);
+    angle_max = Geometrie_get_angle_normalisee(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;
+    }
+}

Geometrie.h

@@ -5,10 +5,14 @@
 #define M_PI (3.14159265358979323846)
 #endif
 
+#define DEGRE_EN_RADIAN  (M_PI / 180.)
 
 struct position_t{
     double x_mm, y_mm;
     double angle_radian;
 };
 
+double Geometrie_get_angle_normalisee(double angle);
+unsigned int Geometrie_compare_angle(double angle, double angle_min, double angle_max);
+
 #endif

Test.c

@@ -59,6 +59,7 @@ int test_i2c_ecriture_pico_annex_nb_2();
 int test_aller_retour();
 void test_trajectoire_teleplot();
 int test_capteurs_balise(void);
+int test_geometrie(void);
 
 
 // Mode test : renvoie 0 pour quitter le mode test
@@ -78,6 +79,7 @@ int mode_test(){
     printf("K - Trajets aller retour avec Gyro\n");
     printf("L - pour la localisation\n");
     printf("M - pour les moteurs\n");
+    printf("N - Fonctions geometrique\n");
     printf("T - Trajectoire\n");
     printf("U - Scan du bus i2c\n");
     printf("V - APDS_9960\n");
@@ -154,6 +156,11 @@ int mode_test(){
         while(test_moteurs());
         break;
 
+    case 'N':
+    case 'n':
+        while(test_geometrie());
+        break;
+
     case 'T':
     case 't':
         while(test_trajectoire());
@@ -1321,3 +1328,44 @@ int test_vitesse_moteur(enum t_moteur moteur){
     }
     return 1;
 }
+
+
+int test_geometrie(){
+    double angle = 270, angle_min, angle_max;
+    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
+    angle = 180;
+    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
+    angle = 181;
+    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
+    angle = 179;
+    printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
+
+
+    angle_min = -100;
+    angle_max = -80;
+    angle = -90;
+    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_min, angle_max, 
+        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
+    angle = 90;
+    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
+        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
+    angle = -120;
+    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
+        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
+
+
+
+    angle_min = 178;
+    angle_max = 182;
+    angle = 179;
+    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_min, angle_max, 
+        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
+    angle = 177;
+    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
+        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
+    angle = 183;
+    printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min, 
+        Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
+
+    return 0;
+}