350 lines
9.6 KiB
C
350 lines
9.6 KiB
C
#include <stdio.h>
|
|
#include "pico/stdio.h"
|
|
#include "pico/error.h"
|
|
#include "pico/multicore.h"
|
|
#include "hardware/i2c.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_attrape_plante(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 - Attrape plante.\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_attrape_plante());
|
|
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_debut(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_attrape_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();
|
|
|
|
|
|
}
|
|
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(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;
|
|
|
|
}
|
|
|
|
|
|
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(">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(100);
|
|
}
|
|
}
|