Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ajout des fonctions pour travailler avec les angles

Browse Source
This commit is contained in:
Samuel 2023-04-10 17:46:57 +02:00
parent fe161fe5c2
commit f5994a7f52
3 changed files with 92 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
Geometrie.c Normal file
View File

@ -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;
}
}

4
Geometrie.h
View File

@ -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

48
Test.c
View File

@ -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;
}