Compare commits
4 Commits
bec6d622f0
...
aa5a5d2d64
| Author | SHA1 | Date | |
|---|---|---|---|
| aa5a5d2d64 | |||
| 5eb8625916 | |||
| e4f0bb4b61 | |||
| 85a08715f0 |
@ -47,9 +47,9 @@ void Asser_Position(struct position_t position_consigne){
|
||||
avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
|
||||
rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
|
||||
|
||||
if(delta_avance_mm < 10){
|
||||
/*if(delta_avance_mm < 10){
|
||||
rotation_radian_s=delta_avance_mm/10 * rotation_radian_s;
|
||||
}
|
||||
}*/
|
||||
|
||||
// Commande en vitesse
|
||||
commande_vitesse(avance_mm_s, rotation_radian_s);
|
||||
|
||||
@ -1,4 +1,9 @@
|
||||
#include "Geometrie.h"
|
||||
#include "config_robot.h"
|
||||
|
||||
#define DISTANCE_ROUES_CENTRE_MM 52.
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
#define DISTANCE_ROUES_CENTRE_MM 104.
|
||||
#else
|
||||
#define DISTANCE_ROUES_CENTRE_MM 52.
|
||||
#endif
|
||||
|
||||
|
||||
52
Moteurs.c
52
Moteurs.c
@ -6,20 +6,21 @@
|
||||
#define MOTEUR_B 1
|
||||
#define MOTEUR_C 2
|
||||
|
||||
#define MOTEUR_A_VITESSE 6
|
||||
#define MOTEUR_B_VITESSE 8
|
||||
#define MOTEUR_C_VITESSE 10
|
||||
|
||||
#define MOTEUR_A_SENS 5
|
||||
#define MOTEUR_B_SENS 7
|
||||
#define MOTEUR_C_SENS 9
|
||||
|
||||
#define M1_SENS1 7
|
||||
#define M1_SENS2 13
|
||||
#define M1_VITESSE 27 //5B
|
||||
#define M2_SENS1 10
|
||||
#define M2_SENS2 5
|
||||
#define M2_VITESSE 9 //4B
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
#define M1_VITESSE 2 //1A
|
||||
#define M1_SENS1 3
|
||||
#define M1_SENS2 4
|
||||
#define M2_VITESSE 6 //3A
|
||||
#define M2_SENS1 7
|
||||
#define M2_SENS2 8
|
||||
#else
|
||||
#define M1_SENS1 7
|
||||
#define M1_SENS2 13
|
||||
#define M1_VITESSE 27 //5B
|
||||
#define M2_SENS1 10
|
||||
#define M2_SENS2 5
|
||||
#define M2_VITESSE 9 //4B
|
||||
#endif
|
||||
|
||||
uint slice_moteur_A, slice_moteur_B, slice_moteur_C;
|
||||
int moteur_a_pwm, moteur_b_pwm;
|
||||
@ -41,13 +42,21 @@ void Moteur_Init(){
|
||||
|
||||
gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM);
|
||||
gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(5, (uint16_t)65535);
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
pwm_set_wrap(1, (uint16_t)65535);
|
||||
pwm_set_wrap(3, (uint16_t)65535);
|
||||
pwm_set_chan_level(1, PWM_CHAN_A, 0);
|
||||
pwm_set_chan_level(3, PWM_CHAN_A, 0);
|
||||
pwm_set_enabled(1, true);
|
||||
pwm_set_enabled(3, true);
|
||||
#else
|
||||
pwm_set_wrap(4, (uint16_t)65535);
|
||||
pwm_set_chan_level(5, PWM_CHAN_B, 0);
|
||||
pwm_set_wrap(5, (uint16_t)65535);
|
||||
pwm_set_chan_level(4, PWM_CHAN_B, 0);
|
||||
pwm_set_enabled(5, true);
|
||||
pwm_set_chan_level(5, PWM_CHAN_B, 0);
|
||||
pwm_set_enabled(4, true);
|
||||
|
||||
pwm_set_enabled(5, true);
|
||||
#endif
|
||||
Moteur_SetVitesse(MOTEUR_A, 0);
|
||||
Moteur_SetVitesse(MOTEUR_B, 0);
|
||||
}
|
||||
@ -90,10 +99,12 @@ 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
|
||||
pwm_set_chan_level(1, PWM_CHAN_A, u_vitesse);
|
||||
if(vitesse > 0){
|
||||
#else
|
||||
pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
|
||||
if(vitesse < 0){
|
||||
#endif
|
||||
gpio_put(M1_SENS1, 1);
|
||||
@ -105,10 +116,11 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
|
||||
break;
|
||||
|
||||
case MOTEUR_B:
|
||||
pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
pwm_set_chan_level(3, PWM_CHAN_A, u_vitesse);
|
||||
if(vitesse < 0){
|
||||
#else
|
||||
pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
|
||||
if(vitesse > 0){
|
||||
#endif
|
||||
gpio_put(M2_SENS1, 1);
|
||||
|
||||
14
QEI.c
14
QEI.c
@ -44,15 +44,15 @@ void QEI_init(int identifiant){
|
||||
}
|
||||
// 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
|
||||
// QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 10 et 11, clock div : 0 pour commencer
|
||||
quadrature_encoder_program_init(pio_QEI, 1, offset, 10, 0);
|
||||
// QEI2 : broche 2 et 3 - pio : pio0, sm : 1, 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, 14, 0);
|
||||
quadrature_encoder_program_init(pio_QEI, 0, offset, 11, 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, 16, 0);
|
||||
quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0);
|
||||
#endif
|
||||
|
||||
QEI_A.value=0;
|
||||
@ -98,11 +98,7 @@ 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:
|
||||
|
||||
84
Readme.md
84
Readme.md
@ -76,59 +76,62 @@ Les adresses en **gras** sont celles pour lesquelles des `#define` existent qui
|
||||
| 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 |
|
||||
| **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 | - | Padding | Padding |
|
||||
| 0x24 | - | Padding | Padding |
|
||||
| 0x25 | - | Padding | Padding |
|
||||
| 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 |
|
||||
| 0x27 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x28 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 0x29 | RW | Flottant 32 bits | Point 1, X |
|
||||
| 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 |
|
||||
| 0x2B | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x2C | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 0x2D | RW | Flottant 32 bits | Point 1, Y |
|
||||
| 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 |
|
||||
| 0x2F | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x30 | RW | Flottant 32 bits | Point 2, X |
|
||||
| 0x31 | RW | Flottant 32 bits | Point 2, X |
|
||||
| 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 |
|
||||
| 0x33 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x34 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 0x35 | RW | Flottant 32 bits | Point 2, Y |
|
||||
| 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 |
|
||||
| 0x37 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x38 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 0x39 | RW | Flottant 32 bits | Point 3, X |
|
||||
| 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 |
|
||||
| 0x3B | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x3C | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 0x3D | RW | Flottant 32 bits | Point 3, Y |
|
||||
| 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 |
|
||||
| 0x3F | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x40 | RW | Flottant 32 bits | Point 4, X |
|
||||
| 0x41 | RW | Flottant 32 bits | Point 4, X |
|
||||
| 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 |
|
||||
| 0x43 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x44 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 0x45 | RW | Flottant 32 bits | Point 4, Y |
|
||||
| 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 |
|
||||
| 0x47 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x48 | RW | Flottant 32 bits | orientation_debut_rad |
|
||||
| 0x49 | RW | Flottant 32 bits | orientation_debut_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 |
|
||||
| 0x4B | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x4C | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 0x4D | RW | Flottant 32 bits | orientation_fin_rad |
|
||||
| 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 |
|
||||
| 0x4F | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x50 | RW | Flottant 32 bits | rayon mm |
|
||||
| 0x51 | RW | Flottant 32 bits | rayon mm |
|
||||
| 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 |
|
||||
| 0x53 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x54 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x55 | RW | Flottant 32 bits | Angle début rad |
|
||||
| 0x56 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x57 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x58 | RW | Flottant 32 bits | Angle fin rad |
|
||||
| 0x59 | 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 |
|
||||
@ -161,4 +164,5 @@ Les adresses en **gras** sont celles pour lesquelles des `#define` existent qui
|
||||
| 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 |
|
||||
| 0xA0 | R | int_8 | État du trajet (ACTION_EN_COURS=0, ACTION_TERMINEE=1, ACTION_ECHEC=2) |
|
||||
| **0xFF** | R | int_8 | Identifiant de la carte : 'P' |
|
||||
@ -15,9 +15,9 @@
|
||||
#define CORR_ANGLE_DEPART_DEGREE (0)
|
||||
|
||||
enum etat_action_t{
|
||||
ACTION_EN_COURS,
|
||||
ACTION_TERMINEE,
|
||||
ACTION_ECHEC
|
||||
ACTION_EN_COURS=0,
|
||||
ACTION_TERMINEE=1,
|
||||
ACTION_ECHEC=2
|
||||
};
|
||||
|
||||
enum longer_direction_t{
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
// Changer le define en fonction qu'on soit sur les PAMIs ou sur le robot principal
|
||||
|
||||
#define ROBOT_PROPULSION_2026
|
||||
//#define ROBOT_TYPE_PAMI
|
||||
//#define ROBOT_PROPULSION_2026
|
||||
#define ROBOT_TYPE_PAMI
|
||||
|
||||
#ifndef ROBOT_PROPULSION_2026
|
||||
#ifndef ROBOT_TYPE_PAMI
|
||||
|
||||
23
main.c
23
main.c
@ -69,8 +69,10 @@ void main(void)
|
||||
|
||||
stdio_init_all();
|
||||
Temps_init();
|
||||
|
||||
#ifdef ROBOT_PROPULSION_2026
|
||||
#else
|
||||
identifiant_init();
|
||||
#endif
|
||||
Localisation_init(get_identifiant());
|
||||
Trajet_init(get_identifiant());
|
||||
//i2c_maitre_init();
|
||||
@ -97,13 +99,8 @@ void main(void)
|
||||
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT );
|
||||
gpio_put(PICO_DEFAULT_LED_PIN, 1);
|
||||
|
||||
gpio_init(TIRETTE_PIN);
|
||||
gpio_set_dir(LED1PIN, GPIO_IN);
|
||||
gpio_pull_up(TIRETTE_PIN);
|
||||
|
||||
|
||||
sleep_ms(5000);
|
||||
|
||||
printf("Demarrage...\n");
|
||||
|
||||
|
||||
@ -164,8 +161,7 @@ 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);
|
||||
Localisation_set(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);
|
||||
@ -204,9 +200,9 @@ void main(void)
|
||||
msg_trajectoire.trajectoire.p4.x, msg_trajectoire.trajectoire.p4.y);
|
||||
trajectoire = msg_trajectoire.trajectoire;
|
||||
mode = 4;
|
||||
uint8_t etat_action = ACTION_EN_COURS;
|
||||
mise_données_dans_échange((uint8_t*) &(etat_action), sizeof(etat_action), REG_PROPULSION_ETAT_TRAJET);
|
||||
Trajet_config(200, 100);
|
||||
|
||||
// Oh la la !
|
||||
}
|
||||
|
||||
}
|
||||
@ -268,6 +264,7 @@ void main(void)
|
||||
}
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
if(temps_ms % step_ms == 0){
|
||||
enum etat_action_t etat_action;
|
||||
QEI_update();
|
||||
Localisation_gestion();
|
||||
|
||||
@ -287,7 +284,11 @@ void main(void)
|
||||
AsserMoteur_Gestion(step_ms);
|
||||
break;
|
||||
case 4:
|
||||
if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
||||
etat_action = Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT);
|
||||
uint8_t etat_action_u8 = etat_action;
|
||||
mise_données_dans_échange((uint8_t*) &(etat_action_u8), sizeof(etat_action_u8), REG_PROPULSION_ETAT_TRAJET);
|
||||
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
mode = 0;
|
||||
}
|
||||
AsserMoteur_Gestion(step_ms);
|
||||
|
||||
@ -14,6 +14,7 @@
|
||||
#define REG_PROPULSION_ABSCISSE 0x8C
|
||||
#define REG_PROPULSION_POSITION_CONSIGNE 0x90
|
||||
#define REG_PROPULSION_VITESSE_ROUES_lecture 0x98
|
||||
#define REG_PROPULSION_ETAT_TRAJET 0xA0
|
||||
|
||||
struct msg_propulsion_position_t{
|
||||
float position_x_mm, position_y_mm, orientation_rad;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user