Compare commits
10 Commits
8db2e226b8
...
bec6d622f0
| Author | SHA1 | Date | |
|---|---|---|---|
| bec6d622f0 | |||
| ff2996f0e6 | |||
| 5f4cacf2d1 | |||
| 23bc31cfac | |||
| bda33196ec | |||
| 71ccb57220 | |||
| bc7e947823 | |||
| 1a72e92a03 | |||
| 4a6e655067 | |||
| d6c64fb05d |
5
.vscode/settings.json
vendored
5
.vscode/settings.json
vendored
@ -13,6 +13,9 @@
|
||||
"stdlib.h": "c",
|
||||
"strategie.h": "c",
|
||||
"strategie_deplacement.h": "c",
|
||||
"servomoteur.h": "c"
|
||||
"servomoteur.h": "c",
|
||||
"moteurs.h": "c",
|
||||
"messagerie_applicative.h": "c",
|
||||
"config_robot.h": "c"
|
||||
}
|
||||
}
|
||||
@ -1,9 +1,20 @@
|
||||
#include "config_robot.h"
|
||||
#include "QEI.h"
|
||||
#include "Moteurs.h"
|
||||
#include "Asser_Moteurs.h"
|
||||
|
||||
#define ASSERMOTEUR_GAIN_P 300000.f
|
||||
#define ASSERMOTEUR_GAIN_I 30000.f
|
||||
// Paramètres pour PAMI
|
||||
#ifdef ROBOT_TYPE_PAMI
|
||||
#define ASSERMOTEUR_GAIN_P 30000.f
|
||||
#define ASSERMOTEUR_GAIN_I 3000.f
|
||||
#endif
|
||||
|
||||
// Paramètre Robot 2026
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
#define ASSERMOTEUR_GAIN_P 150.f
|
||||
#define ASSERMOTEUR_GAIN_I 1.f
|
||||
#endif
|
||||
|
||||
|
||||
float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
|
||||
float commande_I[3]; // Terme integral
|
||||
|
||||
@ -2,8 +2,8 @@
|
||||
#include "Commande_vitesse.h"
|
||||
#include "math.h"
|
||||
|
||||
#define GAIN_P_POSITION 15
|
||||
#define GAIN_P_ORIENTATION 10
|
||||
#define GAIN_P_POSITION 5
|
||||
#define GAIN_P_ORIENTATION 5
|
||||
|
||||
#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
|
||||
|
||||
|
||||
@ -3,14 +3,23 @@
|
||||
#include "Commande_vitesse.h"
|
||||
|
||||
|
||||
float avance_mm_s, orientation_radian_s;
|
||||
|
||||
float get_avance_mm_s(){
|
||||
return avance_mm_s;
|
||||
}
|
||||
|
||||
float get_orientation_radian_s(){
|
||||
return orientation_radian_s;
|
||||
}
|
||||
|
||||
/// @brief Commande de la vitesse dans le référentiel du robot
|
||||
/// @param avance_mm_s : Vitesse d'avance
|
||||
/// @param orientation_radian_s : Rotation en radian/s
|
||||
void commande_vitesse(float avance_mm_s, float orientation_radian_s){
|
||||
void commande_vitesse(float _avance_mm_s, float _orientation_radian_s){
|
||||
float vitesse_roue_gauche, vitesse_roue_droite;
|
||||
avance_mm_s = _avance_mm_s;
|
||||
orientation_radian_s = _orientation_radian_s;
|
||||
|
||||
vitesse_roue_gauche = avance_mm_s - (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM);
|
||||
vitesse_roue_droite = avance_mm_s + (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM);
|
||||
|
||||
@ -1,2 +1,4 @@
|
||||
void commande_vitesse(float vitesse_avance_mm_s, float orientation_radian_s);
|
||||
void commande_vitesse_stop(void);
|
||||
void commande_vitesse_stop(void);
|
||||
float get_avance_mm_s();
|
||||
float get_orientation_radian_s();
|
||||
21
Moteurs.c
21
Moteurs.c
@ -1,3 +1,4 @@
|
||||
#include "config_robot.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "Moteurs.h"
|
||||
|
||||
@ -90,23 +91,31 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
|
||||
switch(moteur){
|
||||
case MOTEUR_A:
|
||||
pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
if(vitesse > 0){
|
||||
gpio_put(M1_SENS1, 0);
|
||||
gpio_put(M1_SENS2, 1);
|
||||
}else{
|
||||
#else
|
||||
if(vitesse < 0){
|
||||
#endif
|
||||
gpio_put(M1_SENS1, 1);
|
||||
gpio_put(M1_SENS2, 0);
|
||||
}else{
|
||||
gpio_put(M1_SENS1, 0);
|
||||
gpio_put(M1_SENS2, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
case MOTEUR_B:
|
||||
pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
if(vitesse < 0){
|
||||
gpio_put(M2_SENS1, 0);
|
||||
gpio_put(M2_SENS2, 1);
|
||||
}else{
|
||||
#else
|
||||
if(vitesse > 0){
|
||||
#endif
|
||||
gpio_put(M2_SENS1, 1);
|
||||
gpio_put(M2_SENS2, 0);
|
||||
}else{
|
||||
gpio_put(M2_SENS1, 0);
|
||||
gpio_put(M2_SENS2, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
21
QEI.c
21
QEI.c
@ -1,3 +1,4 @@
|
||||
#include "config_robot.h"
|
||||
#include <stdio.h>
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pio.h"
|
||||
@ -19,6 +20,7 @@
|
||||
|
||||
#define IMPULSION_PAR_MM_50_1 (12.45f)
|
||||
#define IMPULSION_PAR_MM_30_1 (7.47f)
|
||||
#define IMPULSION_PAR_MM_robot_2026 (7.72f)
|
||||
|
||||
float impulsion_par_mm;
|
||||
|
||||
@ -41,19 +43,30 @@ void QEI_init(int identifiant){
|
||||
printf("PIO init error: offset != 0");
|
||||
}
|
||||
// Initialisation des "machines à états" :
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
// QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 14 et 15, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 0, offset, 14, 0);
|
||||
// QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 16 et 17, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 1, offset, 16, 0);
|
||||
#else
|
||||
// QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 0, offset, 11, 0);
|
||||
quadrature_encoder_program_init(pio_QEI, 0, offset, 14, 0);
|
||||
// QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0);
|
||||
quadrature_encoder_program_init(pio_QEI, 1, offset, 16, 0);
|
||||
#endif
|
||||
|
||||
QEI_A.value=0;
|
||||
QEI_B.value=0;
|
||||
QEI_est_init=true;
|
||||
}
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
impulsion_par_mm = IMPULSION_PAR_MM_robot_2026;
|
||||
#else
|
||||
impulsion_par_mm = IMPULSION_PAR_MM_50_1;
|
||||
if(identifiant == 0 || identifiant >= 4){
|
||||
impulsion_par_mm = IMPULSION_PAR_MM_30_1;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
@ -85,7 +98,11 @@ int QEI_get(enum QEI_name_t qei){
|
||||
break;
|
||||
|
||||
case QEI_B_NAME:
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
return QEI_B.delta;
|
||||
#else
|
||||
return -QEI_B.delta;
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
206
Readme.md
206
Readme.md
@ -34,9 +34,6 @@ Pas de trajectoire rotation sur soi-même ?
|
||||
Annexes
|
||||
=======
|
||||
|
||||
Tables des registres
|
||||
--------------------
|
||||
|
||||
|
||||
Table des registres :
|
||||
---------------------
|
||||
@ -45,90 +42,123 @@ Les adresses en **gras** sont celles pour lesquelles des `#define` existent qui
|
||||
|
||||
| Adresse | R/W | Type | Donnée |
|
||||
| ------- | --- | ---------------- | ------ |
|
||||
| **0x00** | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x01 | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x02 | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x03 | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x04 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x05 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x06 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x07 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x08 | RW | flottant 32 bits | Orientation en radian |
|
||||
| 0x09 | RW | flottant 32 bits | Orientation en radian |
|
||||
| 0x0A | RW | flottant 32 bits | Orientation en radian |
|
||||
| 0x0B | RW | flottant 32 bits | Orientation en radian |
|
||||
| **0x0C** | R | Boolean | Mouvement en cours : 1<br>Mouvement terminé : 0 |
|
||||
| **0x00** | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x01 | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x02 | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x03 | RW | flottant 32 bits | Position X en mm |
|
||||
| 0x04 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x05 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x06 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x07 | RW | flottant 32 bits | Position Y en mm |
|
||||
| 0x08 | RW | flottant 32 bits | Orientation en radian |
|
||||
| 0x09 | RW | flottant 32 bits | Orientation en radian |
|
||||
| 0x0A | RW | flottant 32 bits | Orientation en radian |
|
||||
| 0x0B | RW | flottant 32 bits | Orientation en radian |
|
||||
| **0x0C** | R | Boolean | Mouvement en cours : 1<br>Mouvement terminé : |
|
||||
| **0x0D** | RW | uint8_t | Mode<br>0 : arrêt des moteurs<br>1 : pilotage des moteurs PWM<br>2 : pilotage en vitesse des roues<br>3 : pilotage avec la loi de commande<br>4 : Trajectoire |
|
||||
| **0x0E** | RW | int_16 | Commande PWM moteur gauche |
|
||||
| 0x0F | RW | int_16 | Commande PWM moteur gauche |
|
||||
| 0x10 | RW | int_16 | Commande PWM moteur droit |
|
||||
| 0x11 | RW | int_16 | Commande PWM moteur droit |
|
||||
| **0x12** | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x13 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x14 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x15 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x16 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x17 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x18 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x19 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s |
|
||||
| 0x1A | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1B | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1C | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1D | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1E | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| 0x1F | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| 0x20 | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| 0x21 | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| **0x0E** | RW | int_16 | Commande PWM moteur gauche |
|
||||
| 0x0F | RW | int_16 | Commande PWM moteur gauche |
|
||||
| 0x10 | RW | int_16 | Commande PWM moteur droit |
|
||||
| 0x11 | RW | int_16 | Commande PWM moteur droit |
|
||||
| **0x12** | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x13 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x14 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x15 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x16 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x17 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x18 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x19 | RW | Flottant 32 bits | Consigne vitesse gauche en mm/s|
|
||||
| 0x1A | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1B | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1C | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1D | RW | Flottant 32 bits | Consigne d’avance du robot |
|
||||
| 0x1E | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| 0x1F | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| 0x20 | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| 0x21 | RW | Flottant 32 bits | Consigne de rotation du robot |
|
||||
| **0x22** | RW | Enum 8 bits | Trajectoire, type :<br>0 : Droite<br>1 : Circulaire<br>2 : Bézier<br>3 : Composée<br>4 : Rotation |
|
||||
| 0x23 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x24 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x25 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x26 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x27 | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x28 | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x29 | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x2A | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x2B | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2C | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2D | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2E | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2F | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x30 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x31 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x32 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x33 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x34 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x35 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x36 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x37 | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x38 | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x39 | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x3A | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x3B | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3C | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3D | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3E | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3F | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x40 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x41 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x42 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x43 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x44 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x45 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x46 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x47 | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x48 | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x49 | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x4A | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x4B | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4C | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4D | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4E | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4F | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x50 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x51 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x52 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x53 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x54 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x55 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x56 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x23 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x24 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x25 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x26 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x27 | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x28 | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x29 | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x2A | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x2B | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2C | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2D | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2E | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x2F | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x30 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x31 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x32 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x33 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x34 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x35 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x36 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x37 | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x38 | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x39 | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x3A | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x3B | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3C | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3D | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3E | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x3F | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x40 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x41 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x42 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x43 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x44 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x45 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x46 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x47 | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x48 | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x49 | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x4A | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x4B | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4C | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4D | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4E | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x4F | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x50 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x51 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x52 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x53 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x54 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x55 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x56 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| **0x80** | R | flottant 32 bits | Position X en mm |
|
||||
| 0x81 | R | flottant 32 bits | Position X en mm |
|
||||
| 0x82 | R | flottant 32 bits | Position X en mm |
|
||||
| 0x83 | R | flottant 32 bits | Position X en mm |
|
||||
| 0x84 | R | flottant 32 bits | Position Y en mm |
|
||||
| 0x85 | R | flottant 32 bits | Position Y en mm |
|
||||
| 0x86 | R | flottant 32 bits | Position Y en mm |
|
||||
| 0x87 | R | flottant 32 bits | Position Y en mm |
|
||||
| 0x88 | R | flottant 32 bits | Orientation en radian |
|
||||
| 0x89 | R | flottant 32 bits | Orientation en radian |
|
||||
| 0x8A | R | flottant 32 bits | Orientation en radian |
|
||||
| 0x8B | R | flottant 32 bits | Orientation en radian |
|
||||
| **0x8C** | R | Flottant 32 bits | abscisse |
|
||||
| 0x8D | R | Flottant 32 bits | abscisse |
|
||||
| 0x8E | R | Flottant 32 bits | abscisse |
|
||||
| 0x8F | R | Flottant 32 bits | abscisse |
|
||||
| **0x90** | R | flottant 32 bits | Position consigne X en mm |
|
||||
| 0x91 | R | flottant 32 bits | Position consigne X en mm |
|
||||
| 0x92 | R | flottant 32 bits | Position consigne X en mm |
|
||||
| 0x93 | R | flottant 32 bits | Position consigne X en mm |
|
||||
| 0x94 | R | flottant 32 bits | Position consigne Y en mm |
|
||||
| 0x95 | R | flottant 32 bits | Position consigne Y en mm |
|
||||
| 0x96 | R | flottant 32 bits | Position consigne Y en mm |
|
||||
| 0x97 | R | flottant 32 bits | Position consigne Y en mm |
|
||||
| **0x98** | R | flottant 32 bits | Vitesse roue gauche en mm/s |
|
||||
| 0x99 | R | flottant 32 bits | Vitesse roue gauche en mm/s |
|
||||
| 0x9A | R | flottant 32 bits | Vitesse roue gauche en mm/s |
|
||||
| 0x9B | R | flottant 32 bits | Vitesse roue gauche en mm/s |
|
||||
| 0x9C | R | flottant 32 bits | Vitesse roue droite en mm/s |
|
||||
| 0x9D | R | flottant 32 bits | Vitesse roue droite en mm/s |
|
||||
| 0x9E | R | flottant 32 bits | Vitesse roue droite en mm/s |
|
||||
| 0x9F | R | flottant 32 bits | Vitesse roue droite en mm/s |
|
||||
| **0xFF** | R | int_8 | Identifiant de la carte : 'P' |
|
||||
@ -4,10 +4,10 @@
|
||||
#define NB_MAX_TRAJECTOIRE 10
|
||||
|
||||
enum trajectoire_type_t{
|
||||
TRAJECTOIRE_DROITE,
|
||||
TRAJECTOIRE_CIRCULAIRE,
|
||||
TRAJECTOIRE_BEZIER,
|
||||
TRAJECTOIRE_COMPOSEE
|
||||
TRAJECTOIRE_DROITE=0,
|
||||
TRAJECTOIRE_CIRCULAIRE=1,
|
||||
TRAJECTOIRE_BEZIER=2,
|
||||
TRAJECTOIRE_COMPOSEE=3
|
||||
};
|
||||
|
||||
struct point_xy_t{
|
||||
|
||||
2
Trajet.h
2
Trajet.h
@ -14,7 +14,7 @@ enum etat_trajet_t{
|
||||
#define TRAJECT_CONFIG_RAPIDE_ROUGE 500, 1200
|
||||
// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
|
||||
#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
|
||||
// Vitesse et acceleration - standard (en mm et mm/s²)
|
||||
// Vitesse et acceleration - standard (en mm/s et mm/s²)
|
||||
#define TRAJECT_CONFIG_STD 300, 300
|
||||
// Vitesse et acceleration pour une rotation (rad/s et rad/s²)
|
||||
#define TRAJECT_CONFIG_ROTATION_PURE 2, 2
|
||||
|
||||
16
config_robot.h
Normal file
16
config_robot.h
Normal file
@ -0,0 +1,16 @@
|
||||
// Changer le define en fonction qu'on soit sur les PAMIs ou sur le robot principal
|
||||
|
||||
#define ROBOT_PROPULSION_2026
|
||||
//#define ROBOT_TYPE_PAMI
|
||||
|
||||
#ifndef ROBOT_PROPULSION_2026
|
||||
#ifndef ROBOT_TYPE_PAMI
|
||||
#error "Vous devez choisir un type de robot"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
#ifdef ROBOT_TYPE_PAMI
|
||||
#error "Vous devez choisir un seul type de robot"
|
||||
#endif
|
||||
#endif
|
||||
92
main.c
92
main.c
@ -3,6 +3,7 @@
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
*/
|
||||
#include "config_robot.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "pico/multicore.h"
|
||||
#include "hardware/adc.h"
|
||||
@ -47,7 +48,11 @@ int get_couleur(void);
|
||||
void gestion_PAMI(uint32_t step_ms, int * asser_pos);
|
||||
void gestion_VL53L8CX(void);
|
||||
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
const uint32_t step_ms=10;
|
||||
#else
|
||||
const uint32_t step_ms=1;
|
||||
#endif
|
||||
float distance1_mm=0, distance2_mm=0;
|
||||
|
||||
// DEBUG
|
||||
@ -114,6 +119,7 @@ void main(void)
|
||||
|
||||
|
||||
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
|
||||
struct trajectoire_t trajectoire;
|
||||
uint8_t mode=0;
|
||||
|
||||
while(1){
|
||||
@ -137,7 +143,7 @@ void main(void)
|
||||
struct message_applicatif_t message_applicatif;
|
||||
|
||||
memcpy(&message_applicatif, message.donnees, message.taille_donnees);
|
||||
printf("id_carte:%c %d\n", message_applicatif.id_carte, message_applicatif.id_carte);
|
||||
//printf("id_carte:%c %d\n", message_applicatif.id_carte, message_applicatif.id_carte);
|
||||
|
||||
|
||||
if(message_applicatif.id_carte == 'P'){
|
||||
@ -158,12 +164,12 @@ void main(void)
|
||||
if(mise_a_jour_position){
|
||||
struct msg_propulsion_position_t msg_propulsion_position;
|
||||
get_données_reçues((uint8_t *) &msg_propulsion_position, sizeof(msg_propulsion_position), REG_PROPULSION_POSITION);
|
||||
printf("mise à jour de la position, x:%2.2f, y:%2.2f, orientation: %2.2f\n", msg_propulsion_position.position_x_mm,
|
||||
msg_propulsion_position.position_y_mm, msg_propulsion_position.orientation_rad);
|
||||
//printf("mise à jour de la position, x:%2.2f, y:%2.2f, orientation: %2.2f\n", msg_propulsion_position.position_x_mm,
|
||||
// msg_propulsion_position.position_y_mm, msg_propulsion_position.orientation_rad);
|
||||
}
|
||||
if(mise_a_jour_mode){
|
||||
get_données_reçues(&mode, sizeof(mode), REG_PROPULSION_MODE);
|
||||
printf("mode:%d\n", mode);
|
||||
//rintf("mode:%d\n", mode);
|
||||
}
|
||||
if(mise_a_jour_pwm){
|
||||
struct msg_propulsion_pwm_t msg_propulsion_pwm;
|
||||
@ -176,19 +182,30 @@ void main(void)
|
||||
if(mise_a_jour_vitesse_roues){
|
||||
struct msg_propulsion_vitesse_roues_t msg_propulsion_vitesse_roues;
|
||||
get_données_reçues((uint8_t *) &msg_propulsion_vitesse_roues, sizeof(msg_propulsion_vitesse_roues), REG_PROPULSION_VITESSE_ROUES);
|
||||
printf("vitesse_roue_gauche:%f, vitesse_roue_droite:%f\n", msg_propulsion_vitesse_roues.vitesse_gauche_mm_s, msg_propulsion_vitesse_roues.vitesse_droite_mm_s);
|
||||
//printf("vitesse_roue_gauche:%f, vitesse_roue_droite:%f\n", msg_propulsion_vitesse_roues.vitesse_gauche_mm_s, msg_propulsion_vitesse_roues.vitesse_droite_mm_s);
|
||||
AsserMoteur_setConsigne_mm_s(MOTEUR_A, msg_propulsion_vitesse_roues.vitesse_droite_mm_s);
|
||||
AsserMoteur_setConsigne_mm_s(MOTEUR_B, msg_propulsion_vitesse_roues.vitesse_gauche_mm_s);
|
||||
}
|
||||
if(mise_a_jour_vitesse_robot){
|
||||
struct msg_propulsion_vitesse_robot_t msg_propulsion_vitesse_robot;
|
||||
get_données_reçues((uint8_t *) &msg_propulsion_vitesse_robot, sizeof(msg_propulsion_vitesse_robot), REG_PROPULSION_VITESSE_ROBOT);
|
||||
printf("avance_robot (mm/s) : %f, rotation (rad/s): %f\n", msg_propulsion_vitesse_robot.avance_mm_s,
|
||||
msg_propulsion_vitesse_robot.rotation_rad_s);
|
||||
//printf("avance_robot (mm/s) : %f, rotation (rad/s): %f\n", msg_propulsion_vitesse_robot.avance_mm_s,
|
||||
// msg_propulsion_vitesse_robot.rotation_rad_s);
|
||||
commande_vitesse(msg_propulsion_vitesse_robot.avance_mm_s, msg_propulsion_vitesse_robot.rotation_rad_s);
|
||||
|
||||
}
|
||||
if(mise_a_jour_trajectoire){
|
||||
struct msg_trajectoire_t msg_trajectoire;
|
||||
get_données_reçues((uint8_t *) &msg_trajectoire, sizeof(msg_trajectoire), REG_PROPULSION_TRAJECTOIRE);
|
||||
printf("trajectoire: type: %d,\n point1_x:%.2f, point1_y:%.2f,\n point2_x:%.2f, point2_y:%.2f,\n point3_x:%.2f, point3_y:%.2f\n point4_x:%.2f, point4_y:%.2f\n",
|
||||
msg_trajectoire.trajectoire.type, msg_trajectoire.trajectoire.p1.x, msg_trajectoire.trajectoire.p1.y,
|
||||
msg_trajectoire.trajectoire.p2.x, msg_trajectoire.trajectoire.p2.y,
|
||||
msg_trajectoire.trajectoire.p3.x, msg_trajectoire.trajectoire.p3.y,
|
||||
msg_trajectoire.trajectoire.p4.x, msg_trajectoire.trajectoire.p4.y);
|
||||
trajectoire = msg_trajectoire.trajectoire;
|
||||
mode = 4;
|
||||
Trajet_config(200, 100);
|
||||
|
||||
// Oh la la !
|
||||
}
|
||||
|
||||
@ -200,14 +217,33 @@ void main(void)
|
||||
registre = message.donnees[2];
|
||||
if(id_carte == 'D'){
|
||||
if(message.donnees[4] > 0 && message.donnees[4] < 30){
|
||||
printf("LED ON\n");
|
||||
//printf("LED ON\n");
|
||||
gpio_put(PICO_DEFAULT_LED_PIN, 1);
|
||||
}else{
|
||||
printf("LED OFF\n");
|
||||
//printf("LED OFF\n");
|
||||
gpio_put(PICO_DEFAULT_LED_PIN, 0);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case 'd':
|
||||
//printf("Demande donnees\n");
|
||||
// Demande de données
|
||||
memcpy(&message_applicatif, message.donnees, message.taille_donnees);
|
||||
// printf("id_carte:%c %d\n", message_applicatif.id_carte, message_applicatif.id_carte);
|
||||
// printf("adresse registre:%d\n", message_applicatif.adresse_registre, message_applicatif.id_carte);
|
||||
// printf("taille:%d\n", message_applicatif.taille_donnees, message_applicatif.taille_donnees);
|
||||
stdio_putchar_raw(0xFF);
|
||||
stdio_putchar_raw(0xFF);
|
||||
stdio_putchar_raw('P');
|
||||
stdio_putchar_raw(message_applicatif.taille_donnees+1);
|
||||
for(int i=0; i<message_applicatif.taille_donnees; i++ ){
|
||||
stdio_putchar_raw(memoire_echange[message_applicatif.adresse_registre + i]);
|
||||
}
|
||||
stdio_putchar_raw(0x00);
|
||||
stdio_putchar_raw('\n');
|
||||
break;
|
||||
default:
|
||||
//printf("Message inconnu: %d %c\n", message.donnees[0], message.donnees[0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -218,6 +254,18 @@ void main(void)
|
||||
// Fin du match
|
||||
|
||||
if(temps_ms != Temps_get_temps_ms()){
|
||||
if(Temps_get_temps_ms() - temps_ms > 20){
|
||||
/// PANIC
|
||||
struct msg_propulsion_position_t msg_propulsion_position;
|
||||
msg_propulsion_position.position_x_mm = 0;
|
||||
msg_propulsion_position.position_y_mm = 0;
|
||||
msg_propulsion_position.orientation_rad = 0;
|
||||
mise_données_dans_échange((uint8_t*) &msg_propulsion_position, sizeof(msg_propulsion_position), REG_PROPULSION_POSITION);
|
||||
Moteur_Stop();
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
continue;
|
||||
|
||||
}
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
if(temps_ms % step_ms == 0){
|
||||
QEI_update();
|
||||
@ -236,10 +284,36 @@ void main(void)
|
||||
break;
|
||||
case 2:
|
||||
case 3:
|
||||
AsserMoteur_Gestion(step_ms);
|
||||
break;
|
||||
case 4:
|
||||
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
||||
mode = 0;
|
||||
}
|
||||
AsserMoteur_Gestion(step_ms);
|
||||
break;
|
||||
}
|
||||
|
||||
// Récupération des valeurs pour les mettre dans la mémoire d'échange
|
||||
// Position
|
||||
struct msg_propulsion_position_t msg_propulsion_position;
|
||||
struct position_t position = Localisation_get();
|
||||
msg_propulsion_position.position_x_mm = position.x_mm;
|
||||
msg_propulsion_position.position_y_mm = position.y_mm;
|
||||
msg_propulsion_position.orientation_rad = position.angle_radian;
|
||||
mise_données_dans_échange((uint8_t*) &msg_propulsion_position, sizeof(msg_propulsion_position), REG_PROPULSION_POSITION);
|
||||
mise_données_dans_échange((uint8_t*) &msg_propulsion_position, sizeof(msg_propulsion_position), REG_PROPULSION_POSITION_2);
|
||||
float abscisse = Trajet_get_abscisse();
|
||||
mise_données_dans_échange((uint8_t*) &abscisse, sizeof(abscisse), REG_PROPULSION_ABSCISSE);
|
||||
struct point_xyo_t position_consigne = Trajectoire_get_point(&trajectoire, abscisse);
|
||||
mise_données_dans_échange((uint8_t*) &(position_consigne.point_xy), sizeof(position_consigne.point_xy), REG_PROPULSION_POSITION_CONSIGNE);
|
||||
float vitesse_roue_gauche = AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms);
|
||||
float vitesse_roue_droite = AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms);
|
||||
mise_données_dans_échange((uint8_t*) &(vitesse_roue_gauche), sizeof(vitesse_roue_gauche), REG_PROPULSION_VITESSE_ROUES_lecture);
|
||||
mise_données_dans_échange((uint8_t*) &(vitesse_roue_droite), sizeof(vitesse_roue_droite), REG_PROPULSION_VITESSE_ROUES_lecture +4);
|
||||
|
||||
//sleep_ms(1);
|
||||
//printf("x_mm:%.2f, y_mm:%.2f\n", position.x_mm, position.y_mm);
|
||||
|
||||
//gestion_PAMI(step_ms, &asser_pos);
|
||||
/*if(asser_pos){
|
||||
|
||||
@ -16,6 +16,9 @@ void get_données_reçues(uint8_t * dst, unsigned int taille, unsigned int regis
|
||||
memcpy(dst, &(memoire_echange[registre]), taille);
|
||||
}
|
||||
|
||||
void mise_données_dans_échange(uint8_t * source, unsigned int taille, unsigned int registre){
|
||||
memcpy(&(memoire_echange[registre]), source, taille);
|
||||
}
|
||||
|
||||
void écriture_données(unsigned int adresse, uint8_t donnée){
|
||||
if( adresse <= 11){
|
||||
|
||||
@ -3,6 +3,9 @@
|
||||
|
||||
void écriture_données(unsigned int adresse, uint8_t donnée);
|
||||
void get_données_reçues(uint8_t * dst, unsigned int taille, unsigned int registre);
|
||||
void mise_données_dans_échange(uint8_t * source, unsigned int taille, unsigned int registre);
|
||||
|
||||
extern uint8_t memoire_echange[];
|
||||
|
||||
extern bool mise_a_jour_position;
|
||||
extern bool mise_a_jour_mode;
|
||||
|
||||
@ -1,12 +1,19 @@
|
||||
#ifndef MESSAGE_PROPULSION_H
|
||||
#define MESSAGE_PROPULSION_H
|
||||
|
||||
#include "Trajectoire.h"
|
||||
#define USB_ID_PROPULSION 'P'
|
||||
|
||||
#define REG_PROPULSION_POSITION 0x00
|
||||
#define REG_PROPULSION_POSITION_2 0x80
|
||||
#define REG_PROPULSION_MODE 0x0D
|
||||
#define REG_PROPULSION_PWM 0x0E
|
||||
#define REG_PROPULSION_VITESSE_ROUES 0x12
|
||||
#define REG_PROPULSION_VITESSE_ROBOT 0x1A
|
||||
#define REG_PROPULSION_TRAJECTOIRE 0x22
|
||||
#define REG_PROPULSION_ABSCISSE 0x8C
|
||||
#define REG_PROPULSION_POSITION_CONSIGNE 0x90
|
||||
#define REG_PROPULSION_VITESSE_ROUES_lecture 0x98
|
||||
|
||||
struct msg_propulsion_position_t{
|
||||
float position_x_mm, position_y_mm, orientation_rad;
|
||||
@ -24,4 +31,19 @@ struct msg_propulsion_vitesse_robot_t {
|
||||
float avance_mm_s, rotation_rad_s;
|
||||
};
|
||||
|
||||
struct msg_trajectoire_t {
|
||||
struct trajectoire_t trajectoire;
|
||||
};
|
||||
/*
|
||||
struct trajectoire_t {
|
||||
enum trajectoire_type_t type;
|
||||
struct point_xy_t p1, p2, p3, p4;
|
||||
float orientation_debut_rad, orientation_fin_rad;
|
||||
float rayon, angle_debut_rad, angle_fin_rad;
|
||||
float longueur;
|
||||
// Pour les trajectoires composées
|
||||
struct trajectoire_t * trajectoires[NB_MAX_TRAJECTOIRE];
|
||||
int nb_trajectoire;
|
||||
};*/
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user