Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
c3d19bc8bc
RPiPico-Holonome2023/Test_strategie.c

718 lines
20 KiB
C
Raw Blame History

#include "pico/multicore.h"
#include "stdio.h"
#include "hardware/i2c.h"
#include "math.h"
#include "Asser_Moteurs.h"
#include "Balise_VL53L1X.h"
#include "Evitement.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "gyro.h"
#include "Localisation.h"
#include "Monitoring.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Strategie.h"
#include "Strategie_prise_cerises.h"
#include "Temps.h"
#include "Trajet.h"
#include "Trajectoire.h"
#include "Test.h"
int test_accostage(void);
int test_longe(void);
int test_panier(void);
int test_homologation(void);
int test_evitement(void);
int test_tirette_et_couleur();
int test_cerise_laterales_droite(void);
int test_cerise_laterales_gauche(void);
void affichage_test_evitement();
void affichage_test_strategie(){
uint32_t temps;
while(true){
temps = time_us_32()/1000;
temps_cycle_display();
printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C());
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/M_PI*180.);
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);
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());
/*switch(etat_strategie){
case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break;
case APPROCHE_CERISE_1_A: printf(">etat_strat:APPROCHE_CERISE_1_A|t\n");break;
case APPROCHE_CERISE_1_B: printf(">etat_strat:APPROCHE_CERISE_1_B|t\n");break;
case ATTRAPE_CERISE_1: printf(">etat_strat:ATTRAPE_CERISE_1|t\n");break;
case APPROCHE_PANIER_1: printf(">etat_strat:APPROCHE_PANIER_1|t\n");break;
case CALAGE_PANIER_1: printf(">etat_strat:CALAGE_PANIER_1|t\n");break;
case RECULE_PANIER: printf(">etat_strat:RECULE_PANIER|t\n");break;
case LANCE_DANS_PANIER: printf(">etat_strat:LANCE_DANS_PANIER|t\n");break;
case STRATEGIE_FIN: printf(">etat_strat:STRATEGIE_FIN|t\n");break;
}*/
sleep_ms(100);
}
}
int test_strategie(){
printf("A - Accoster.\n");
printf("C - Couleur et tirette.\n");
printf("D - Attraper cerises laterales.\n");
printf("E - Evitement\n");
printf("H - Homologation.\n");
printf("L - Longer.\n");
printf("P - Panier.\n");
int lettre;
do{
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
switch(lettre){
case 'a':
case 'A':
while(test_accostage());
break;
case 'c':
case 'C':
while(test_tirette_et_couleur());
break;
case 'd':
case 'D':
while(test_cerise_laterales_droite());
break;
case 'e':
case 'E':
while(test_evitement());
break;
case 'g':
case 'G':
while(test_cerise_laterales_gauche());
break;
case 'h':
case 'H':
//while(test_homologation());
break;
case 'l':
case 'L':
while(test_longe());
break;
case 'p':
case 'P':
while(test_panier());
break;
case 'q':
case 'Q':
return 0;
default:
return 1;
}
}
int test_cerise_laterales_gauche(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle_old;
enum etat_action_t etat_action;
printf("Attaper cerise latérales\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
temps_cycle_old= time_us_32();
uint32_t tempo_ms=1000;
Localisation_set(250, 1500, -150 * DEGRE_EN_RADIAN);
do{
etat_action = ACTION_EN_COURS;
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
static enum {
TEMPO_AVANT,
TEST,
TEMPO_APRES
}etat_test;
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_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 TEMPO_AVANT:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEST;
}
break;
case TEST:
if(cerises_attraper_cerises_gauches(_step_ms) == ACTION_TERMINEE){
tempo_ms = 1000;
etat_test = TEMPO_APRES;
}
break;
case TEMPO_APRES:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEMPO_AVANT;
etat_action = ACTION_TERMINEE;
}
break;
}
}
//lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_cerise_laterales_droite(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle_old;
enum etat_action_t etat_action;
printf("Attaper cerise latérales\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
temps_cycle_old= time_us_32();
uint32_t tempo_ms=1000;
Localisation_set(1775, 1500, 30 * DEGRE_EN_RADIAN);
do{
etat_action = ACTION_EN_COURS;
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
// Routines à 1 ms
if(temps_ms != Temps_get_temps_ms()){
static enum {
TEMPO_AVANT,
TEST,
TEMPO_APRES
}etat_test;
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
AsserMoteur_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 TEMPO_AVANT:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEST;
}
break;
case TEST:
if(cerises_attraper_cerises_droite(_step_ms) == ACTION_TERMINEE){
tempo_ms = 1000;
etat_test = TEMPO_APRES;
}
break;
case TEMPO_APRES:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test = TEMPO_AVANT;
etat_action = ACTION_TERMINEE;
}
break;
}
}
//lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(etat_action == ACTION_EN_COURS);
Moteur_Stop();
return 0;
}
int test_homologation(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0;
printf("Homologation\n");
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(1);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
temps_cycle_old= time_us_32();
do{
/*temps_cycle[index_temps_cycle++]= time_us_32() - temps_cycle_old;
if(index_temps_cycle >= 10){
for(int i=0; i<10; i++){
printf("t_cycle:%d\n", temps_cycle[i]);
}
index_temps_cycle=0;
}
temps_cycle_old=time_us_32();*/
temps_cycle_check();
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);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
//Homologation(_step_ms);
}
//lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(1);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
void affichage_test_evitement(){
while(1){
for(uint8_t capteur=0; capteur<12; capteur++){
printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
}
printf(">distance_minimale:%f\n",Trajet_get_obstacle_mm());
}
}
int test_evitement(){
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("Evitement\n");
printf("A - Sans evitement.\n");
printf("B - Pause devant obstacle.\n");
printf("C - Arrêt devant obstacle.\n");
printf("D - Retour si obstacle.\n");
printf("E - Contournement.\n");
do{
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
switch(lettre){
case 'a':
case 'A':evitement=SANS_EVITEMENT;break;
case 'b':
case 'B':evitement=PAUSE_DEVANT_OBSTACLE;break;
case 'c':
case 'C':evitement=ARRET_DEVANT_OBSTACLE;break;
case 'd':
case 'D':evitement=RETOUR_SI_OBSTABLE;break;
case 'e':
case 'E':evitement=CONTOURNEMENT;break;
}
i2c_maitre_init();
Trajet_init();
Balise_VL53L1X_init();
Localisation_set(200,200,0);
//printf("Init gyroscope\n");
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
Trajet_config(100, 500);
multicore_launch_core1(affichage_test_evitement);
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);
}
}
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
1000,200,
0, 0); // Angles
etat_action = Strategie_parcourir_trajet(trajectoire, _step_ms, evitement);
}
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);
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
int test_panier(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
printf("Test panier\n");
static enum{
TEST_PANIER_INIT,
TEST_PANIER_ASPIRE,
TEST_PANIER_TEMPO,
TEST_PANIER_LANCE_BALLES,
TEST_PANIER_PORTE_OUVERTE,
} etat_test_panier=TEST_PANIER_INIT;
static uint32_t tempo_ms;
i2c_maitre_init();
Trajet_init();
//printf("Init gyroscope\n");
set_position_avec_gyroscope(0);
if(get_position_avec_gyroscope()){
Gyro_Init();
}
stdio_flush();
multicore_launch_core1(affichage_test_strategie);
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);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
if(get_position_avec_gyroscope()){
Gyro_Read(_step_ms_gyro);
}
}
switch(etat_test_panier){
case TEST_PANIER_INIT:
if(lire_couleur()== COULEUR_VERT){
i2c_annexe_ferme_porte();
i2c_annexe_active_turbine();
etat_test_panier=TEST_PANIER_ASPIRE;
}
break;
case TEST_PANIER_ASPIRE:
if(lire_couleur()== COULEUR_BLEU){
i2c_annexe_desactive_turbine();
tempo_ms = 3000;
etat_test_panier=TEST_PANIER_TEMPO;
}
break;
case TEST_PANIER_TEMPO:
if(temporisation_terminee(&tempo_ms, _step_ms)){
etat_test_panier=TEST_PANIER_LANCE_BALLES;
}
break;
case TEST_PANIER_LANCE_BALLES:
if(lance_balles(_step_ms, 10) == ACTION_TERMINEE){
etat_test_panier=TEST_PANIER_PORTE_OUVERTE;
}
break;
case TEST_PANIER_PORTE_OUVERTE:
if(temporisation_terminee(&tempo_ms, _step_ms)){
i2c_annexe_ouvre_porte();
}
if(lire_couleur()== COULEUR_BLEU){
etat_test_panier=TEST_PANIER_INIT;
}
break;
}
}
lettre = getchar_timeout_us(0);
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}while(1);
printf("STRATEGIE_LOOP_2\n");
printf("Lettre : %d; %c\n", lettre, lettre);
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 0;
}
int test_longe(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
i2c_maitre_init();
Trajet_init();
//printf("Init gyroscope\n");
//Gyro_Init();
stdio_flush();
//set_position_avec_gyroscope(1);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_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);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
// Gyro_Read(_step_ms_gyro);
}
if(temps_ms > temps_ms_init + 200){
if(avance_puis_longe_bordure(LONGER_VERS_A) == ACTION_TERMINEE){
printf("Accostage_terminee\n");
}
}
}
lettre = getchar_timeout_us(0);
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
printf("Lettre : %d; %c\n", lettre, lettre);
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 1;
}
int test_accostage(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
i2c_maitre_init();
Trajet_init();
//printf("Init gyroscope\n");
//Gyro_Init();
stdio_flush();
//set_position_avec_gyroscope(1);
temps_ms = Temps_get_temps_ms();
temps_ms_init = temps_ms;
do{
i2c_gestion(i2c0);
i2c_annexe_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);
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
// Gyro_Read(_step_ms_gyro);
}
if(temps_ms > temps_ms_init + 200){
if(cerise_accostage() == ACTION_TERMINEE){
printf("Accostage_terminee\n");
}
}
}
lettre = getchar_timeout_us(0);
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
printf("Lettre : %d; %c\n", lettre, lettre);
if(lettre == 'q' && lettre == 'Q'){
return 0;
}
return 1;
}
int test_tirette_et_couleur(){
int lettre;
uint couleur, tirette;
enum couleur_t couleur_old;
couleur_old = COULEUR_INCONNUE;
printf("Tirette et couleur\n");
i2c_maitre_init();
stdio_flush();
tirette= attente_tirette();
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
printf(">tirette:%d\n", attente_tirette());
if(lire_couleur() == COULEUR_VERT){
printf(">couleur:Vert|t\n");
}else{
printf(">couleur:Bleu|t\n");
}
if(attente_tirette()){
if(couleur_old != lire_couleur() || tirette != attente_tirette()){
tirette = attente_tirette();
couleur_old = lire_couleur();
if(couleur_old == COULEUR_VERT){
// Tout vert
i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
}else{
// Tout bleu
i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
}
}
}else{
if(tirette != attente_tirette()){
tirette = attente_tirette();
// Tout libre
i2c_annexe_couleur_balise(0, 0x00);
}
}
sleep_ms(10);
lettre = getchar_timeout_us(0);
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
}
Reference in New Issue View Git Blame Copy Permalink