Mode colonne lumineuse + nouveau code de démonstration
This commit is contained in:
parent
2eb2ceff20
commit
7e24a79115
298
Demonstration.c
298
Demonstration.c
@ -2,16 +2,20 @@
|
||||
#include "Demonstration.h"
|
||||
|
||||
#define TEST_TIMEOUT_US 10000000
|
||||
#define CAPTEUR_POUR_ATTENTE 11
|
||||
#define CAPTEUR_POUR_ATTENTE 6
|
||||
|
||||
|
||||
int Demonstration_init(void);
|
||||
enum etat_action_t Demonstration_calage();
|
||||
enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm);
|
||||
enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees);
|
||||
enum etat_action_t Demonstration_tourne(float angle_degrees);
|
||||
enum etat_action_t Demonstration_bezier();
|
||||
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_actionneurs(void);
|
||||
|
||||
uint32_t temps_ms_demo = 0, temps_ms_old;
|
||||
|
||||
@ -22,6 +26,7 @@ void demo_affiche_localisation(){
|
||||
printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
|
||||
printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie() / 10.);
|
||||
printf(">capteur:%d\n", Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE));
|
||||
sleep_ms(50);
|
||||
}
|
||||
}
|
||||
|
||||
@ -29,6 +34,7 @@ int Demonstration_menu(void){
|
||||
static int iteration = 2;
|
||||
int rep;
|
||||
printf("Mode demo - init\n");
|
||||
set_position_avec_gyroscope(false);
|
||||
Demonstration_init();
|
||||
while(1){
|
||||
do{
|
||||
@ -40,6 +46,8 @@ int Demonstration_menu(void){
|
||||
/*printf("C - Trajets enchaines - manuels\n");
|
||||
printf("D - Trajets enchaines - auto\n");
|
||||
printf("E - Asservissement angulaire\n");*/
|
||||
printf("F - Rotation et actionneurs\n");
|
||||
printf("Z - Sem-automatique\n");
|
||||
printf("Q - Quitter\n");
|
||||
rep = getchar_timeout_us(TEST_TIMEOUT_US);
|
||||
}while(rep == 0 || rep == PICO_ERROR_TIMEOUT);
|
||||
@ -75,6 +83,13 @@ int Demonstration_menu(void){
|
||||
printf("Fin attente\n");
|
||||
break;
|
||||
|
||||
case 'F':
|
||||
while(1){
|
||||
printf("Demo actionneur\n");
|
||||
Demonstration_actionneurs();
|
||||
}
|
||||
break;
|
||||
|
||||
case 'q':
|
||||
case 'Q':
|
||||
return 0;
|
||||
@ -82,6 +97,7 @@ int Demonstration_menu(void){
|
||||
|
||||
case 'z':
|
||||
case 'Z':
|
||||
printf("Demo semi-auto\n");
|
||||
Demonstration_semiauto();
|
||||
break;
|
||||
|
||||
@ -135,6 +151,112 @@ void Demonstration_auto(){
|
||||
}
|
||||
}
|
||||
|
||||
void Demonstration_actionneurs(){
|
||||
Demonstration_attente();
|
||||
Demonstration_attrape_plante();
|
||||
Demonstration_tourne(30);
|
||||
Demonstration_leve_bras(0);
|
||||
Demonstration_tourne(-60);
|
||||
Demonstration_leve_bras(5);
|
||||
Demonstration_tourne(120);
|
||||
Demonstration_leve_bras(1);
|
||||
Demonstration_tourne(-180);
|
||||
Demonstration_leve_bras(4);
|
||||
Demonstration_tourne(240);
|
||||
Demonstration_leve_bras(2);
|
||||
Demonstration_tourne(-300);
|
||||
Demonstration_leve_bras(3);
|
||||
Demonstration_tourne(150);
|
||||
Demonstration_baisse_bras();
|
||||
}
|
||||
|
||||
void Balise_cli_orange_maintenance(){
|
||||
static int32_t temps_ms_led_cli, timer_led_ms = 0;
|
||||
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;
|
||||
// Toutes les 1 ms.
|
||||
if(temps_ms_led_cli != Temps_get_temps_ms()){
|
||||
temps_ms_led_cli = Temps_get_temps_ms();
|
||||
|
||||
timer_led_ms--;
|
||||
|
||||
switch(etat_led_cli_orange_maintenance){
|
||||
case CLI_ON_1:
|
||||
if(timer_led_ms<= 0){
|
||||
i2c_annexe_couleur_balise(0, 0xFFFF);
|
||||
timer_led_ms = 100;
|
||||
etat_led_cli_orange_maintenance=CLI_OFF_1;
|
||||
}
|
||||
break;
|
||||
case CLI_OFF_1:
|
||||
if(timer_led_ms<= 0){
|
||||
i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
|
||||
timer_led_ms = 100;
|
||||
etat_led_cli_orange_maintenance=CLI_ON_2;
|
||||
}
|
||||
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:
|
||||
if(timer_led_ms<= 0){
|
||||
i2c_annexe_couleur_balise(0b11101000, 0xFFFF);
|
||||
timer_led_ms = 100;
|
||||
etat_led_cli_orange_maintenance=CLI_ON_1;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void Balise_pulse_vert(){
|
||||
static int32_t temps_ms_led_cli, timer_led_ms = 800;
|
||||
int32_t timer_led_max_ms =800;
|
||||
int32_t vert_led, vert_led_max = 0b111;
|
||||
static enum etat_led_pulse_vert_t{
|
||||
PULSE_RISE,
|
||||
PULSE_FALL,
|
||||
}etat_led_pulse_vert=PULSE_FALL;
|
||||
// Toutes les 1 ms.
|
||||
if(temps_ms_led_cli != Temps_get_temps_ms()){
|
||||
temps_ms_led_cli = Temps_get_temps_ms();
|
||||
|
||||
timer_led_ms--;
|
||||
|
||||
switch(etat_led_pulse_vert){
|
||||
case PULSE_RISE:
|
||||
vert_led = (vert_led_max-1) - (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
|
||||
vert_led = vert_led & 0b111;
|
||||
i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
|
||||
if(timer_led_ms<= 0){
|
||||
i2c_annexe_couleur_balise(0, 0xFFFF);
|
||||
timer_led_ms = timer_led_max_ms;
|
||||
etat_led_pulse_vert=PULSE_FALL;
|
||||
}
|
||||
break;
|
||||
case PULSE_FALL:
|
||||
vert_led = (vert_led_max-1) * timer_led_ms / timer_led_max_ms + 1;
|
||||
vert_led = vert_led & 0b111;
|
||||
i2c_annexe_couleur_balise(vert_led << 2, 0xFFFF);
|
||||
if(timer_led_ms<= 0){
|
||||
timer_led_ms = timer_led_max_ms;
|
||||
etat_led_pulse_vert=PULSE_RISE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
enum etat_action_t Demonstration_attente(){
|
||||
enum {
|
||||
ATTENTE_DETECTION,
|
||||
@ -145,6 +267,7 @@ enum etat_action_t Demonstration_attente(){
|
||||
|
||||
while(true){
|
||||
Holonome_cyclique(PARAM_NO_MOTORS);
|
||||
Balise_cli_orange_maintenance();
|
||||
|
||||
switch(etat_attente){
|
||||
case ATTENTE_DETECTION:
|
||||
@ -376,6 +499,52 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
|
||||
/// @brief Rotation du robot sur lui-même du robot
|
||||
/// @param angle_degrees : Rotation du robot sur lui-même
|
||||
/// @return ACTION_TERMINEE
|
||||
enum etat_action_t Demonstration_tourne(float angle_degrees){
|
||||
enum {
|
||||
DEMO_TOURNE,
|
||||
DEMO_TOURNE_TERMINE
|
||||
} etat_avance_puis_tourne = DEMO_TOURNE;
|
||||
int pos_x_init_mm, pos_y_init_mm;
|
||||
while(true){
|
||||
struct trajectoire_t trajectoire;
|
||||
|
||||
Holonome_cyclique(PARAM_DEFAULT);
|
||||
|
||||
// Toutes les 1 ms.
|
||||
if(temps_ms_demo != Temps_get_temps_ms()){
|
||||
temps_ms_demo = Temps_get_temps_ms();
|
||||
|
||||
|
||||
switch (etat_avance_puis_tourne)
|
||||
{
|
||||
|
||||
case DEMO_TOURNE:
|
||||
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
|
||||
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
|
||||
if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
|
||||
etat_avance_puis_tourne = DEMO_TOURNE_TERMINE;
|
||||
Trajet_config(TRAJECT_CONFIG_STD);
|
||||
}
|
||||
break;
|
||||
|
||||
case DEMO_TOURNE_TERMINE:
|
||||
etat_avance_puis_tourne = DEMO_TOURNE;
|
||||
Moteur_Stop();
|
||||
return ACTION_TERMINEE;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
|
||||
/// @brief Déplacement suivant deux courbes de Bézier. Recommandé pour une démo sur une planche de 1m x 1,5m
|
||||
/// @return ACTION_TERMINEE
|
||||
enum etat_action_t Demonstration_bezier(){
|
||||
@ -434,3 +603,128 @@ enum etat_action_t Demonstration_bezier(){
|
||||
}
|
||||
return ACTION_ECHEC;
|
||||
}
|
||||
|
||||
/// @brief Leve le bras, et agite le doigt
|
||||
/// @param bras
|
||||
enum etat_action_t Demonstration_leve_bras(uint32_t bras){
|
||||
static enum{
|
||||
LB_LEVE_BRAS,
|
||||
LB_TEMPO_BRAS,
|
||||
LB_DOIGT_ATTRAPE_1,
|
||||
LB_DOIGT_LACHE,
|
||||
LB_DOIGT_ATTRAPE_2,
|
||||
} DLB_status;
|
||||
int timer_ms;
|
||||
while(true){
|
||||
Holonome_cyclique(PARAM_NO_MOTORS);
|
||||
if(temps_ms_demo != Temps_get_temps_ms()){
|
||||
temps_ms_demo = Temps_get_temps_ms();
|
||||
timer_ms--;
|
||||
|
||||
switch(DLB_status){
|
||||
case LB_LEVE_BRAS:
|
||||
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
|
||||
DLB_status = LB_TEMPO_BRAS;
|
||||
timer_ms = 250;
|
||||
break;
|
||||
|
||||
case LB_TEMPO_BRAS:
|
||||
if(timer_ms <= 0){
|
||||
DLB_status = LB_DOIGT_ATTRAPE_1;
|
||||
timer_ms=150;
|
||||
}
|
||||
break;
|
||||
|
||||
case LB_DOIGT_ATTRAPE_1:
|
||||
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
|
||||
if(timer_ms <= 0){
|
||||
DLB_status = LB_DOIGT_LACHE;
|
||||
timer_ms=200;
|
||||
}
|
||||
break;
|
||||
|
||||
case LB_DOIGT_LACHE:
|
||||
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_LACHE);
|
||||
if(timer_ms <= 0){
|
||||
DLB_status = LB_DOIGT_ATTRAPE_2;
|
||||
timer_ms=200;
|
||||
}
|
||||
break;
|
||||
|
||||
case LB_DOIGT_ATTRAPE_2:
|
||||
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
|
||||
if(timer_ms <= 0){
|
||||
DLB_status = LB_LEVE_BRAS;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
enum etat_action_t Demonstration_attrape_plante(void){
|
||||
int32_t timer_ms;
|
||||
enum {
|
||||
DAP_ENVOI,
|
||||
DAP_ATTENTE
|
||||
} dap_status=DAP_ENVOI;
|
||||
while(true){
|
||||
Holonome_cyclique(PARAM_NO_MOTORS);
|
||||
if(temps_ms_demo != Temps_get_temps_ms()){
|
||||
temps_ms_demo = Temps_get_temps_ms();
|
||||
timer_ms--;
|
||||
|
||||
switch(dap_status){
|
||||
case DAP_ENVOI:
|
||||
i2c_annexe_attrape_plante(PLANTE_BRAS_1);
|
||||
dap_status = DAP_ATTENTE;
|
||||
timer_ms=5000;
|
||||
break;
|
||||
|
||||
case DAP_ATTENTE:
|
||||
if (timer_ms <= 0){
|
||||
dap_status=DAP_ENVOI;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
enum etat_action_t Demonstration_baisse_bras(void){
|
||||
static int32_t timer_ms;
|
||||
enum {
|
||||
DBB_ENVOI,
|
||||
DBB_ATTENTE
|
||||
} dbb_status=DBB_ENVOI;
|
||||
while(true){
|
||||
Holonome_cyclique(PARAM_NO_MOTORS);
|
||||
if(temps_ms_demo != Temps_get_temps_ms()){
|
||||
temps_ms_demo = Temps_get_temps_ms();
|
||||
timer_ms--;
|
||||
|
||||
switch(dbb_status){
|
||||
case DBB_ENVOI:
|
||||
for(int bras=0; bras < 6; bras++){
|
||||
i2c_annexe_actionneur_pot(bras, BRAS_PLIE, DOIGT_LACHE);
|
||||
}
|
||||
dbb_status = DBB_ATTENTE;
|
||||
timer_ms=500;
|
||||
break;
|
||||
|
||||
case DBB_ATTENTE:
|
||||
if (timer_ms <= 0){
|
||||
dbb_status = DBB_ENVOI;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
@ -45,7 +45,7 @@ int main() {
|
||||
stdio_init_all();
|
||||
|
||||
//Demonstration_init();Demonstration_auto();
|
||||
//while(mode_test());
|
||||
while(mode_test());
|
||||
//test_pseudo_homologation();
|
||||
Holonome2023_init();
|
||||
|
||||
|
@ -345,8 +345,8 @@ int test_aller_retour(){
|
||||
switch(lettre){
|
||||
case 'b':
|
||||
case 'B':
|
||||
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
|
||||
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
|
||||
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
||||
Trajectoire_bezier(&trajectoire, 0, 0, 2500, 0, 250, 1300, 0, 0, 0, 0);
|
||||
printf("Trajectoire de Bézier\n");
|
||||
break;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user