Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Demo attrape plante

Browse Source
This commit is contained in:
Samuel 2025-03-15 11:50:58 +01:00
parent 7e24a79115
commit 45c578c984
1 changed files with 96 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
Demonstration.c
View File

@ -1,4 +1,5 @@
#include "Holonome2023.h"
#include "Strategie_2024.h"
#include "Demonstration.h"
#define TEST_TIMEOUT_US 10000000
@ -15,6 +16,7 @@ enum etat_action_t Demonstration_attente();
enum etat_action_t Demonstration_leve_bras(uint32_t bras);
enum etat_action_t Demonstration_baisse_bras(void);
enum etat_action_t Demonstration_attrape_plante();
void Demonstration_prise_plante(void);
void Demonstration_actionneurs(void);
uint32_t temps_ms_demo = 0, temps_ms_old;
@ -47,6 +49,7 @@ int Demonstration_menu(void){
printf("D - Trajets enchaines - auto\n");
printf("E - Asservissement angulaire\n");*/
printf("F - Rotation et actionneurs\n");
printf("G - Prise plante\n");
printf("Z - Sem-automatique\n");
printf("Q - Quitter\n");
rep = getchar_timeout_us(TEST_TIMEOUT_US);
@ -77,12 +80,14 @@ int Demonstration_menu(void){
Demonstration_bezier();
break;
case 'e':
case 'E':
printf("Début attente\n");
Demonstration_attente();
printf("Fin attente\n");
break;
case 'f':
case 'F':
while(1){
printf("Demo actionneur\n");
@ -90,6 +95,13 @@ int Demonstration_menu(void){
}
break;
case 'g':
case 'G':
printf("Début prise plante\n");
Demonstration_prise_plante();
printf("Fin prise plante\n");
break;
case 'q':
case 'Q':
return 0;
@ -170,14 +182,77 @@ void Demonstration_actionneurs(){
Demonstration_baisse_bras();
}
void Balise_cli_orange_maintenance(){
void Demonstration_prise_plante(){
static int32_t m_temps_ms, m_timer;
enum etat_action_t etat_action = ACTION_EN_COURS;
enum etat_action_t etat_action_step;
static enum {
INIT,
TIENT_POT,
LEVE_POT,
ATTRAPE_PLANTE_1,
ATTRAPE_PLANTE_2,
RETOUR_MAISON
} etat_prise_plante = INIT;
while(etat_action == ACTION_EN_COURS){
Holonome_cyclique(PARAM_DEFAULT);
if(m_temps_ms != Temps_get_temps_ms()){
m_temps_ms = Temps_get_temps_ms();
switch (etat_prise_plante){
case INIT:
m_timer = 3000;
etat_prise_plante = TIENT_POT;
etat_action = ACTION_EN_COURS;
case TIENT_POT:
m_timer--;
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
if(m_timer <= 0){
etat_prise_plante = LEVE_POT;
m_timer = 1000;
}
break;
case LEVE_POT:
m_timer--;
i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
if(m_timer <= 0){
etat_prise_plante = ATTRAPE_PLANTE_1;
}
break;
case ATTRAPE_PLANTE_1:
etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_1, ZONE_PLANTE_1);
if(etat_action_step == ACTION_TERMINEE){
etat_prise_plante = ATTRAPE_PLANTE_2;
}
break;
case ATTRAPE_PLANTE_2:
etat_action_step = Strat_2024_plante_dans_pot(1, PLANTE_BRAS_6, ZONE_PLANTE_1);
if(etat_action_step == ACTION_TERMINEE){
etat_prise_plante = RETOUR_MAISON;
}
break;
case RETOUR_MAISON:{
etat_action = Strategie_aller_a(0, 0, EVITEMENT_SANS_EVITEMENT, 1);
}
}
}
}
Moteur_Stop();
}
void Balise_cli_orange_maintenance(int nb_cli){
static int32_t temps_ms_led_cli, timer_led_ms = 0;
static int32_t m_nb_cli;
static enum etat_led_cli_orange_maintenance_t{
CLI_ON_1,
CLI_OFF_1,
CLI_ON_2,
CLI_OFF_2,
}etat_led_cli_orange_maintenance=CLI_ON_1;
CLI_ON_COURT,
CLI_OFF_COURT,
CLI_OFF_LONG,
}etat_led_cli_orange_maintenance=CLI_OFF_LONG;
// Toutes les 1 ms.
if(temps_ms_led_cli != Temps_get_temps_ms()){
temps_ms_led_cli = Temps_get_temps_ms();
@ -185,32 +260,33 @@ void Balise_cli_orange_maintenance(){
timer_led_ms--;
switch(etat_led_cli_orange_maintenance){
case CLI_ON_1:
case CLI_ON_COURT:
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0, 0xFFFF);
timer_led_ms = 100;
etat_led_cli_orange_maintenance=CLI_OFF_1;
m_nb_cli--;
if(m_nb_cli){
etat_led_cli_orange_maintenance=CLI_OFF_COURT;
timer_led_ms = 100;
}else{
etat_led_cli_orange_maintenance=CLI_OFF_LONG;
timer_led_ms = 700;
}
}
break;
case CLI_OFF_1:
case CLI_OFF_COURT:
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
timer_led_ms = 100;
etat_led_cli_orange_maintenance=CLI_ON_2;
etat_led_cli_orange_maintenance=CLI_ON_COURT;
}
break;
case CLI_ON_2:
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0, 0xFFFF);
timer_led_ms = 800;
etat_led_cli_orange_maintenance=CLI_OFF_2;
}
break;
case CLI_OFF_2:
case CLI_OFF_LONG:
m_nb_cli = nb_cli;
if(timer_led_ms<= 0){
i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
timer_led_ms = 100;
etat_led_cli_orange_maintenance=CLI_ON_1;
etat_led_cli_orange_maintenance=CLI_ON_COURT;
}
break;
}
@ -267,7 +343,7 @@ enum etat_action_t Demonstration_attente(){
while(true){
Holonome_cyclique(PARAM_NO_MOTORS);
Balise_cli_orange_maintenance();
Balise_cli_orange_maintenance(2);
switch(etat_attente){
case ATTENTE_DETECTION: