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

double => float + optimisation du temps de cycle

Browse Source
This commit is contained in:
Samuel 2023-05-06 23:46:00 +02:00
parent 69524fe01e
commit ec67302805
15 changed files with 328 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
Holonome2023.c
View File

@ -2,6 +2,7 @@
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "hardware/timer.h"
#include "pico/binary_info.h"
#include "math.h"
@ -10,10 +11,15 @@
#include "gyro.h"
#include "Asser_Moteurs.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Strategie.h"
@ -41,6 +47,7 @@ int main() {
uint32_t temps_ms = 0, temps_ms_old;
uint32_t temps_us_debut_cycle;
uint32_t score=0;
stdio_init_all();
@ -75,64 +82,94 @@ int main() {
AsserMoteur_Init();
Localisation_init();
while(mode_test());
//test_panier();
//test_homologation();
//while(mode_test());
i2c_maitre_init();
Trajet_init();
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
multicore_launch_core1(Monitoring_display);
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
while (1) {
u_int16_t step_ms = 2;
float coef_filtre = 1-0.8;
static enum {
MATCH_ATTENTE_TIRETTE,
MATCH_EN_COURS,
MATCH_ARRET_EN_COURS,
MATCH_TERMINEE
} statu_match=MATCH_ATTENTE_TIRETTE;
uint16_t _step_ms_gyro = 2;
const uint16_t _step_ms = 1;
static uint32_t timer_match_ms=0;
static enum couleur_t couleur;
while(temps_ms == Temps_get_temps_ms());
// Surveillance du temps d'execution
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
if(temps_ms != Temps_get_temps_ms()){
timer_match_ms = timer_match_ms + _step_ms;
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
// Tous les pas de step_ms
if(!(temps_ms % step_ms)){
temps_us_debut_cycle = time_us_32();
Gyro_Read(step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
//gyro_affiche(gyro_get_vitesse(), "Angle :");
// Filtre
angle_gyro = gyro_get_vitesse();
if(vitesse_filtre_x == V_INIT){
vitesse_filtre_x = angle_gyro.rot_x;
vitesse_filtre_y = angle_gyro.rot_y;
vitesse_filtre_z = angle_gyro.rot_z;
switch(statu_match){
case MATCH_ATTENTE_TIRETTE:
if(lire_couleur() == COULEUR_VERT){
// Tout vert
i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
}else{
vitesse_filtre_x = vitesse_filtre_x * (1-coef_filtre) + angle_gyro.rot_x * coef_filtre;
vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre;
vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre;
// Tout bleu
i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
}
temps_cycle = time_us_32() - temps_us_debut_cycle;
if(attente_tirette() == 0){
statu_match = MATCH_EN_COURS;
couleur = lire_couleur();
timer_match_ms=0;
i2c_annexe_couleur_balise(0, 0x00);
score=5;
i2c_annexe_envoie_score(score);
}
break;
//printf("%#x, %#x\n", (float)temps_ms_old / 1000, vitesse_filtre_z);
//printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000));
//gyro_affiche(angle_gyro, "Vitesse (°/s),");
case MATCH_EN_COURS:
if (timer_match_ms > 98000){
printf("MATCH_ARRET_EN_COURS\n");
statu_match = MATCH_ARRET_EN_COURS;
}
Strategie(couleur, _step_ms);
break;
case MATCH_ARRET_EN_COURS:
commande_vitesse_stop();
i2c_annexe_active_deguisement();
if (timer_match_ms > 100000){
statu_match = MATCH_TERMINEE;
}
// Toutes les 50 ms
if((Temps_get_temps_ms() % 50) == 0){
struct t_angle_gyro_float m_gyro;
m_gyro = gyro_get_angle_degres();
printf("%f, %f, %d\n", (float)temps_ms / 1000, m_gyro.rot_z, temps_cycle);
}
break;
// Toutes les 500 ms
if((Temps_get_temps_ms() % 500) == 0){
//gyro_affiche(gyro_get_angle(), "Angle :");
case MATCH_TERMINEE:
Moteur_Stop();
while(1);
break;
}
// Toutes les secondes
if((Temps_get_temps_ms() % 500) == 0){
//gyro_get_temp();
}
}
}

17
Monitoring.c
View File

@ -1,9 +1,12 @@
#include "pico/stdlib.h"
#include <stdio.h>
#include "Monitoring.h"
uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0;
int lock=0;
uint32_t debug_var=0;
float debug_varf=0;
void temps_cycle_check(){
static uint32_t temps_old;
@ -27,11 +30,17 @@ void temps_cycle_reset(){
temps_cycle_max=0;
}
void Monitoring_display(){
temps_cycle_display();
}
void temps_cycle_display(){
uint32_t temps;
temps = time_us_32()/1000;
printf(">T_cycle_min(us):%ld:%d\n", temps, temps_cycle_min);
printf(">T_cycle_max(us):%ld:%d\n", temps, temps_cycle_max);
printf(">DebugVar:%ld:%d\n", temps, debug_var);
printf(">DebugVarf:%ld:%f\n", temps, debug_varf);
temps_cycle_reset();
}
@ -42,3 +51,11 @@ uint32_t temps_cycle_get_min(){
uint32_t temps_cycle_get_max(){
return temps_cycle_max;
}
void set_debug_var(uint32_t variable){
debug_var = variable;
}
void set_debug_varf(float variable){
debug_varf = variable;
}

3
Monitoring.h
View File

@ -5,3 +5,6 @@ void temps_cycle_reset();
void temps_cycle_display();
uint32_t temps_cycle_get_min();
uint32_t temps_cycle_get_max();
void set_debug_var(uint32_t variable);
void set_debug_varf(float variable);
void Monitoring_display();

203
Strategie.c
View File

@ -19,10 +19,114 @@
// TODO: Peut-être à remetttre en variable locale après
float distance_obstacle;
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms);
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
enum etat_action_t lance_balles(uint32_t step_ms);
enum etat_strategie_t etat_strategie=STRATEGIE_INIT;
enum etat_homologation_t etat_homologation=STRATEGIE_INIT;
void Strategie(enum couleur_t couleur, uint32_t step_ms){
float angle_fin;
float recal_pos_x, recal_pos_y;
enum longer_direction_t longer_direction;
enum etat_action_t etat_action;
enum etat_trajet_t etat_trajet;
struct trajectoire_t trajectoire;
static enum etat_strategie_t {
STRATEGIE_INIT,
ALLER_CERISE_BAS,
ATTRAPER_CERISE_BAS,
ALLER_CERISE_HAUT,
ATTRAPER_CERISE_HAUT,
ALLER_CERISE_GAUCHE,
ATTRAPER_CERISE_GAUCHE,
ALLER_CERISE_DROITE,
ATTRAPER_CERISE_DROITE,
ALLER_PANIER,
LANCER_PANIER,
}etat_strategie;
switch(etat_strategie){
case STRATEGIE_INIT:
if(couleur == COULEUR_BLEU){
Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
}else{
Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
}
etat_strategie = ALLER_CERISE_BAS;
break;
case ALLER_CERISE_BAS:
if(couleur == COULEUR_BLEU){
angle_fin = 30. * DEGRE_EN_RADIAN;
}else{
angle_fin = -150. * DEGRE_EN_RADIAN;
}
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 857, 156,
Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPER_CERISE_BAS;
}
break;
case ATTRAPER_CERISE_BAS:
recal_pos_y = RAYON_ROBOT;
if(couleur == COULEUR_BLEU){
longer_direction = LONGER_VERS_C;
recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
}else{
longer_direction = LONGER_VERS_A;
recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
}
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
if(etat_action == ACTION_TERMINEE){
etat_strategie = ALLER_PANIER;
}
break;
case ALLER_PANIER:
Trajet_config(500, 500);
if(couleur == COULEUR_BLEU){
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
465, 857,
465,2830,
+30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
}else{
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
485, Localisation_get().y_mm,
2000-465, 857,
2000-465,2830,
-150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
}
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = LANCER_PANIER;
}
break;
case LANCER_PANIER:
if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){
etat_homologation = STRATEGIE_FIN;
}
break;
case STRATEGIE_FIN:
i2c_annexe_desactive_propulseur();
commande_vitesse_stop();
break;
}
}
void Homologation(uint32_t step_ms){
@ -32,15 +136,15 @@ void Homologation(uint32_t step_ms){
struct trajectoire_t trajectoire;
switch(etat_strategie){
switch(etat_homologation){
case STRATEGIE_INIT:
Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
etat_strategie = ATTENTE_TIRETTE;
etat_homologation = ATTENTE_TIRETTE;
break;
case ATTENTE_TIRETTE:
if(attente_tirette() == 0){
etat_strategie = APPROCHE_CERISE_1_A;
etat_homologation = APPROCHE_CERISE_1_A;
}
break;
@ -48,14 +152,14 @@ void Homologation(uint32_t step_ms){
Trajet_config(250, 500);
Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
etat_strategie = ATTRAPE_CERISE_1;
etat_homologation = ATTRAPE_CERISE_1;
}
break;
case ATTRAPE_CERISE_1:
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, RAYON_ROBOT);
if(etat_action == ACTION_TERMINEE){
etat_strategie = APPROCHE_PANIER_1;
etat_homologation = APPROCHE_PANIER_1;
}
break;
@ -68,24 +172,13 @@ void Homologation(uint32_t step_ms){
+30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
etat_homologation = CALAGE_PANIER_1;
}
break;
case APPROCHE_PANIER_2:
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
265,2830,
+120. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = CALAGE_PANIER_1;
}
break;
case CALAGE_PANIER_1:
if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
etat_strategie = RECULE_PANIER;
etat_homologation = RECULE_PANIER;
}
break;
@ -96,14 +189,14 @@ void Homologation(uint32_t step_ms){
120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = LANCE_DANS_PANIER;
etat_homologation = LANCE_DANS_PANIER;
}
break;
case LANCE_DANS_PANIER:
Asser_Position_maintien();
if(lance_balles(step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
etat_homologation = STRATEGIE_FIN;
}
break;
@ -114,14 +207,14 @@ void Homologation(uint32_t step_ms){
Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_strategie = STRATEGIE_FIN;
etat_homologation = STRATEGIE_FIN;
}
break;
case ATTRAPPE_CERISE_2:
etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, 3000-RAYON_ROBOT);
if(etat_action == ACTION_TERMINEE){
etat_strategie = APPROCHE_PANIER_2;
etat_homologation = APPROCHE_PANIER_2;
}
break;
@ -134,6 +227,64 @@ void Homologation(uint32_t step_ms){
}
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms){
static enum {
CALAGE_PANIER_1,
RECULE_PANIER,
LANCE_DANS_PANIER,
}etat_lance_balles_dans_panier;
float recal_pos_x, recal_pos_y;
enum longer_direction_t longer_direction;
float point_x, point_y;
enum etat_action_t etat_action = ACTION_EN_COURS;
struct trajectoire_t trajectoire;
switch(etat_lance_balles_dans_panier){
case CALAGE_PANIER_1:
if(couleur == COULEUR_BLEU){
recal_pos_x = RAYON_ROBOT;
longer_direction = LONGER_VERS_A;
}else{
recal_pos_x = 2000- RAYON_ROBOT;
longer_direction = LONGER_VERS_C;
}
if(calage_angle(longer_direction, recal_pos_x, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
etat_lance_balles_dans_panier = RECULE_PANIER;
}
break;
case RECULE_PANIER:
Trajet_config(250, 500);
if(couleur == COULEUR_BLEU){
point_x = 180;
point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
}else{
point_x = 1735;
point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
}
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
point_x, point_y,
120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
}
break;
case LANCE_DANS_PANIER:
Asser_Position_maintien();
if(lance_balles(step_ms) == ACTION_TERMINEE){
etat_action=ACTION_TERMINEE;
}
break;
}
return etat_action;
}
/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
/// @param step_ms : pas de temps.
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
@ -185,7 +336,7 @@ enum etat_action_t lance_balles(uint32_t step_ms){
if(temporisation_terminee(&tempo_ms, step_ms)){
i2c_annexe_mi_ferme_porte();
etat_lance_balle = LANCE_TEMPO_PROP_ON;
tempo_ms = 500;
tempo_ms = 750;
}
break;

5
Strategie.h
View File

@ -25,7 +25,7 @@ enum couleur_t{
COULEUR_INCONNUE,
};
extern enum etat_strategie_t{
extern enum etat_homologation_t{
STRATEGIE_INIT,
ATTENTE_TIRETTE,
APPROCHE_CERISE_1_A,
@ -42,7 +42,7 @@ extern enum etat_strategie_t{
APPROCHE_PANIER_3,
STRATEGIE_FIN,
}etat_strategie;
}etat_homologation;
enum etat_action_t cerise_accostage(void);
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
@ -54,6 +54,7 @@ enum couleur_t lire_couleur(void);
uint attente_tirette(void);
enum etat_action_t lance_balles(uint32_t step_ms);
int test_panier(void);
void Strategie(enum couleur_t couleur, uint32_t step_ms);
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);

6
Strategie_prise_cerises.c
View File

@ -30,8 +30,11 @@ float vitesse_accostage_mm_s=100;
/// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure.
/// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
/// @param longer_direction : direction dans laquelle se trouve la bordure
/// @param step_ms : fréquence d'appel
/// @param pos_recalage_x_mm : Position de recalage contre le support de cerises
/// @param pos_recalage_y_mm : Position de recalage contre la bordure du terrain
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm){
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm, float pos_recalage_y_mm){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum longer_direction_t longer_direction_aspire;
static uint32_t tempo_ms = 0;
@ -55,6 +58,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
(longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
Localisation_set_x(pos_recalage_x_mm);
Localisation_set_y(pos_recalage_y_mm);
etat_attrape = TURBINE_DEMARRAGE;
}
break;

2
Strategie_prise_cerises.h
View File

@ -1,2 +1,2 @@
#include "Strategie.h"
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm);
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);

2
Test.c
View File

@ -1421,7 +1421,7 @@ void affiche_monitoring(){
int test_angle_balise(void){
int lettre;
float distance, angle=0;
float distance, angle=3.1281;
i2c_maitre_init();
Balise_VL53L1X_init();

7
Test_strategie.c
View File

@ -47,7 +47,10 @@ void affichage_test_strategie(){
printf(">tirette:%ld:%d\n", temps, attente_tirette());
printf(">etat_strat:%ld:%d\n", temps, etat_strategie);
printf(">etat_strat:%ld:%d\n", temps, etat_homologation);
printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());
/*switch(etat_strategie){
case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break;
@ -125,6 +128,7 @@ int test_homologation(){
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
@ -191,6 +195,7 @@ void affichage_test_evitement(){
for(uint8_t capteur=0; capteur<12; capteur++){
printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
}
printf(">distance_minimale:%f\n",Trajet_get_obstacle_mm());
}
}

11
Trajectoire.c
View File

@ -3,11 +3,12 @@
#include "Trajectoire_circulaire.h"
#include "Trajectoire_droite.h"
#include "Trajectoire_rotation.h"
#include "Monitoring.h"
#include "math.h"
#define NB_MAX_TRAJECTOIRES 5
#define PRECISION_ABSCISSE 0.001
#define PRECISION_ABSCISSE 0.001f
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon,
@ -115,7 +116,7 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
/// @brief Renvoie le point d'une trajectoire à partir de son abscisse
/// @param abscisse : abscisse sur la trajectoire
/// @return point en coordonnées X/Y
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse){
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
struct point_xyo_t point_xyo;
switch(trajectoire->type){
case TRAJECTOIRE_DROITE:
@ -149,8 +150,8 @@ float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float
/// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire
/// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire
/// @return nouvelle abscisse
float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm){
float delta_abscisse, delta_mm, erreur_relative;
float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){
double delta_abscisse, delta_mm, erreur_relative;
if(distance_mm == 0){
return abscisse;
@ -175,7 +176,7 @@ float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, flo
return abscisse + delta_abscisse;
}
float distance_points(struct point_xy_t point, struct point_xy_t point_old){
double distance_points(struct point_xy_t point, struct point_xy_t point_old){
return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2));
}

8
Trajectoire.h
View File

@ -9,7 +9,7 @@ enum trajectoire_type_t{
};
struct point_xy_t{
float x, y;
double x, y;
};
struct point_xyo_t{
@ -26,10 +26,10 @@ struct trajectoire_t {
};
float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse);
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse);
float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse);
float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm);
float distance_points(struct point_xy_t point, struct point_xy_t point_old);
float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm);
double distance_points(struct point_xy_t point, struct point_xy_t point_old);
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre,
float rayon, float orientation_debut_rad, float orientation_fin_rad);
void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad);

18
Trajectoire_bezier.c
View File

@ -19,17 +19,17 @@ void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){
/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
/// @param abscisse : compris entre 0 et 1
struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse){
struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse){
struct point_xy_t point;
point.x = (float) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) +
3 * (float) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) +
3 * (float) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) +
(float) trajectoire->p4.x * abscisse * abscisse * abscisse;
point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) +
3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) +
3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) +
(double) trajectoire->p4.x * abscisse * abscisse * abscisse;
point.y = (float) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) +
3 * (float) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) +
3 * (float) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) +
(float) trajectoire->p4.y * abscisse * abscisse * abscisse;
point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) +
3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) +
3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) +
(double) trajectoire->p4.y * abscisse * abscisse * abscisse;
return point;
}

2
Trajectoire_bezier.h
View File

@ -2,4 +2,4 @@
void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire);
struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse);
struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse);

12
Trajet.c
View File

@ -3,6 +3,7 @@
#include "Trajectoire.h"
#include "Trajet.h"
#include "Asser_Position.h"
#include "Monitoring.h"
float Trajet_calcul_vitesse(float temps_s);
int Trajet_terminee(float abscisse);
@ -60,6 +61,7 @@ enum etat_trajet_t Trajet_avance(float pas_de_temps_s){
// Calcul de l'abscisse sur la trajectoire
abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm);
set_debug_varf(abscisse);
// Obtention du point consigne
point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
@ -126,14 +128,14 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm;
// En cas de dépassement, on veut garder la contrainte, pour l'instant
if(distance_contrainte > 0){
vitesse_max_contrainte = sqrt(2 * acceleration_mm_ss * distance_contrainte);
vitesse_max_contrainte = sqrtf(2 * acceleration_mm_ss * distance_contrainte);
}else{
vitesse_max_contrainte = 0;
}
distance_contrainte_obstacle = Trajet_get_obstacle_mm();
if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
vitesse_max_contrainte_obstacle = sqrt(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
}
@ -177,6 +179,10 @@ float Trajet_get_orientation_avance(){
point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse);
angle = atan2(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x);
angle = atan2f(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x);
return angle;
}
float Trajet_get_abscisse(){
return abscisse;
}

1
Trajet.h
View File

@ -23,3 +23,4 @@ float Trajet_get_obstacle_mm(void);
void Trajet_set_obstacle_mm(float distance_mm);
void Trajet_stop(float);
float Trajet_get_orientation_avance(void);
float Trajet_get_abscisse();