Keuronde/Holonome_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Demo_2025_03
Holonome_2024/Test_strategie_2024.c
Samuel 1b1d38d9d4 Prépa match 3
2024-05-10 10:11:30 +02:00

911 lines
26 KiB
C
Raw Permalink Blame History

#include <stdio.h>
#include "pico/stdio.h"
#include "pico/error.h"
#include "pico/multicore.h"
#include "hardware/i2c.h"
#include "Commande_vitesse.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Evitement.h"
#include "Geometrie.h"
#include "Geometrie_robot.h"
#include "Asser_Moteurs.h"
#include "Localisation.h"
#include "Robot_config.h"
#include "Strategie_2024_plante.h"
#include "Strategie_2024_pots.h"
#include "Strategie.h"
#include "Trajectoire.h"
#include "QEI.h"
#include "gyro.h"
#include "Moteurs.h"
int test_calcul_position_pot(void);
int test_calage_debut(void);
int test_attrape_pot(void);
int test_aller_a_plante(void);
int test_attrape_plante(void);
int test_aller_zone_plante();
int test_pseudo_homologation(void);
int test_attrape_1_pot(void);
int test_echange_pot(void);
void affichage_test_strategie_2024(void);
int test_strategie_2024(){
printf("A - Position groupes pot.\n");
printf("B - Calage debut.\n");
printf("C - Attrape pot.\n");
printf("D - Aller a plante.\n");
printf("E - Attrape plante.\n");
printf("F - Aller zone plante.\n");
printf("G - Pseudo homologation.\n");
printf("H - reglage pots.\n");
printf("I - echange pots.\n");
printf("J - Calage fin.\n");
int lettre;
do{
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
switch(lettre){
case 'a':
case 'A':
while(test_calcul_position_pot());
break;
case 'b':
case 'B':
while(test_calage_debut());
break;
case 'c':
case 'C':
while(test_attrape_pot());
break;
case 'd':
case 'D':
while(test_aller_a_plante());
break;
case 'e':
case 'E':
while(test_attrape_plante());
break;
case 'f':
case 'F':
while(test_aller_zone_plante());
break;
case 'g':
case 'G':
while(test_pseudo_homologation());
break;
case 'h':
case 'H':
while(test_attrape_1_pot());
break;
case 'i':
case 'I':
while(test_echange_pot());
break;
case 'q':
case 'Q':
return 0;
}
}
void print_position(struct position_t position){
printf("x_mm: %.2f, y_mm: %.2f, angle: %.2f\n", position.x_mm, position.y_mm, position.angle_radian/DEGRE_EN_RADIAN);
}
int test_calcul_position_pot(){
printf("\ngroupe: GROUPE_POT_B1, pot: 5 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_B1, POT_5));
printf("\ngroupe: GROUPE_POT_B2, pot: 1 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_B2, POT_1));
printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));
printf("\ngroupe: GROUPE_POT_L2, pot: 1 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_L2, POT_1));
printf("\ngroupe: GROUPE_POT_R1, pot: 1 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_R1, POT_1));
printf("\ngroupe: GROUPE_POT_R2, pot: 1 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_R2, POT_1));
printf("\ngroupe: GROUPE_POT_L1, pot: 1 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_1));
printf("\ngroupe: GROUPE_POT_L1, pot: 2 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_2));
printf("\ngroupe: GROUPE_POT_L1, pot: 3 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_3));
printf("\ngroupe: GROUPE_POT_L1, pot: 4 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_4));
printf("\ngroupe: GROUPE_POT_L1, pot: 5 \n");
print_position(groupe_pot_get_pot(GROUPE_POT_L1, POT_5));
return 0;
}
int test_calage_debut(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action;
printf("test_calage_debut\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(250,250,0);
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
Trajet_config(100, 500);
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
etat_action = Strategie_calage_bas(COULEUR_BLEU, _step_ms);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_attrape_plante(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
printf("test_aller_a_plante\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_aller_a_plante(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
printf("test_aller_a_plante\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
etat_action = Strat_2024_aller_a_plante(ZONE_PLANTE_AUCUNE);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_aller_zone_plante(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
printf("test_aller_a_plante\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie_2024);
Localisation_set(800, 200, 0);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
etat_action = Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms);
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_attrape_pot(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
enum {
TAP_CALAGE,
TAP_POT
} etat_test = TAP_CALAGE;
printf("test_attrape_pot\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
Trajet_config(TRAJECT_CONFIG_STD);
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut_manuel(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;
case TAP_POT:
etat_action = Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms);
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
while(1){Moteur_Stop();}
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
int test_pseudo_homologation(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
static int tempo_ms;
enum {
TAP_CALAGE,
TAP_POT,
TAP_PLANTE_ORIENTATION,
TAP_PLANTE_ATTRAPE_1,
TAP_PLANTE_ATTRAPE_2,
TAP_ECHANGE_POT,
TAP_PLANTE_ATTRAPE_3,
TAP_PLANTE_ATTRAPE_4,
TAP_RENTRE,
TAP_DEPOSE
} etat_test = TAP_CALAGE;
printf("test_pseudo_homologation\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
Trajet_config(TRAJECT_CONFIG_STD);
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_debut_manuel(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_POT;
}
break;
case TAP_POT:
if(Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ORIENTATION;
}
break;
case TAP_PLANTE_ORIENTATION:
if(Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_1;
}
break;
case TAP_PLANTE_ATTRAPE_1:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_2;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_2:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6, ZONE_PLANTE_3);
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_ECHANGE_POT;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_ECHANGE_POT:
if(Strat_2024_echange_pot_avant_arriere(_step_ms) == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_3;
}
break;
case TAP_PLANTE_ATTRAPE_3:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_PLANTE_ATTRAPE_4;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_PLANTE_ATTRAPE_4:
etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1, ZONE_PLANTE_3);
if( etat_action == ACTION_TERMINEE){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}else if( etat_action == ACTION_ECHEC){
etat_test=TAP_RENTRE;
etat_action = ACTION_EN_COURS;
}
break;
case TAP_RENTRE:
float angle_destination;
angle_destination = 15 * DEGRE_EN_RADIAN;
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_tourner_et_aller_a(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_DEPOSE;
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
tempo_ms=500;
}
break;
case TAP_DEPOSE:
tempo_ms--;
commande_vitesse_stop();
if(tempo_ms<= 0){
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE);
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
while(1){Moteur_Stop();}
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
extern float abscisse;
void affichage_test_strategie_2024(){
uint32_t temps;
enum validite_vl53l8_t validite_vl53l8;
float angle, distance;
while(true){
temps = time_us_32()/1000;
//temps_cycle_display();
printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
printf(">pos_x:%ld:%f\n", temps, Localisation_get().x_mm);
printf(">pos_y:%ld:%f\n", temps, Localisation_get().y_mm);
printf(">pos_angle:%ld:%f\n", temps, (Localisation_get().angle_radian + ANGLE_PINCE)/ DEGRE_EN_RADIAN );
printf(">c_pos_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
printf(">c_pos_angle:%ld:%f\n", temps, (Trajet_get_consigne().angle_radian+ ANGLE_PINCE) / DEGRE_EN_RADIAN);
printf(">abscisse:%ld:%f\n", temps, abscisse);
printf(">tirette:%ld:%d\n", temps, attente_tirette());
printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());
i2c_annexe_get_VL53L8(&validite_vl53l8, &angle, &distance);
printf(">v:%d\n", validite_vl53l8);
switch(validite_vl53l8){
case VL53L8_BORDURE:
printf(">b_angle:%.2f\n>b_distance:%.2f\n", angle, distance);
break;
case VL53L8_PLANTE:
printf(">p_angle:%.2f\n>p_distance:%.2f\n", angle, distance);
break;
case VL53L8_DISTANCE_LOIN:
printf("\n>v_distance:%.2f\n", distance);
break;
}
sleep_ms(10);
}
}
/// @brief Fonction abandonnée - pour le réglage des bras un par un
/// @param
/// @return
int test_attrape_1_pot(void){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
int32_t tempo_ms;
enum {
TAP_CALAGE,
TAP_APPROCHE_POT,
TAP_TOURNE_POT,
TAP_ATTRAPE_POT,
TAP_ATTRAPE_TEMPO
} etat_test = TAP_CALAGE;
printf("test_attrape_1_pot\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
Trajet_config(TRAJECT_CONFIG_STD);
multicore_launch_core1(affichage_test_strategie_2024);
float angle_bras[6] =
{
180 * DEGRE_EN_RADIAN,
120 * DEGRE_EN_RADIAN,
60 * DEGRE_EN_RADIAN,
0,
-60 * DEGRE_EN_RADIAN,
-120 * DEGRE_EN_RADIAN
};
float angle_bras_correction[6] =
{
11 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0 * DEGRE_EN_RADIAN,
0,
0 * DEGRE_EN_RADIAN,
13 * DEGRE_EN_RADIAN
};
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
static struct position_t position_pot, position_approche_pot, position_attrape_pot;
position_pot = groupe_pot_get_pot(GROUPE_POT_B1, POT_5);
position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM);
position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM);
int bras = BRAS_1;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
switch(etat_test){
case TAP_CALAGE:
if(Strategie_calage_bas(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_APPROCHE_POT;
}
break;
case TAP_APPROCHE_POT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
if(Strategie_aller_a(position_approche_pot.x_mm, position_approche_pot.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){
etat_test=TAP_TOURNE_POT;
}
break;
case TAP_TOURNE_POT:
if( Strategie_tourner_a(position_approche_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras], _step_ms) == ACTION_TERMINEE){
etat_test=TAP_ATTRAPE_POT;
}
break;
case TAP_ATTRAPE_POT:
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT);
if( Strategie_tourner_et_aller_a(
position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras],
EVITEMENT_SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){
i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT);
tempo_ms = 2000;
etat_test = TAP_ATTRAPE_TEMPO;
}
break;
case TAP_ATTRAPE_TEMPO:
tempo_ms--;
if(tempo_ms < 0){
etat_action = ACTION_TERMINEE;
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
while(1){Moteur_Stop();}
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
int test_echange_pot(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
struct trajectoire_t trajectoire;
enum evitement_t evitement;
enum etat_action_t etat_action=ACTION_EN_COURS;
static int tempo_ms;
static enum {
TEP_INIT,
TEP_ACTION,
} etat_echange_pot = TEP_INIT;
printf("test_echange_pot\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie_2024);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
Evitement_gestion(_step_ms);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
switch(etat_echange_pot){
case TEP_INIT:
i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(1, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
i2c_annexe_actionneur_pot(4, BRAS_HAUT, DOIGT_TIENT);
i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
tempo_ms=3000;
etat_echange_pot = TEP_ACTION;
break;
case TEP_ACTION:
tempo_ms--;
if (tempo_ms <= 0)
{
etat_action = Strat_2024_echange_pot_avant_arriere(_step_ms);
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
Reference in New Issue View Git Blame Copy Permalink