Debut de la dance des PAMI + optimisation tirette/attente
This commit is contained in:
parent
243e8c1e36
commit
2d84c5748a
74
Servomoteur.c
Normal file
74
Servomoteur.c
Normal file
@ -0,0 +1,74 @@
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "Servomoteur.h"
|
||||
|
||||
|
||||
|
||||
void Servomoteur_Init(void){
|
||||
uint slice_num;
|
||||
gpio_set_function(SERVO5, GPIO_FUNC_PWM);
|
||||
gpio_set_function(SERVO6, GPIO_FUNC_PWM);
|
||||
gpio_set_function(SERVO7, GPIO_FUNC_PWM);
|
||||
gpio_set_function(SERVO_TURBINE, GPIO_FUNC_PWM);
|
||||
|
||||
pwm_config pwm_servo = pwm_get_default_config();
|
||||
|
||||
// On veut un rebouclage au bout de 20 ms (50 Hz).
|
||||
// Division de l'horloge système (133 MHz) par 254 : 523 kHz
|
||||
pwm_config_set_clkdiv_int(&pwm_servo, 254);
|
||||
// Valeur max du PXM pour avoir 50 Hz : 523 kHz / 50 Hz : 10460
|
||||
pwm_config_set_wrap(&pwm_servo, 10460);
|
||||
// À la valeur finale, rebouclage à 0 (et non un compte à rebours)
|
||||
pwm_config_set_phase_correct(&pwm_servo, false);
|
||||
|
||||
slice_num = pwm_gpio_to_slice_num(SERVO5);
|
||||
pwm_init(slice_num, &pwm_servo, true);
|
||||
|
||||
slice_num = pwm_gpio_to_slice_num(SERVO_TURBINE);
|
||||
pwm_init(slice_num, &pwm_servo, true);
|
||||
|
||||
slice_num = pwm_gpio_to_slice_num(SERVO6);
|
||||
pwm_init(slice_num, &pwm_servo, true);
|
||||
|
||||
slice_num = pwm_gpio_to_slice_num(SERVO7);
|
||||
pwm_init(slice_num, &pwm_servo, true);
|
||||
|
||||
Servomoteur_set(SERVO5, SERVO_VALEUR_1_5MS);
|
||||
Servomoteur_set(SERVO6, SERVO_VALEUR_1_5MS);
|
||||
Servomoteur_set(SERVO7, SERVO_VALEUR_1_5MS);
|
||||
Servomoteur_set(SERVO_TURBINE, SERVO_VALEUR_1MS);
|
||||
}
|
||||
|
||||
void Servomoteur_set(uint8_t servomoteur, uint16_t valeur){
|
||||
uint slice_num = pwm_gpio_to_slice_num(servomoteur);
|
||||
// Chan : A ou 0 si la GPIO est paire
|
||||
// B ou 1 si la GPIO est impaire
|
||||
pwm_set_chan_level(slice_num, servomoteur & 0x01, valeur);
|
||||
}
|
||||
|
||||
void Moteur_Init(){
|
||||
uint slice_num;
|
||||
gpio_set_function(SERVO8_MOT_PROPULSEUR, GPIO_FUNC_PWM);
|
||||
|
||||
pwm_config pwm_servo = pwm_get_default_config();
|
||||
|
||||
// On veut un rebouclage au bout de 20 ms (50 Hz).
|
||||
// Division de l'horloge système (133 MHz) par 25 : 5230 kHz
|
||||
pwm_config_set_clkdiv_int(&pwm_servo, 25);
|
||||
// Valeur max du PXM pour avoir 5 kHz : 5230 kHz / 5 kHz : 1064
|
||||
pwm_config_set_wrap(&pwm_servo, 1064);
|
||||
// À la valeur finale, rebouclage à 0 (et non un compte à rebours)
|
||||
pwm_config_set_phase_correct(&pwm_servo, false);
|
||||
|
||||
slice_num = pwm_gpio_to_slice_num(SERVO8_MOT_PROPULSEUR);
|
||||
pwm_init(slice_num, &pwm_servo, true);
|
||||
|
||||
Moteur_Set(0);
|
||||
|
||||
}
|
||||
|
||||
/// @brief Envoie la consigne PWM au moteur.
|
||||
/// @param consigne : consigne entre 0 et 1.
|
||||
void Moteur_Set(double consigne){
|
||||
Servomoteur_set(SERVO8_MOT_PROPULSEUR, (uint16_t) (1064 * consigne));
|
||||
}
|
25
Servomoteur.h
Normal file
25
Servomoteur.h
Normal file
@ -0,0 +1,25 @@
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
// Valeur des servomoteurs
|
||||
#define SERVO_VALEUR_MAX 10459
|
||||
#define SERVO_VALEUR_2MS 1056
|
||||
#define SERVO_VALEUR_1_5MS 784
|
||||
#define SERVO_VALEUR_1MS 523
|
||||
#define SERVO_VALEUR_0_5MS 261
|
||||
|
||||
|
||||
// Servomoteurs
|
||||
#define SERVO5 4
|
||||
#define SERVO6 5
|
||||
#define SERVO7 6
|
||||
#define SERVO8_MOT_PROPULSEUR 15
|
||||
#define SERVO_TURBINE 7
|
||||
|
||||
#define DOIGT_AVANCE 1, SERVO_VALEUR_1_5MS
|
||||
#define DOIGT_RETRAIT 1, SERVO_VALEUR_1_5MS
|
||||
|
||||
|
||||
void Servomoteur_Init(void);
|
||||
void Servomoteur_set(uint8_t servomoteur, uint16_t valeur);
|
||||
void Moteur_Init(void);
|
||||
void Moteur_Set(double consigne);
|
131
Strategie.c
131
Strategie.c
@ -1,5 +1,6 @@
|
||||
#include "Strategie.h"
|
||||
#include "Geometrie.h"
|
||||
#include "Servomoteur.h"
|
||||
|
||||
|
||||
|
||||
@ -51,7 +52,7 @@ enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t couleur
|
||||
|
||||
|
||||
|
||||
enum etat_action_t Strategie_groupie_1(uint32_t step_ms){
|
||||
enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur){
|
||||
static enum{
|
||||
SSS_INIT,
|
||||
SSS_AVANCE,
|
||||
@ -88,3 +89,131 @@ enum etat_action_t Strategie_groupie_1(uint32_t step_ms){
|
||||
return ACTION_EN_COURS;
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur){
|
||||
static enum{
|
||||
SSS_INIT,
|
||||
SSS_AVANCE,
|
||||
SSS_DANCE
|
||||
} etat_sss = SSS_INIT;
|
||||
static struct trajectoire_t trajectoire_composee;
|
||||
static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
|
||||
|
||||
|
||||
|
||||
switch(etat_sss){
|
||||
case SSS_INIT:
|
||||
Localisation_set(45, 1895, 0);
|
||||
Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
|
||||
Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
|
||||
Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
|
||||
Trajectoire_composee_init(&trajectoire_composee);
|
||||
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
|
||||
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
|
||||
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
|
||||
etat_sss = SSS_AVANCE;
|
||||
break;
|
||||
|
||||
case SSS_AVANCE:
|
||||
Trajet_config(TRAJECT_CONFIG_RAPIDE);
|
||||
if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
||||
etat_sss = SSS_DANCE;
|
||||
}
|
||||
break;
|
||||
|
||||
case SSS_DANCE:
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
return ACTION_EN_COURS;
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie_groupie_3(uint32_t step_ms, enum couleur_t couleur){
|
||||
static enum{
|
||||
SSS_INIT,
|
||||
SSS_AVANCE,
|
||||
SSS_DANCE
|
||||
} etat_sss = SSS_INIT;
|
||||
static struct trajectoire_t trajectoire_composee;
|
||||
static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
|
||||
|
||||
|
||||
|
||||
switch(etat_sss){
|
||||
case SSS_INIT:
|
||||
Localisation_set(45, 1895, 0);
|
||||
Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
|
||||
Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
|
||||
Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
|
||||
Trajectoire_composee_init(&trajectoire_composee);
|
||||
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
|
||||
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
|
||||
Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
|
||||
etat_sss = SSS_AVANCE;
|
||||
break;
|
||||
|
||||
case SSS_AVANCE:
|
||||
Trajet_config(TRAJECT_CONFIG_RAPIDE);
|
||||
if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
||||
etat_sss = SSS_DANCE;
|
||||
}
|
||||
break;
|
||||
|
||||
case SSS_DANCE:
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
return ACTION_EN_COURS;
|
||||
|
||||
}
|
||||
|
||||
void PAMI_dance(int identifiant){
|
||||
switch (identifiant)
|
||||
{
|
||||
case 0:
|
||||
sleep_ms(500);
|
||||
Servomoteur_set(DOIGT_AVANCE);
|
||||
sleep_ms(250);
|
||||
Servomoteur_set(DOIGT_RETRAIT);
|
||||
sleep_ms(250); // t = 1,0 s
|
||||
sleep_ms(750); // t = 1,75 s
|
||||
Servomoteur_set(DOIGT_AVANCE);
|
||||
sleep_ms(250);
|
||||
Servomoteur_set(DOIGT_RETRAIT);
|
||||
sleep_ms(250); // t = 2,25 s
|
||||
sleep_ms(950); // t = 3,2 s
|
||||
for(int i =0; i<3;i++){
|
||||
Servomoteur_set(DOIGT_AVANCE);
|
||||
sleep_ms(200);
|
||||
Servomoteur_set(DOIGT_RETRAIT);
|
||||
sleep_ms(200);
|
||||
}// t = 4,4 s
|
||||
sleep_ms(1400); // t = 5,8
|
||||
break;
|
||||
case 1:
|
||||
case 2:
|
||||
case 3:
|
||||
sleep_ms(1100); // t = 1,1 s
|
||||
Servomoteur_set(DOIGT_AVANCE);
|
||||
sleep_ms(250);
|
||||
Servomoteur_set(DOIGT_RETRAIT);
|
||||
sleep_ms(250); // t = 1,6 s
|
||||
sleep_ms(800); // t = 2,4 s
|
||||
Servomoteur_set(DOIGT_AVANCE);
|
||||
sleep_ms(250);
|
||||
Servomoteur_set(DOIGT_RETRAIT);
|
||||
sleep_ms(250); // t = 2,9 s
|
||||
sleep_ms(1600); // t = 4,5 s
|
||||
for(int i =0; i<3;i++){
|
||||
Servomoteur_set(DOIGT_AVANCE);
|
||||
sleep_ms(150);
|
||||
Servomoteur_set(DOIGT_RETRAIT);
|
||||
sleep_ms(150);
|
||||
}// t = 5,4 s
|
||||
sleep_ms(400); // t = 5,8 s
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
@ -46,6 +46,10 @@ struct objectif_t{
|
||||
};
|
||||
|
||||
enum etat_action_t Strategie_super_star(uint32_t step_ms, enum couleur_t);
|
||||
enum etat_action_t Strategie_groupie_1(uint32_t step_ms, enum couleur_t couleur);
|
||||
enum etat_action_t Strategie_groupie_2(uint32_t step_ms, enum couleur_t couleur);
|
||||
enum etat_action_t Strategie_groupie_3(uint32_t step_ms, enum couleur_t couleur);
|
||||
void PAMI_dance(int);
|
||||
|
||||
enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
|
||||
|
||||
|
86
main.c
86
main.c
@ -36,7 +36,7 @@ void tension_batterie_init(void);
|
||||
uint16_t tension_batterie_lire(void);
|
||||
|
||||
void identifiant_init(void);
|
||||
uint identifiant_lire(void);
|
||||
uint get_identifiant(void);
|
||||
|
||||
int get_tirette(void);
|
||||
int get_couleur(void);
|
||||
@ -63,8 +63,8 @@ void main(void)
|
||||
Temps_init();
|
||||
|
||||
identifiant_init();
|
||||
Localisation_init(identifiant_lire());
|
||||
Trajet_init(identifiant_lire());
|
||||
Localisation_init(get_identifiant());
|
||||
Trajet_init(get_identifiant());
|
||||
i2c_maitre_init();
|
||||
|
||||
|
||||
@ -79,11 +79,16 @@ void main(void)
|
||||
gpio_set_dir(LED1PIN, GPIO_OUT );
|
||||
gpio_put(LED1PIN, 1);
|
||||
|
||||
gpio_init(TIRETTE_PIN);
|
||||
gpio_set_dir(LED1PIN, GPIO_IN);
|
||||
|
||||
|
||||
|
||||
multicore_launch_core1(gestion_affichage);
|
||||
// TODO: A remettre - quand on aura récupéré un capteur
|
||||
//multicore_launch_core1(gestion_VL53L8CX);
|
||||
sleep_ms(1500);
|
||||
|
||||
sleep_ms(85000);
|
||||
printf("Demarrage...\n");
|
||||
|
||||
|
||||
@ -96,7 +101,7 @@ void main(void)
|
||||
gpio_put(LED1PIN, 0);
|
||||
|
||||
// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
|
||||
if(identifiant_lire() == 3){
|
||||
if(get_identifiant() == 3){
|
||||
sleep_ms(90000);
|
||||
}
|
||||
|
||||
@ -107,7 +112,9 @@ void main(void)
|
||||
// Fin du match
|
||||
if((Temps_get_temps_ms() -temps_depart_ms) >15000 || (fin_match == 1)){
|
||||
Moteur_Stop();
|
||||
while(1);
|
||||
while(1){
|
||||
|
||||
}
|
||||
}
|
||||
if(temps_ms != Temps_get_temps_ms()){
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
@ -115,7 +122,7 @@ void main(void)
|
||||
QEI_update();
|
||||
Localisation_gestion();
|
||||
|
||||
if(Strategie_super_star(step_ms, COULEUR_BLEU) == ACTION_TERMINEE){
|
||||
if(Strategie_super_star(step_ms, COULEUR_JAUNE) == ACTION_TERMINEE){
|
||||
Asser_Position_maintien();
|
||||
if(Asser_Position_panic_angle()){
|
||||
fin_match=1;
|
||||
@ -130,6 +137,65 @@ void main(void)
|
||||
}
|
||||
}
|
||||
|
||||
void gestion_PAMI(uint32_t step_ms){
|
||||
static enum{
|
||||
PAMI_ATTENTE_TIRETTE,
|
||||
PAMI_ATTENTE_TEMPO,
|
||||
PAMI_FIN_TEMPO_MANUELLE,
|
||||
PAMI_TRAJECTOIRE,
|
||||
PAMI_DANCE,
|
||||
}etat_PAMI;
|
||||
static uint32_t temps_tirette, temps_mouvement;
|
||||
|
||||
switch (etat_PAMI)
|
||||
{
|
||||
case PAMI_ATTENTE_TIRETTE:
|
||||
if(get_tirette() == 0){
|
||||
etat_PAMI = PAMI_ATTENTE_TEMPO;
|
||||
temps_tirette = Temps_get_temps_ms();
|
||||
}
|
||||
break;
|
||||
|
||||
case PAMI_ATTENTE_TEMPO:
|
||||
if(get_tirette() == 1 && (Temps_get_temps_ms() - temps_tirette > 1000)){
|
||||
etat_PAMI = PAMI_FIN_TEMPO_MANUELLE;
|
||||
}
|
||||
if (Temps_get_temps_ms() - temps_tirette > 85000){
|
||||
etat_PAMI = PAMI_TRAJECTOIRE;
|
||||
temps_mouvement = Temps_get_temps_ms();
|
||||
}
|
||||
break;
|
||||
|
||||
case PAMI_FIN_TEMPO_MANUELLE:
|
||||
if(get_tirette() == 0){
|
||||
etat_PAMI = PAMI_TRAJECTOIRE;
|
||||
temps_mouvement = Temps_get_temps_ms();
|
||||
}
|
||||
break;
|
||||
|
||||
case PAMI_TRAJECTOIRE:
|
||||
switch (get_identifiant())
|
||||
{
|
||||
case 0: Strategie_super_star(step_ms, get_couleur()); break;
|
||||
case 1: Strategie_groupie_1(step_ms, get_couleur()); break;
|
||||
case 2: Strategie_groupie_2(step_ms, get_couleur()); break;
|
||||
case 3: Strategie_groupie_3(step_ms, get_couleur()); break;
|
||||
default: break;
|
||||
}
|
||||
break;
|
||||
|
||||
case PAMI_DANCE:
|
||||
Moteur_Stop();
|
||||
while(1){
|
||||
PAMI_dance(get_identifiant());
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/// @brief Obtient la distance de l'obstacle le plus proche.
|
||||
/// @param
|
||||
void gestion_VL53L8CX(void){
|
||||
@ -176,7 +242,7 @@ void affichage(void){
|
||||
printf(">pos_x:%lu:%.2f\n>pos_y:%lu:%.2f\n", temps_ms, position_actuelle.x_mm, temps_ms, position_actuelle.y_mm);
|
||||
printf(">con_x:%lu:%.2f\n>con_y:%lu:%.2f\n", temps_ms, point.point_xy.x, temps_ms, point.point_xy.y);
|
||||
|
||||
printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette());
|
||||
printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), get_identifiant(), get_tirette());
|
||||
}
|
||||
|
||||
void tension_batterie_init(void){
|
||||
@ -216,6 +282,8 @@ void identifiant_init(){
|
||||
gpio_set_dir(COULEUR_PIN, GPIO_IN);
|
||||
}
|
||||
|
||||
/// @brief Lit la tirette
|
||||
/// @return 1 si la tirette est branchée, 0 sinon
|
||||
int get_tirette(){
|
||||
return !gpio_get(TIRETTE_PIN);
|
||||
|
||||
@ -227,6 +295,6 @@ int get_couleur(void){
|
||||
|
||||
/// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse
|
||||
/// @return identifiant du robot (PDI switch)
|
||||
uint identifiant_lire(){
|
||||
uint get_identifiant(){
|
||||
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user