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

Float -> Double

Browse Source
This commit is contained in:
Samuel 2023-04-28 23:51:43 +02:00
parent 87cf522550
commit 7c22d99acd
45 changed files with 306 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/c_cpp_properties.json vendored
View File

@ -2,6 +2,7 @@
"env": {
"myDefaultIncludePath": [
"${workspaceFolder}",
"${workspaceFolder}/build",
"${env:PICO_SDK_PATH}/src/./common/pico_binary_info/include",
"${env:PICO_SDK_PATH}/src/./common/pico_base/include",
"${env:PICO_SDK_PATH}/src/./common/pico_time/include",
@ -76,8 +77,7 @@
"${myDefaultIncludePath}"
],
"defines": [
"FOO",
"BAR=100"
"GYRO_ADXRS453"
],
"compilerPath": "/usr/bin/arm-none-eabi-gcc",
"cStandard": "c11",

20
Asser_Moteurs.c
View File

@ -17,8 +17,8 @@
#define ASSERMOTEUR_GAIN_P 160
#define ASSERMOTEUR_GAIN_I .80f
double consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
double commande_I[3]; // Terme integral
float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
float commande_I[3]; // Terme integral
void AsserMoteur_Init(){
for(unsigned int i =0; i< 3; i ++){
@ -30,22 +30,22 @@ void AsserMoteur_Init(){
/// @brief Défini une consigne de vitesse pour le moteur indiqué.
/// @param moteur : Moteur à asservir
/// @param _consigne_mm_s : consigne de vitesse en mm/s
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double _consigne_mm_s){
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){
consigne_mm_s[moteur] = _consigne_mm_s;
}
/// @brief Envoie la consigne du moteur
/// @param moteur : Moteur à asservir
double AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){
return consigne_mm_s[moteur];
}
double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
enum QEI_name_t qei;
double distance, temps;
float distance, temps;
switch (moteur)
{
case MOTEUR_A: qei = QEI_A_NAME; break;
@ -55,7 +55,7 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
default: break;
}
distance = QEI_get_mm(qei);
temps = step_ms / 1000.0;
temps = step_ms / 1000.0f;
return distance / temps;
}
@ -65,9 +65,9 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
void AsserMoteur_Gestion(int step_ms){
// Pour chaque moteur
for(uint moteur=MOTEUR_A; moteur<MOTEUR_C+1; moteur++ ){
double erreur; // Erreur entre la consigne et la vitesse actuelle
double commande_P; // Terme proportionnel
double commande;
float erreur; // Erreur entre la consigne et la vitesse actuelle
float commande_P; // Terme proportionnel
float commande;
// Calcul de l'erreur
erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);

6
Asser_Moteurs.h
View File

@ -1,7 +1,7 @@
#include "Moteurs.h"
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double consigne_mm_s);
double AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s);
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
void AsserMoteur_Gestion(int step_ms);
void AsserMoteur_Init();

10
Asser_Position.c
View File

@ -12,9 +12,9 @@ struct position_t position_maintien;
/// 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.
void Asser_Position(struct position_t position_consigne){
double vitesse_x_mm_s, vitesse_y_mm_s, rotation_radian_s;
double vitesse_robot_x_mm_s, vitesse_robot_y_mm_s;
double delta_x_mm, delta_y_mm, delta_orientation_radian;
float vitesse_x_mm_s, vitesse_y_mm_s, rotation_radian_s;
float vitesse_robot_x_mm_s, vitesse_robot_y_mm_s;
float delta_x_mm, delta_y_mm, delta_orientation_radian;
struct position_t position_actuelle;
position_actuelle = Localisation_get();
@ -33,8 +33,8 @@ void Asser_Position(struct position_t position_consigne){
// C'est pas bon, c'est l'inverse !!!
//vitesse_robot_x_mm_s = cos(position_actuelle.angle_radian) * vitesse_x_mm_s - sin(position_actuelle.angle_radian) * vitesse_y_mm_s;
//vitesse_robot_y_mm_s = sin(position_actuelle.angle_radian) * vitesse_x_mm_s + cos(position_actuelle.angle_radian) * vitesse_y_mm_s;
vitesse_robot_x_mm_s = cos(position_actuelle.angle_radian) * vitesse_x_mm_s + sin(position_actuelle.angle_radian) * vitesse_y_mm_s;
vitesse_robot_y_mm_s = -sin(position_actuelle.angle_radian) * vitesse_x_mm_s + cos(position_actuelle.angle_radian) * vitesse_y_mm_s;
vitesse_robot_x_mm_s = cosf(position_actuelle.angle_radian) * vitesse_x_mm_s + sinf(position_actuelle.angle_radian) * vitesse_y_mm_s;
vitesse_robot_y_mm_s = -sinf(position_actuelle.angle_radian) * vitesse_x_mm_s + cosf(position_actuelle.angle_radian) * vitesse_y_mm_s;
// Commande en vitesse
commande_vitesse(vitesse_robot_x_mm_s, vitesse_robot_y_mm_s, rotation_radian_s);

46
Balise_VL53L1X.c
View File

@ -11,14 +11,15 @@
#define DEMI_CONE_CAPTEUR_RADIAN 0.2225
uint8_t distance_capteur_cm[NB_CAPTEURS];
uint8_t capteur_courant=0; /* capteur en cours d'actualisation */
struct capteur_VL53L1X_t{
uint8_t distance_lue_cm; // Distance entre le capteur et l'obstacle.
double distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
double angle_ref_robot; // Orientation du capteur dans le référentiel du robot
double angle_ref_terrain; // Orientation du capteur dans le référentiel du terrain
double angle_ref_terrain_min; // Cone de détection du capteur (min)
double angle_ref_terrain_max; // Cone de détection du capteur (max)
float distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
float angle_ref_robot; // Orientation du capteur dans le référentiel du robot
float angle_ref_terrain; // Orientation du capteur dans le référentiel du terrain
float angle_ref_terrain_min; // Cone de détection du capteur (min)
float angle_ref_terrain_max; // Cone de détection du capteur (max)
uint donnee_valide; // L'obstacle détecté est dans le terrain et n'est pas dans le robot
uint donnee_ignore; // Le capteur est ignoré car derrière le robot.
}capteurs_VL53L1X[NB_CAPTEURS];
@ -37,15 +38,8 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
struct position_t position_robot;
position_robot = Localisation_get();
// Actualisation de l'angle du capteur
capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian;
// Maintien de l'angle entre -PI et PI
while(capteur_VL53L1X->angle_ref_terrain > M_PI){
capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
}
while(capteur_VL53L1X->angle_ref_terrain < -M_PI){
capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
}
capteur_VL53L1X->angle_ref_terrain = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian);
capteur_VL53L1X->distance_lue_cm = distance_capteur_cm;
capteur_VL53L1X->distance_obstacle_robot_mm = 10 * (distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM);
capteur_VL53L1X->angle_ref_terrain_min = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_terrain - DEMI_CONE_CAPTEUR_RADIAN);
@ -61,8 +55,8 @@ void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct positio
// Positionne l'obstacle sur le terrain
struct position_t position_obstacle;
//printf("Angle:%.1f\n",capteur_VL53L1X->angle_ref_terrain);
position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
position_obstacle.x_mm = position_robot.x_mm + cosf(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
position_obstacle.y_mm = position_robot.y_mm + sinf(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
capteur_VL53L1X->donnee_valide=1;
// Si la distance vaut 0, à invalider
@ -116,12 +110,12 @@ uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
/// * +/- 90°, à 350 mm
/// @param angle_avancement_radiant : angle d'avancement du robot entre -PI et PI
/// @return
double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
float Balise_VL53L1X_get_distance_obstacle_mm(float angle_avancement_radiant){
const uint8_t NB_CONE=3;
uint16_t masque_led=0;
struct cone_t{
double distance_mm;
double angle;
float distance_mm;
float angle, angle_min, angle_max;
} cone[NB_CONE];
cone[0].angle = 22 * DEGRE_EN_RADIAN;
cone[0].distance_mm = 1200;
@ -132,21 +126,21 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
cone[2].angle = 90 * DEGRE_EN_RADIAN;
cone[2].distance_mm = 350;
double angle_min, angle_max;
double distance_minimale = DISTANCE_OBSTACLE_IGNORE_MM;
for(uint8_t cone_index=0; cone_index<NB_CONE; cone_index++){
cone[cone_index].angle_min = Geometrie_get_angle_normalisee( angle_avancement_radiant - cone[cone_index].angle);
cone[cone_index].angle_max = Geometrie_get_angle_normalisee( angle_avancement_radiant + cone[cone_index].angle);
}
float angle_min, angle_max;
float distance_minimale = DISTANCE_OBSTACLE_IGNORE_MM;
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
capteurs_VL53L1X[capteur].donnee_ignore = 1;
for(uint8_t cone_index=0; cone_index<NB_CONE; cone_index++){
/*printf("capteur:%d\n", capteur);
printf("capteur_angle_min:%f\n", capteurs_VL53L1X[capteur].angle_ref_terrain_min);
printf("capteur_angle_max:%f\n", capteurs_VL53L1X[capteur].angle_ref_terrain_max);
printf("cone angel min:%f\n", angle_avancement_radiant - cone[cone_index].angle);
printf("cone angel max:%f\n", angle_avancement_radiant + cone[cone_index].angle);*/
//On test si le capteur détecte dans la plage du cône
if(Geometrie_intersecte_plage_angle(
angle_avancement_radiant - cone[cone_index].angle, angle_avancement_radiant + cone[cone_index].angle,
cone[cone_index].angle_min, cone[cone_index].angle_max,
capteurs_VL53L1X[capteur].angle_ref_terrain_min, capteurs_VL53L1X[capteur].angle_ref_terrain_max)){
// Si l'obstacle est sur le terrain
if(capteurs_VL53L1X[capteur].donnee_valide){

3
Balise_VL53L1X.h
View File

@ -1,6 +1,7 @@
#include "pico/stdlib.h"
void Balise_VL53L1X_init(void);
void Balise_VL53L1X_gestion(void);
uint8_t Balise_VL53L1X_get_min_distance(void);
uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur);
double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant);
float Balise_VL53L1X_get_distance_obstacle_mm(float angle_avancement_radiant);

1
CMakeLists.txt
View File

@ -23,6 +23,7 @@ i2c_maitre.c
i2c_annexe.c
Localisation.c
Moteurs.c
Monitoring.c
Robot_config.c
Servomoteur.c
Strategie.c

8
Commande_vitesse.c
View File

@ -10,8 +10,8 @@
/// @param rotation_rad_s : Rotation en rad/s
/// @param centre_x : centre de rotation (coordonnée X)
/// @param centre_y : centre de rotation (coordonnée Y)
void commande_rotation(double rotation_rad_s, double centre_x, double centre_y){
double vitesse_x_mm_s, vitesse_y_mm_s;
void commande_rotation(float rotation_rad_s, float centre_x, float centre_y){
float vitesse_x_mm_s, vitesse_y_mm_s;
vitesse_x_mm_s = centre_y * rotation_rad_s;
vitesse_y_mm_s = -centre_x * rotation_rad_s;
@ -24,8 +24,8 @@ void commande_rotation(double rotation_rad_s, double centre_x, double centre_y){
/// @param vitesse_x_mm_s : Vitesse x en mm/s dans le référentiel du robot
/// @param vitesse_y_mm_s : Vitesse y en mm/s dans le référentiel du robot
/// @param orientation_radian_s : Rotation en radian/s dans le référentiel du robot
void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s){
double vitesse_roue_a, vitesse_roue_b, vitesse_roue_c;
void commande_vitesse(float vitesse_x_mm_s, float vitesse_y_mm_s, float orientation_radian_s){
float vitesse_roue_a, vitesse_roue_b, vitesse_roue_c;
vitesse_roue_a = vitesse_x_mm_s / 2.0 - vitesse_y_mm_s * RACINE_DE_3 / 2.0 - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s;
vitesse_roue_b = vitesse_x_mm_s / 2.0 + vitesse_y_mm_s * RACINE_DE_3 / 2.0 - DISTANCE_ROUES_CENTRE_MM * orientation_radian_s;

4
Commande_vitesse.h
View File

@ -1,3 +1,3 @@
void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s);
void commande_rotation(double rotation_rad_s, double centre_x, double centre_y);
void commande_vitesse(float vitesse_x_mm_s, float vitesse_y_mm_s, float orientation_radian_s);
void commande_rotation(float rotation_rad_s, float centre_x, float centre_y);
void commande_vitesse_stop(void);

12
Geometrie.c
View File

@ -4,7 +4,7 @@
/// @brief Retourne l'angle entre -PI et +PI
/// @param angle
/// @return
double Geometrie_get_angle_normalisee(double angle){
float Geometrie_get_angle_normalisee(float angle){
while(angle > M_PI){
angle -= 2* M_PI;
}
@ -14,16 +14,12 @@ double Geometrie_get_angle_normalisee(double angle){
return angle;
}
/// @brief Indique si un angle est compris entre deux angles
/// @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(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);
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)){
@ -45,7 +41,7 @@ unsigned int Geometrie_compare_angle(double angle, double angle_min, double angl
/// @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(double angle1_min, double angle1_max, double angle2_min, double angle2_max){
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

10
Geometrie.h
View File

@ -9,12 +9,12 @@
#define DISTANCE_INVALIDE (-1.)
struct position_t{
double x_mm, y_mm;
double angle_radian;
float x_mm, y_mm;
float angle_radian;
};
double Geometrie_get_angle_normalisee(double angle);
unsigned int Geometrie_compare_angle(double angle, double angle_min, double angle_max);
unsigned int Geometrie_intersecte_plage_angle(double angle1_min, double angle1_max, double angle2_min, double angle2_max);
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);
#endif

10
Holonome2023.c
View File

@ -36,8 +36,8 @@ int mode_test();
int main() {
bi_decl(bi_program_description("This is a test binary."));
bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
double vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT;
struct t_angle_gyro_double angle_gyro;
float vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT;
struct t_angle_gyro_float angle_gyro;
uint32_t temps_ms = 0, temps_ms_old;
uint32_t temps_us_debut_cycle;
@ -112,7 +112,7 @@ int main() {
}
temps_cycle = time_us_32() - temps_us_debut_cycle;
//printf("%#x, %#x\n", (double)temps_ms_old / 1000, vitesse_filtre_z);
//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),");
@ -121,9 +121,9 @@ int main() {
// Toutes les 50 ms
if((Temps_get_temps_ms() % 50) == 0){
struct t_angle_gyro_double m_gyro;
struct t_angle_gyro_float m_gyro;
m_gyro = gyro_get_angle_degres();
printf("%f, %f, %d\n", (double)temps_ms / 1000, m_gyro.rot_z, temps_cycle);
printf("%f, %f, %d\n", (float)temps_ms / 1000, m_gyro.rot_z, temps_cycle);
}
// Toutes les 500 ms

24
Localisation.c
View File

@ -13,34 +13,34 @@ void Localisation_init(){
position.angle_radian = 0;
}
void Localisation_set(double x_mm, double y_mm, double angle_radian){
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;
gyro_set_angle_radian(angle_radian);
}
void Localisation_set_x(double x_mm){
void Localisation_set_x(float x_mm){
position.x_mm = x_mm;
}
void Localisation_set_y(double y_mm){
void Localisation_set_y(float y_mm){
position.y_mm = y_mm;
}
void Localisation_set_angle(double angle_radian){
void Localisation_set_angle(float angle_radian){
position.angle_radian = angle_radian;
}
void Localisation_gestion(){
struct t_angle_gyro_double angle_gyro;
struct t_angle_gyro_float angle_gyro;
// Voir http://poivron-robotique.fr/Robot-holonome-localisation-partie-2.html
double distance_roue_a_mm = QEI_get_mm(QEI_A_NAME);
double distance_roue_b_mm = QEI_get_mm(QEI_B_NAME);
double distance_roue_c_mm = QEI_get_mm(QEI_C_NAME);
double delta_x_ref_robot, delta_y_ref_robot;
float distance_roue_a_mm = QEI_get_mm(QEI_A_NAME);
float distance_roue_b_mm = QEI_get_mm(QEI_B_NAME);
float distance_roue_c_mm = QEI_get_mm(QEI_C_NAME);
float delta_x_ref_robot, delta_y_ref_robot;
double old_orientation_radian = position.angle_radian;
float old_orientation_radian = position.angle_radian;
delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm) / 3.0;
delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm) * RACINE_DE_3 / 3.0;
@ -53,8 +53,8 @@ void Localisation_gestion(){
}
// Projection dans le référentiel de la table
position.x_mm += delta_x_ref_robot * cos(position.angle_radian) - delta_y_ref_robot * sin(position.angle_radian);
position.y_mm += delta_x_ref_robot * sin(position.angle_radian) + delta_y_ref_robot * cos(position.angle_radian);
position.x_mm += delta_x_ref_robot * cosf(position.angle_radian) - delta_y_ref_robot * sinf(position.angle_radian);
position.y_mm += delta_x_ref_robot * sinf(position.angle_radian) + delta_y_ref_robot * cosf(position.angle_radian);
}

8
Localisation.h
View File

@ -4,7 +4,7 @@ struct position_t Localisation_get(void);
void Localisation_gestion();
void Localisation_init();
void Localisation_set(double x_mm, double y_mm, double angle_radian);
void Localisation_set_x(double x_mm);
void Localisation_set_y(double y_mm);
void Localisation_set_angle(double angle_radian);
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);

44
Monitoring.c Normal file
View File

@ -0,0 +1,44 @@
#include "pico/stdlib.h"
#include <stdio.h>
uint32_t temps_cycle_min = UINT32_MAX;
uint32_t temps_cycle_max=0;
int lock=0;
void temps_cycle_check(){
static uint32_t temps_old;
uint32_t temps, temps_cycle;
temps = time_us_32();
temps_cycle = temps - temps_old;
if(temps_cycle < temps_cycle_min){
temps_cycle_min = temps_cycle;
}
if(temps_cycle > temps_cycle_max){
temps_cycle_max = temps_cycle;
}
temps_old=time_us_32();
}
void temps_cycle_reset(){
temps_cycle_min = UINT32_MAX;
temps_cycle_max=0;
}
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);
temps_cycle_reset();
}
uint32_t temps_cycle_get_min(){
return temps_cycle_min;
}
uint32_t temps_cycle_get_max(){
return temps_cycle_max;
}

7
Monitoring.h Normal file
View File

@ -0,0 +1,7 @@
#include "pico/stdlib.h"
void temps_cycle_check();
void temps_cycle_reset();
void temps_cycle_display();
uint32_t temps_cycle_get_min();
uint32_t temps_cycle_get_max();

4
QEI.c
View File

@ -109,6 +109,6 @@ int QEI_get(enum QEI_name_t qei){
/// @brief Renvoi la distance parcourue en mm depuis la lecture précédente
/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
/// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update()
double QEI_get_mm(enum QEI_name_t qei){
return (double) QEI_get(qei) / (double)IMPULSION_PAR_MM;
float QEI_get_mm(enum QEI_name_t qei){
return (float) QEI_get(qei) / (float)IMPULSION_PAR_MM;
}

2
QEI.h
View File

@ -14,4 +14,4 @@ extern struct QEI_t QEI_A, QEI_B, QEI_C;
void QEI_update(void);
void QEI_init(void);
int QEI_get(enum QEI_name_t qei);
double QEI_get_mm(enum QEI_name_t qei);
float QEI_get_mm(enum QEI_name_t qei);

8
Strategie.c
View File

@ -17,9 +17,9 @@
#define DISTANCE_PAS_OBSTACLE_MM 2000
// TODO: Peut-être à remetttre en variable locale après
double distance_obstacle;
float distance_obstacle;
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian);
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;
@ -194,7 +194,7 @@ enum etat_action_t lance_balles(uint32_t step_ms){
}
/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian){
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 etat_action = ACTION_EN_COURS;
struct position_t position;
@ -220,7 +220,7 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_trajet_t etat_trajet;
double angle_avancement;
float angle_avancement;
static enum {
PARCOURS_INIT,

5
Strategie.h
View File

@ -6,7 +6,8 @@
#define COULEUR 15
#define TIRETTE 14
#define CORR_ANGLE_DEPART_DEGREE (-1.145)
//#define CORR_ANGLE_DEPART_DEGREE (-1.145)
#define CORR_ANGLE_DEPART_DEGREE (0)
enum etat_action_t{
ACTION_EN_COURS,
@ -56,7 +57,7 @@ int test_panier(void);
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
extern double distance_obstacle;
extern float distance_obstacle;
// STRATEGIE_H
#endif

6
Strategie_prise_cerises.c
View File

@ -25,13 +25,13 @@ void commande_translation_longer_vers_C();
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
double vitesse_accostage_mm_s=100;
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
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, double 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){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum longer_direction_t longer_direction_aspire;
static uint32_t tempo_ms = 0;
@ -157,7 +157,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire
enum etat_action_t cerise_accostage(void){
enum etat_action_t etat_action = ACTION_EN_COURS;
double rotation;
float rotation;
static enum {
CERISE_AVANCE_DROIT,

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, double pos_x_mm);
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm);

49
Test.c
View File

@ -19,6 +19,7 @@
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Monitoring.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Servomoteur.h"
@ -43,7 +44,7 @@ int test_localisation(void);
int test_avance(void);
int test_cde_vitesse(void);
int test_cde_vitesse_rotation(void);
int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm);
int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm);
int test_cde_vitesse_rectangle(void);
int test_cde_vitesse_cercle(void);
int test_asser_position_avance(void);
@ -726,7 +727,7 @@ void test_trajectoire_teleplot(){
int test_aller_retour(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
const double corr_angle = 1.145;
const float corr_angle = 1.145;
Trajet_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
@ -937,8 +938,8 @@ int test_asser_position_avance_et_tourne(int m_gyro){
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
position_consigne.angle_radian = (double) (temps_ms - temps_ms_init) /1000.;
position_consigne.y_mm = (double) (temps_ms - temps_ms_init) * 100. / 1000.;
position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.;
Asser_Position(position_consigne);
@ -965,9 +966,9 @@ int test_asser_position_avance(){
AsserMoteur_Gestion(_step_ms);
if(temps_ms < 5000){
position.y_mm = (double) temps_ms * 100. / 1000.;
position.y_mm = (float) temps_ms * 100. / 1000.;
}else if(temps_ms < 10000){
position.y_mm = 1000 - (double) temps_ms * 100. / 1000.;
position.y_mm = 1000 - (float) temps_ms * 100. / 1000.;
}else{
temps_ms = 0;
}
@ -1026,7 +1027,7 @@ int test_cde_vitesse(){
int test_cde_vitesse_rotation(){
int lettre, _step_ms = 1;
double vitesse =90.0/2 * 3.14159 /180.0;
float vitesse =90.0/2 * 3.14159 /180.0;
printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);
commande_vitesse(0, 0, vitesse);
@ -1042,9 +1043,9 @@ int test_cde_vitesse_rotation(){
}
int test_cde_rotation_ref_robot(double centre_x_mm, double centre_y_mm){
int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm){
int lettre, _step_ms = 1;
double vitesse =90.0/4 * 3.14159 /180.0;
float vitesse =90.0/4 * 3.14159 /180.0;
printf("Rotation du robot par rapport au point (Rayon, O)\nVitesse : %f rad/s\n", vitesse);
commande_rotation(vitesse, centre_x_mm, centre_y_mm);
@ -1095,7 +1096,7 @@ int test_cde_vitesse_cercle(){
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
commande_vitesse(cos((double)temps_ms / 1000.) * 200.0, sin((double)temps_ms /1000.) * 200.0, 0);
commande_vitesse(cos((float)temps_ms / 1000.) * 200.0, sin((float)temps_ms /1000.) * 200.0, 0);
temps_ms += _step_ms;
sleep_ms(_step_ms);
@ -1189,7 +1190,7 @@ int test_QIE(){
int test_QIE_mm(){
int lettre;
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
double a_mm=0, b_mm=0, c_mm=0;
float a_mm=0, b_mm=0, c_mm=0;
do{
QEI_update();
a_mm += QEI_get_mm(QEI_A_NAME);
@ -1354,7 +1355,7 @@ int test_vitesse_moteur(enum t_moteur moteur){
int test_geometrie(){
double angle = 270, angle_min, angle_max;
float 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);
@ -1393,26 +1394,36 @@ int test_geometrie(){
return 0;
}
void affiche_monitoring(){
while(1){
temps_cycle_display();
sleep_ms(100);
}
}
int test_angle_balise(void){
int lettre;
float distance, angle=0;
i2c_maitre_init();
Balise_VL53L1X_init();
Localisation_set(1000,1500,0);
int lettre;
double distance, angle=0;
multicore_launch_core1(affiche_monitoring);
do{
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
distance = Balise_VL53L1X_get_distance_obstacle_mm(angle);
printf(">distance_obstacle:%3.0f\n", distance);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
}while(1);
//}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
return 0;

22
Test_strategie.c
View File

@ -9,6 +9,7 @@
#include "i2c_maitre.h"
#include "gyro.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Strategie.h"
@ -30,6 +31,7 @@ void affichage_test_strategie(){
while(true){
temps = time_us_32()/1000;
temps_cycle_display();
printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
@ -45,7 +47,7 @@ void affichage_test_strategie(){
printf(">tirette:%ld:%d\n", temps, attente_tirette());
printf(">etat_strat:%d\n",etat_strategie);
printf(">etat_strat:%ld:%d\n", temps, etat_strategie);
/*switch(etat_strategie){
case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break;
@ -118,6 +120,7 @@ int test_strategie(){
int test_homologation(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0;
printf("Homologation\n");
i2c_maitre_init();
@ -134,16 +137,31 @@ int test_homologation(){
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
temps_cycle_old= time_us_32();
do{
/*temps_cycle[index_temps_cycle++]= time_us_32() - temps_cycle_old;
if(index_temps_cycle >= 10){
for(int i=0; i<10; i++){
printf("t_cycle:%d\n", temps_cycle[i]);
}
index_temps_cycle=0;
}
temps_cycle_old=time_us_32();*/
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
@ -155,7 +173,7 @@ int test_homologation(){
Homologation(_step_ms);
}
lettre = getchar_timeout_us(0);
//lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(1);
printf("STRATEGIE_LOOP_2\n");

28
Trajectoire.c
View File

@ -10,8 +10,8 @@
#define PRECISION_ABSCISSE 0.001
void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon,
double orientation_debut_rad, double orientation_fin_rad){
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){
trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
trajectoire->p1.x = centre_x;
trajectoire->p1.y = centre_y;
@ -23,8 +23,8 @@ void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x,
trajectoire->orientation_fin_rad = orientation_fin_rad;
}
void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y,
double orientation_debut_rad, double 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){
trajectoire->type = TRAJECTOIRE_DROITE;
trajectoire->p1.x = p1_x;
trajectoire->p1.y = p1_y;
@ -35,8 +35,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double
trajectoire->orientation_fin_rad = orientation_fin_rad;
}
void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y,
double orientation_debut_rad, double orientation_fin_rad){
void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,
float orientation_debut_rad, float orientation_fin_rad){
trajectoire->type = TRAJECTOIRE_BEZIER;
trajectoire->p1.x = p1_x;
trajectoire->p1.y = p1_y;
@ -51,7 +51,7 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double
trajectoire->orientation_fin_rad = orientation_fin_rad;
}
void Trajectoire_rotation(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double orientation_debut_rad, double orientation_fin_rad){
void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad){
trajectoire->type = TRAJECTOIRE_ROTATION;
trajectoire->p1.x = p1_x;
trajectoire->p1.y = p1_y;
@ -91,7 +91,7 @@ void Trajectoire_inverse(struct trajectoire_t * trajectoire){
/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin
/// @param trajectoire
/// @return Longueur de la trajectoire
double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
if(trajectoire->longueur > 0){
// La longueur est déjà calculée
}else{
@ -115,7 +115,7 @@ double 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, double abscisse){
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse){
struct point_xyo_t point_xyo;
switch(trajectoire->type){
case TRAJECTOIRE_DROITE:
@ -141,16 +141,16 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
return point_xyo;
}
double Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, double abscisse){
return (double) trajectoire->orientation_debut_rad * (1-abscisse) + (double) trajectoire->orientation_fin_rad * abscisse;
float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse){
return (float) trajectoire->orientation_debut_rad * (1-abscisse) + (float) trajectoire->orientation_fin_rad * abscisse;
}
/// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée
/// @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
double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){
double delta_abscisse, delta_mm, erreur_relative;
float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm){
float delta_abscisse, delta_mm, erreur_relative;
if(distance_mm == 0){
return abscisse;
@ -175,7 +175,7 @@ double Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, d
return abscisse + delta_abscisse;
}
double distance_points(struct point_xy_t point, struct point_xy_t point_old){
float 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));
}

32
Trajectoire.h
View File

@ -9,33 +9,33 @@ enum trajectoire_type_t{
};
struct point_xy_t{
double x, y;
float x, y;
};
struct point_xyo_t{
struct point_xy_t point_xy;
double orientation;
float orientation;
};
struct trajectoire_t {
enum trajectoire_type_t type;
struct point_xy_t p1, p2, p3, p4;
double orientation_debut_rad, orientation_fin_rad;
double rayon, angle_debut_degre, angle_fin_degre;
double longueur;
float orientation_debut_rad, orientation_fin_rad;
float rayon, angle_debut_degre, angle_fin_degre;
float longueur;
};
double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse);
double Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, double abscisse);
double 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, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre,
double rayon, double orientation_debut_rad, double orientation_fin_rad);
void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double orientation_debut_rad, double orientation_fin_rad);
void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y,
double orientation_debut_rad, double orientation_fin_rad);
void Trajectoire_rotation(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double orientation_debut_rad, double orientation_fin_rad);
float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float 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);
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);
void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,
float orientation_debut_rad, float orientation_fin_rad);
void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad);
void Trajectoire_inverse(struct trajectoire_t * trajectoire);
#endif

22
Trajectoire_bezier.c
View File

@ -4,12 +4,12 @@
void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){
struct point_xy_t point, point_old;
double nb_pas=500;
float nb_pas=500;
trajectoire->longueur=0;
point_old = trajectoire->p1;
for(double abscisse=0; abscisse<=1; abscisse += 1./nb_pas){
for(float abscisse=0; abscisse<=1; abscisse += 1./nb_pas){
point = Trajectoire_bezier_get_point(trajectoire, abscisse);
trajectoire->longueur += distance_points(point, point_old);
point_old = point;
@ -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, double abscisse){
struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse){
struct point_xy_t point;
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.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.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;
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;
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, double abscisse);
struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse);

8
Trajectoire_circulaire.c
View File

@ -3,7 +3,7 @@
void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){
double distance_angulaire;
float distance_angulaire;
if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){
distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre;
}else{
@ -14,11 +14,11 @@ void Trajectoire_circulaire_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_circulaire_get_point(struct trajectoire_t * trajectoire, double abscisse){
struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){
struct point_xy_t point;
double angle_degre;
float angle_degre;
angle_degre = (double) trajectoire->angle_debut_degre * (1-abscisse) + (double) trajectoire->angle_fin_degre * abscisse;
angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse;
point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon;
point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon;

2
Trajectoire_circulaire.h
View File

@ -1,4 +1,4 @@
#include "math.h"
void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire);
struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, double avancement);
struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float avancement);

6
Trajectoire_droite.c
View File

@ -7,11 +7,11 @@ void Trajectoire_droite_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_droite_get_point(struct trajectoire_t * trajectoire, double abscisse){
struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse){
struct point_xy_t point;
point.x = (double) trajectoire->p1.x * (1. - abscisse) + (double) trajectoire->p2.x * abscisse;
point.y = (double) trajectoire->p1.y * (1. - abscisse) + (double) trajectoire->p2.y * abscisse;
point.x = (float) trajectoire->p1.x * (1. - abscisse) + (float) trajectoire->p2.x * abscisse;
point.y = (float) trajectoire->p1.y * (1. - abscisse) + (float) trajectoire->p2.y * abscisse;
return point;
}

2
Trajectoire_droite.h
View File

@ -1,4 +1,4 @@
#include "Trajectoire.h"
void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire);
struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, double abscisse);
struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse);

2
Trajectoire_rotation.c
View File

@ -11,6 +11,6 @@ void Trajectoire_rotation_get_longueur(struct trajectoire_t * trajectoire){
}
// Le robot reste sur place
struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, double abscisse){
struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, float abscisse){
return trajectoire->p1;
}

2
Trajectoire_rotation.h
View File

@ -1,4 +1,4 @@
#include "Trajectoire.h"
void Trajectoire_rotation_get_longueur(struct trajectoire_t * trajectoire);
struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, double abscisse);
struct point_xy_t Trajectoire_rotation_get_point(struct trajectoire_t * trajectoire, float abscisse);

50
Trajet.c
View File

@ -4,20 +4,20 @@
#include "Trajet.h"
#include "Asser_Position.h"
double Trajet_calcul_vitesse(double temps_s);
int Trajet_terminee(double abscisse);
float Trajet_calcul_vitesse(float temps_s);
int Trajet_terminee(float abscisse);
double abscisse; // Position entre 0 et 1 sur la trajectoire
double position_mm; // Position en mm sur la trajectoire
double vitesse_mm_s;
double vitesse_max_trajet_mm_s=500;
double acceleration_mm_ss;
const double acceleration_mm_ss_obstacle = 1500;
float abscisse; // Position entre 0 et 1 sur la trajectoire
float position_mm; // Position en mm sur la trajectoire
float vitesse_mm_s;
float vitesse_max_trajet_mm_s=500;
float acceleration_mm_ss;
const float acceleration_mm_ss_obstacle = 1500;
struct trajectoire_t trajet_trajectoire;
struct position_t position_consigne;
double distance_obstacle_mm;
const double distance_pas_obstacle = 2000;
float distance_obstacle_mm;
const float distance_pas_obstacle = 2000;
/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
void Trajet_init(){
@ -30,7 +30,7 @@ void Trajet_init(){
/// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets
/// @param _vitesse_max_trajet_mm_s
/// @param _acceleration_mm_ss
void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss){
void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss){
vitesse_max_trajet_mm_s = _vitesse_max_trajet_mm_s;
acceleration_mm_ss = _acceleration_mm_ss;
}
@ -45,8 +45,8 @@ 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
enum etat_trajet_t Trajet_avance(double pas_de_temps_s){
double distance_mm;
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;
@ -79,7 +79,7 @@ enum etat_trajet_t Trajet_avance(double pas_de_temps_s){
}
void Trajet_stop(double pas_de_temps_s){
void Trajet_stop(float pas_de_temps_s){
vitesse_mm_s = 0;
Trajet_avance(0);
}
@ -88,7 +88,7 @@ void Trajet_stop(double pas_de_temps_s){
/// où les approximations font que l'abscisse peut ne pas atteindre 1.
/// @param abscisse : abscisse sur la trajectoire
/// @return 1 si le trajet est terminé, 0 sinon
int Trajet_terminee(double abscisse){
int Trajet_terminee(float abscisse){
/*if(abscisse >= 0.99 ){
return 1;
}*/
@ -113,11 +113,11 @@ struct position_t Trajet_get_consigne(){
/// @brief Calcule la vitesse à partir de l’accélération du robot, de la vitesse maximale et de la contrainte en fin de trajectoire
/// @param pas_de_temps_s : temps écoulé en ms
/// @return vitesse déterminée en m/s
double Trajet_calcul_vitesse(double pas_de_temps_s){
double vitesse_max_contrainte;
double vitesse_max_contrainte_obstacle;
double distance_contrainte,distance_contrainte_obstacle;
double vitesse;
float Trajet_calcul_vitesse(float pas_de_temps_s){
float vitesse_max_contrainte;
float vitesse_max_contrainte_obstacle;
float distance_contrainte,distance_contrainte_obstacle;
float vitesse;
// Calcul de la vitesse avec acceleration
vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s;
@ -151,21 +151,21 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){
}
double Trajet_get_obstacle_mm(void){
float Trajet_get_obstacle_mm(void){
return distance_obstacle_mm;
}
void Trajet_set_obstacle_mm(double distance_mm){
void Trajet_set_obstacle_mm(float distance_mm){
distance_obstacle_mm = distance_mm;
}
/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain
/// @return angle en radian.
double Trajet_get_orientation_avance(){
float Trajet_get_orientation_avance(){
struct point_xyo_t point, point_suivant;
double avance_abscisse = 0.01;
double angle;
float avance_abscisse = 0.01;
float angle;
if(abscisse >= 1){
return 0;

14
Trajet.h
View File

@ -12,14 +12,14 @@ enum etat_trajet_t{
// Vitesse et acceleration pour une rotation (rad/s et rad/s²)
#define TRAJECT_CONFIG_ROTATION_PURE 2, 2
extern const double distance_pas_obstacle;
extern const float distance_pas_obstacle;
void Trajet_init();
void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss);
void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss);
void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
enum etat_trajet_t Trajet_avance(double temps_s);
enum etat_trajet_t Trajet_avance(float temps_s);
struct position_t Trajet_get_consigne(void);
double Trajet_get_obstacle_mm(void);
void Trajet_set_obstacle_mm(double distance_mm);
void Trajet_stop(double);
double Trajet_get_orientation_avance(void);
float Trajet_get_obstacle_mm(void);
void Trajet_set_obstacle_mm(float distance_mm);
void Trajet_stop(float);
float Trajet_get_orientation_avance(void);

15
gyro.c
View File

@ -3,6 +3,7 @@
#include "hardware/gpio.h"
#include "hardware/spi.h"
#include "hardware/structs/spi.h"
#include "Geometrie.h"
#include "spi_nb.h"
#include "Temps.h"
#include "gyro.h"
@ -25,8 +26,8 @@
/// @brief structure d'échange des angles du gyrocope
struct t_angle_gyro _vitesse_calibration;
struct t_angle_gyro *vitesse_calibration;
struct t_angle_gyro_double _vitesse_angulaire;
struct t_angle_gyro_double *vitesse_angulaire;
struct t_angle_gyro_float _vitesse_angulaire;
struct t_angle_gyro_float *vitesse_angulaire;
int gyro_read_register_blocking(uint8_t registrer, uint8_t *tampon, uint8_t nb_a_lire);
void gyro_calibration(void);
@ -35,16 +36,16 @@ uint32_t rot_x_zero, rot_y_zero, rot_z_zero;
struct t_angle_gyro_double angle_gyro, vitesse_gyro;
struct t_angle_gyro_float angle_gyro, vitesse_gyro;
void gyro_set_angle_radian(double angle_radian){
void gyro_set_angle_radian(float angle_radian){
angle_gyro.rot_z = angle_radian * RADIAN_VERS_DEGRES;
}
struct t_angle_gyro_double gyro_get_angle_degres(void){
struct t_angle_gyro_float gyro_get_angle_degres(void){
return angle_gyro;
}
struct t_angle_gyro_double gyro_get_vitesse(void){
struct t_angle_gyro_float gyro_get_vitesse(void){
return vitesse_gyro;
}
@ -121,7 +122,7 @@ int16_t gyro_get_temp(void){
}
void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre){
void gyro_affiche(struct t_angle_gyro_float angle_gyro, char * titre){
if(titre != NULL){
printf("%s ",titre);
}

8
gyro.h
View File

@ -2,8 +2,8 @@
void Gyro_Init(void);
void Gyro_Read(uint16_t);
void gyro_affiche(struct t_angle_gyro_double angle_gyro, char * titre);
void gyro_set_angle_radian(double angle_radian);
struct t_angle_gyro_double gyro_get_angle_degres(void);
struct t_angle_gyro_double gyro_get_vitesse(void);
void gyro_affiche(struct t_angle_gyro_float angle_gyro, char * titre);
void gyro_set_angle_radian(float angle_radian);
struct t_angle_gyro_float gyro_get_angle_degres(void);
struct t_angle_gyro_float gyro_get_vitesse(void);
int16_t gyro_get_temp(void);

8
gyro_ADXRS453.c
View File

@ -176,10 +176,10 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro
}
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire,
struct t_angle_gyro_double * _vitesse_gyro){
_vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.0125 / 32.0;
_vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.0125 / 32.0;
_vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.0125 / 32.0 * 360. / 357.; // Gain mesuré
struct t_angle_gyro_float * _vitesse_gyro){
_vitesse_gyro->rot_x = (float)_vitesse_angulaire->rot_x * 0.0125 / 32.0;
_vitesse_gyro->rot_y = (float)_vitesse_angulaire->rot_y * 0.0125 / 32.0;
_vitesse_gyro->rot_z = (float)_vitesse_angulaire->rot_z * 0.0125 / 32.0 * 360. / 357.; // Gain mesuré
}

2
gyro_ADXRS453.h
View File

@ -3,4 +3,4 @@
int gyro_init_check();
int gyro_config();
void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro);
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_float * vitesse_gyro);

8
gyro_L3GD20H.c
View File

@ -91,10 +91,10 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro
}
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire,
struct t_angle_gyro_double * _vitesse_gyro){
_vitesse_gyro->rot_x = (double)_vitesse_angulaire->rot_x * 0.00875 / 32.0;
_vitesse_gyro->rot_y = (double)_vitesse_angulaire->rot_y * 0.00875 / 32.0;
_vitesse_gyro->rot_z = (double)_vitesse_angulaire->rot_z * 0.00875 / 32.0;
struct t_angle_gyro_float * _vitesse_gyro){
_vitesse_gyro->rot_x = (float)_vitesse_angulaire->rot_x * 0.00875 / 32.0;
_vitesse_gyro->rot_y = (float)_vitesse_angulaire->rot_y * 0.00875 / 32.0;
_vitesse_gyro->rot_z = (float)_vitesse_angulaire->rot_z * 0.00875 / 32.0;
}
#endif

2
gyro_L3GD20H.h
View File

@ -3,4 +3,4 @@
int gyro_init_check();
int gyro_config();
void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro* angle_gyro_moy);
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_double * vitesse_gyro);
void gyro_get_vitesse_normalisee(struct t_angle_gyro* _vitesse_angulaire, struct t_angle_gyro_float * vitesse_gyro);

4
gyro_data.h
View File

@ -3,8 +3,8 @@
#ifndef GYRO_DATA_H
#define GYRO_DATA_H
struct t_angle_gyro_double{
double rot_x, rot_y, rot_z;
struct t_angle_gyro_float{
float rot_x, rot_y, rot_z;
};
struct t_angle_gyro{

2
i2c_annexe.c
View File

@ -73,8 +73,10 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){
}
void i2c_annexe_active_turbine(void){
/*
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
donnees_a_envoyer=1;
*/
}
void i2c_annexe_desactive_turbine(void){
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;

2
i2c_maitre.c
View File

@ -185,7 +185,7 @@ enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission,
I2C_nb_a_recevoir = nb_reception;
// On appelle la fonction gestion pour gagner du temps.
i2c_gestion(i2c0);
//i2c_gestion(i2c0);
m_statu = I2C_STATU_EN_COURS;
break;