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

683 lines
21 KiB
C
Raw Permalink Blame History

#include <stdio.h>
#include <math.h>
#include "pico/multicore.h"
#include "Asser_Moteurs.h"
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "gyro.h"
#include "Localisation.h"
#include "Log.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "Temps.h"
#include "Robot_config.h"
int test_avance(void);
int test_cde_vitesse(void);
int test_cde_vitesse_rotation(void);
int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm);
int test_cde_vitesse_rectangle(void);
int test_cde_vitesse_cercle(void);
int test_trajectoire(void);
int test_aller_retour(void);
int test_endurance_aller_retour(void);
int test_trajectoire_calibration(void);
bool continuous_printf=true;
#define TEST_TIMEOUT_US 10000000
// Mode test : renvoie 0 pour quitter le mode test
int mode_test_deplacement(){
static int iteration = 2;
printf("Appuyez sur une touche pour entrer en mode test :\n");
printf("A - AsserMoteur - pour avancer selon Y\n");
printf("B - AsserMoteur - Commande en vitesse...\n");
printf("C - Trajectoires - sans gyroscope...\n");
printf("D - Aller-retour - avec gyroscope...\n");
printf("E - Aller-retour - Endurance - avec gyroscope...\n");
printf("F - Calibration distance\n");
stdio_flush();
int rep = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
switch (rep)
{
case 'a':
case 'A':
while(test_avance());
break;
case 'b':
case 'B':
while(test_cde_vitesse());
break;
case 'c':
case 'C':
while(test_trajectoire());
break;
case 'd':
case 'D':
while(test_aller_retour());
break;
case 'e':
case 'E':
while(test_endurance_aller_retour());
break;
case 'f':
case 'F':
while(test_trajectoire_calibration());
break;
case PICO_ERROR_TIMEOUT:
iteration--;
if(iteration == 0){
//printf("Sortie du mode test\n");
//return 0;
}
default:
printf("Commande inconnue\n");
break;
}
return 1;
}
/// @brief Avance doucement (<10cm/s) selon l'axe X
/// @param
/// @return
int test_avance(void){
int lettre;
int _step_ms = 1;
AsserMoteur_Init();
AsserMoteur_setConsigne_mm_s(MOTEUR_A, -100);
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
return 0;
}
int test_cde_vitesse(){
printf("A - Commande en vitesse - rectangle\n");
printf("B - Commande en vitesse - cercle\n");
printf("C - Commande en vitesse - rotation pure\n");
printf("D - Commande en vitesse - rotation par rapport à un point (contacteur longer A)\n");
printf("E - Commande en vitesse - rotation par rapport à un point (contacteur longer C)\n");
int lettre;
do{
lettre = getchar_timeout_us(0);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');
switch(lettre){
case 'a':
case 'A':
while(test_cde_vitesse_rectangle());
break;
case 'b':
case 'B':
while(test_cde_vitesse_cercle());
break;
case 'c':
case 'C':
while(test_cde_vitesse_rotation());
break;
case 'd':
case 'D':
while(test_cde_rotation_ref_robot(RAYON_ROBOT, 0));
break;
case 'e':
case 'E':
while(test_cde_rotation_ref_robot(RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2));
break;
}
}
int test_cde_vitesse_rotation(){
int lettre, _step_ms = 1;
float vitesse =90.0/2 * 3.14159 /180.0;
AsserMoteur_Init();
printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);
commande_vitesse(0, 0, vitesse);
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
Moteur_Stop();
return 0;
}
int test_cde_rotation_ref_robot(float centre_x_mm, float centre_y_mm){
int lettre, _step_ms = 1;
float vitesse =90.0/4 * 3.14159 /180.0;
AsserMoteur_Init();
printf("Rotation du robot par rapport au point (Rayon, O)\nVitesse : %f rad/s\n", vitesse);
commande_rotation(vitesse, centre_x_mm, centre_y_mm);
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
Moteur_Stop();
return 0;
}
int test_cde_vitesse_rectangle(){
int lettre, _step_ms = 1, temps_ms=0;
AsserMoteur_Init();
printf("déplacement en rectangle du robot : 500x200 mm, 100 mm/s\n");
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
if(temps_ms < 5000){
commande_vitesse(0, 100, 0);
}else if(temps_ms < 7000){
commande_vitesse(-100, 0, 0);
}else if(temps_ms < 12000){
commande_vitesse(0, -100, 0);
}else if(temps_ms < 14000){
commande_vitesse(100, 0, 0);
}else{
temps_ms = 0;
}
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
Moteur_Stop();
return 0;
}
int test_cde_vitesse_cercle(){
int lettre, _step_ms = 1, temps_ms=0;
AsserMoteur_Init();
printf("déplacement en cercle du robot : 100 mm/s\n");
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
commande_vitesse(cos((float)temps_ms / 1000.) * 200.0, sin((float)temps_ms /1000.) * 200.0, 0);
temps_ms += _step_ms;
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
Moteur_Stop();
return 0;
}
void test_trajectoire_teleplot(){
struct position_t _position, _consigne;
_consigne = Trajet_get_consigne();
while(1){
_consigne = Trajet_get_consigne();
_position = Localisation_get();
uint32_t temps;
temps = time_us_32()/1000;
if(continuous_printf){
printf(">X:%ld:%f\n>Y:%ld:%f\n>orientation:%ld:%f\n", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180);
printf(">Consigne_X:%ld:%f\n>Consigne_Y:%ld:%f\n>Consigne_orientation:%ld:%f\n", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180);
printf(">Position:%f:%f:%ld|xy\n>Consigne_Position:%f:%f:%ld|xy\n", _position.x_mm, _position.y_mm, temps, _consigne.x_mm, _consigne.y_mm, temps);
printf(">V_A:%ld:%f\n>V_B:%ld:%f\n>V_C:%ld:%f\n", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME));
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));
}
Log_gestion();
}
}
/// @brief Lance le robot sur une trajectoire (Bezier, circulaire ou droite)
/// Localisation sans Gyroscope
/// @return
int test_trajectoire(){
int lettre, _step_ms = 1, temps_ms=0;
Trajet_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
printf("B - Bezier\n");
printf("C - Circulaire\n");
printf("D - Droite\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'b':
case 'B':
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
printf("Trajectoire Bezier\n");
break;
case 'c':
case 'C':
Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0);
printf("Trajectoire circulaire\n");
break;
case 'd':
case 'D':
Trajectoire_droite(&trajectoire, 0, 0, 1000, 0, 0, 0);
printf("Trajectoire droite\n");
break;
default: return 0;
}
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_teleplot);
do{
// Routines à 1 ms
QEI_update();
Localisation_gestion();
if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
}else{
AsserMoteur_Gestion(_step_ms);
}
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
lettre = PICO_ERROR_TIMEOUT;
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_aller_retour(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
const float corr_angle = 1.145;
Trajet_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
printf("B - Bezier\n");
printf("C - Circulaire\n");
printf("D - Droite\n");
printf("E - Avance et tourne (ok)\n");
printf("F - Avance et tourne (Nok)\n");
printf("G - Avance (Calibre angle)\n");
printf("R - Rotation pure\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'b':
case 'B':
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;
case 'c':
case 'C':
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0);
printf("Trajectoire circulaire\n");
break;
case 'd':
case 'D':
Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, 0);
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
printf("Trajectoire droite\n");
break;
case 'e':
case 'E':
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajectoire_droite(&trajectoire, 0, 0, 0, 1500, 0, M_PI);
printf("Trajectoire droite avec rotation (OK)\n");
break;
case 'f':
case 'F':
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI);
printf("Trajectoire droite avec rotation (Nok)\n");
break;
case 'g':
case 'G':
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajectoire_droite(&trajectoire, 0, 0,
2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)),
0, 0);
printf("Trajectoire droite pour calibration angle de départ\n");
break;
case 'r':
case 'R':
Trajectoire_rotation(&trajectoire, 0, 0, 0, 700);
trajectoire.orientation_debut_rad = 0;
trajectoire.orientation_fin_rad = M_PI;
printf("Trajectoire rotation pure\n");
break;
default: return 0;
}
printf("Init gyroscope\n");
Gyro_Init();
printf("C'est parti !\n");
stdio_flush();
set_position_avec_gyroscope(1);
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_teleplot);
temps_ms = Temps_get_temps_ms();
do{
// Routines à 1 ms
while(temps_ms == Temps_get_temps_ms());
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
Trajectoire_inverse(&trajectoire);
Trajet_debut_trajectoire(trajectoire);
}else{
AsserMoteur_Gestion(_step_ms);
}
lettre = getchar_timeout_us(0);
//lettre = PICO_ERROR_TIMEOUT;
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
printf("Lettre : %d; %c\n", lettre, lettre);
Moteur_Stop();
multicore_reset_core1();
return 0;
}
/// @brief Fonction de test d'endurance, notamment pour valider le bon fonctionnement du gyroscope
/// sur le long terme et en dynamique. Tente aussi de valider les enregistrements des messages de log.
/// Pour l'instant aucune erreur enregistrée, donc pas hyper probant.
/// @return
int test_endurance_aller_retour(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
const float corr_angle = 1.145;
Trajet_init();
Log_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
printf("B - Bezier\n");
printf("C - Circulaire\n");
printf("D - Droite\n");
printf("E - Avance et tourne (ok)\n");
printf("F - Avance et tourne (Nok)\n");
printf("G - Avance (Calibre angle)\n");
printf("R - Rotation pure\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
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);
Log_message("Trajectoire de Bézier\n", INFO);
break;
case 'c':
case 'C':
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0);
printf("Trajectoire circulaire\n");
break;
case 'd':
case 'D':
Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, 0);
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
printf("Trajectoire droite\n");
break;
case 'e':
case 'E':
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI);
printf("Trajectoire droite avec rotation (OK)\n");
break;
case 'f':
case 'F':
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI);
printf("Trajectoire droite avec rotation (Nok)\n");
break;
case 'g':
case 'G':
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
Trajectoire_droite(&trajectoire, 0, 0,
2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)),
0, 0);
printf("Trajectoire droite pour calibration angle de départ\n");
break;
case 'r':
case 'R':
Trajectoire_rotation(&trajectoire, 0, 0, 0, 700);
trajectoire.orientation_debut_rad = 0;
trajectoire.orientation_fin_rad = M_PI;
printf("Trajectoire droite avec rotation\n");
break;
default: return 0;
}
printf("Init gyroscope\n");
Gyro_Init();
//printf("C'est parti !\n");
stdio_flush();
set_position_avec_gyroscope(1);
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_teleplot);
temps_ms = Temps_get_temps_ms();
int nb_aller=0;
do{
// Routines à 1 ms
while(temps_ms == Temps_get_temps_ms());
temps_ms = Temps_get_temps_ms();
QEI_update();
Localisation_gestion();
// Routine à 2 ms
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
// Routine à 100 ms
if(temps_ms % 100 == 0){
char message[256];
uint32_t temps;
struct position_t _position, _consigne;
temps = time_us_32()/1000;
_consigne = Trajet_get_consigne();
_position = Localisation_get();
/*sprintf(message, ">X:%ld:%.2f\n>Y:%ld:%.2f\n>orientation:%ld:%.2f", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180);
Log_message(message, TELEPLOT);
sprintf(message, ">C_X:%ld:%.2f\n>C_Y:%ld:%.2f\n>C_orientation:%ld:%.2f", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180);
Log_message(message, TELEPLOT);
sprintf(message, ">V_A:%ld:%.2f\n>V_B:%ld:%.2f\n>V_C:%ld:%.2f", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME));
Log_message(message, TELEPLOT);
sprintf(message, ">Vc_A:%ld:%.2f\n>Vc_B:%ld:%.2f\n>Vc_C:%ld:%.2f", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
Log_message(message, TELEPLOT);*/
}
if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
char message[256];
nb_aller++;
Trajectoire_inverse(&trajectoire);
Trajet_debut_trajectoire(trajectoire);
if(nb_aller % 2){
sprintf(message, "Aller %d\n", nb_aller / 2 +1);
}else{
sprintf(message, "retour %d\n", nb_aller / 2);
}
Log_message(message, INFO);
}else{
AsserMoteur_Gestion(_step_ms);
}
lettre = getchar_timeout_us(0);
//lettre = PICO_ERROR_TIMEOUT;
}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
printf("Lettre : %d; %c\n", lettre, lettre);
Moteur_Stop();
continuous_printf = 0;
printf("Boucle Log\n");
do{
Log_gestion();
lettre = getchar_timeout_us(0);
if(lettre == 'L'){
Log_get_full_log();
}
}while(lettre != 'q');
printf("Fin boucle Log\n");
return 0;
}
/// @brief Lance le robot sur une trajectoire (Bezier, circulaire ou droite)
/// Localisation avec Gyroscope
/// @return
int test_trajectoire_calibration(){
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2;
Trajet_init();
struct trajectoire_t trajectoire;
printf("Choix trajectoire :\n");
printf("A - Rotation sans gyro\n");
printf("B - Bezier\n");
printf("C - Circulaire\n");
printf("D - Droite\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'a':
case 'A':
Localisation_set(0, 0, 0);
set_position_avec_gyroscope(0);
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
Trajectoire_rotation(&trajectoire, 0, 0, 0, 4 * M_PI);
printf("Rotation sans gyro\n");
break;
case 'b':
case 'B':
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0);
printf("Trajectoire Bezier\n");
break;
case 'c':
case 'C':
Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0);
printf("Trajectoire circulaire\n");
break;
case 'd':
case 'D':
Localisation_set(250, 250, 180. * DEGRE_EN_RADIAN );
set_position_avec_gyroscope(1);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
Localisation_get().x_mm, Localisation_get().y_mm + 1000,
Localisation_get().angle_radian, Localisation_get().angle_radian - (M_PI / 2.));
printf("Trajectoire droite\n");
break;
default: return 0;
}
if(get_position_avec_gyroscope()){
printf("Init gyroscope\n");
Gyro_Init();
}
Trajet_debut_trajectoire(trajectoire);
multicore_launch_core1(test_trajectoire_teleplot);
do{
// Routines à 1 ms
QEI_update();
Localisation_gestion();
// Routine à 2 ms
if(get_position_avec_gyroscope()){
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
}
if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
Moteur_SetVitesse(MOTEUR_C, 0);
}else{
AsserMoteur_Gestion(_step_ms);
}
sleep_ms(_step_ms);
temps_ms += _step_ms;
lettre = getchar_timeout_us(0);
lettre = PICO_ERROR_TIMEOUT;
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
Reference in New Issue View Git Blame Copy Permalink