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

1172 lines
32 KiB
C
Raw Blame History

#include <stdio.h>
#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "pico/stdio.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "pico/binary_info.h"
#include "math.h"
#include "Test.h"
#include "APDS_9960.h"
#include "gyro.h"
#include "Asser_Moteurs.h"
#include "Asser_Position.h"
#include "Balise_VL53L1X.h"
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Monitoring.h"
#include "QEI.h"
#include "Robot_config.h"
#include "Servomoteur.h"
#include "spi_nb.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "Test_gyro.h"
#include "Test_i2c.h"
#include "Test_log.h"
#include "Test_strategie.h"
#include "Test.h"
#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000
int test_APDS9960(void);
int test_moteurs(void);
int test_QIE(void);
int test_QIE_mm(void);
int test_vitesse_moteur(enum t_moteur moteur);
int test_asser_moteur(void);
int test_localisation(void);
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_asser_position_avance(void);
int test_asser_position_avance_et_tourne(int, int);
int test_transition_gyro_pas_gyro(void);
int test_trajectoire(void);
void affiche_localisation(void);
int test_aller_retour();
void test_trajectoire_teleplot();
int test_capteurs_balise(void);
int test_geometrie(void);
int test_angle_balise(void);
// Mode test : renvoie 0 pour quitter le mode test
int mode_test(){
static int iteration = 2;
printf("Appuyez sur une touche pour entrer en mode test :\n");
printf("A - pour asser_moteurs (rotation)\n");
printf("B - pour avance (asser_moteur)\n");
printf("C - pour les codeurs\n");
printf("D - pour les codeurs (somme en mm)\n");
printf("E - Commande en vitesse...\n");
printf("F - Strategie...\n");
printf("G - Lecture des capteurs\n");
printf("H - Asser Position - avance\n");
printf("I - Asser Position - avance et tourne (gyro)\n");
printf("J - Asser Position - avance et tourne (sans gyro)\n");
printf("K - Trajets aller retour avec Gyro\n");
printf("L - pour la localisation\n");
printf("M - pour les moteurs\n");
printf("N - Fonctions geometrique\n");
printf("O - Analyse obstacle\n");
printf("P - Asser Position - perturbation\n");
printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
printf("R - Test des logs...\n");
printf("S - Test du gyroscope...\n");
printf("T - Trajectoire\n");
printf("U - Tests i2c...\n");
printf("V - APDS_9960\n");
stdio_flush();
int rep = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
switch (rep)
{
case 'a':
case 'A':
while(test_asser_moteur());
break;
case 'b':
case 'B':
while(test_avance());
break;
case 'C':
case 'c':
while(test_QIE());
break;
case 'D':
case 'd':
while(test_QIE_mm());
break;
case 'E':
case 'e':
while(test_cde_vitesse());
break;
case 'F':
case 'f':
while(test_strategie());
break;
case 'G':
case 'g':
while(test_capteurs_balise());
break;
case 'H':
case 'h':
while(test_asser_position_avance());
break;
case 'I':
case 'i':
while(test_asser_position_avance_et_tourne(1, 0));
break;
case 'J':
case 'j':
while(test_asser_position_avance_et_tourne(0, 0));
break;
case 'K':
case 'k':
while(test_aller_retour());
break;
case 'L':
case 'l':
while(test_localisation());
break;
case 'M':
case 'm':
while(test_moteurs());
break;
case 'N':
case 'n':
while(test_geometrie());
break;
case 'O':
case 'o':
while(test_angle_balise());
break;
case 'P':
case 'p':
while(test_asser_position_avance_et_tourne(1, 1));
break;
case 'Q':
case 'q':
while(test_transition_gyro_pas_gyro());
break;
case 'R':
case 'r':
while(test_log());
break;
case 'S':
case 's':
while(test_gyro());
break;
case 'T':
case 't':
while(test_trajectoire());
break;
case 'U':
case 'u':
while(test_i2c());
break;
case 'V':
case 'v':
while(test_APDS9960());
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;
}
int test_continue_test(){
int lettre;
//printf("q pour quitter, une autre touche pour un nouveau test.\n");
do{
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
switch(lettre){
case 'q':
case 'Q':
return 0;
default:
return 1;
}
}
int test_capteurs_balise(void){
printf("Test de la balise\n");
i2c_maitre_init();
Localisation_set(0,0,0);
Balise_VL53L1X_init();
while(true){
uint8_t min_distance;
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
min_distance = Balise_VL53L1X_get_min_distance();
printf(">min_distance:%d\n",min_distance);
for(uint8_t capteur=0; capteur<12; capteur++){
printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
}
sleep_ms(20);
}
}
int test_APDS9960(){
int lettre;
printf("Initialisation\n");
APDS9960_Init();
printf("Lecture...\n");
/*
do{
APDS9960_Lire();
lettre = getchar_timeout_us(0);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);*/
while(1){
APDS9960_Lire();
sleep_ms(100);
}
return 1;
}
void test_trajectoire_printf(){
struct position_t _position;
while(1){
_position = Trajet_get_consigne();
printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180);
}
}
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;
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));
}
}
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_DROIT);
Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 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 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();
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;
}
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, 0, 700, 0, 0);
printf("Trajectoire droite\n");
break;
default: return 0;
}
sleep_ms(3000);
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;
}
/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
/// @param propulseur : 1 pour activer le propulseur toutes les secondes
/// @return
int test_transition_gyro_pas_gyro(void){
int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
struct position_t position_consigne;
position_consigne.angle_radian = 0;
position_consigne.x_mm = 0;
position_consigne.y_mm = 0;
printf("Le robot tourne sur lui-même, transition sans gyro @ t=5s\n");
printf("Init gyroscope\n");
Gyro_Init();
printf("C'est parti !\n");
stdio_flush();
set_position_avec_gyroscope(1);
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
temps_ms_init = temps_ms;
multicore_launch_core1(affiche_localisation);
do{
while(temps_ms == Temps_get_temps_ms());
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
QEI_update();
if(get_position_avec_gyroscope()){
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
}
if(temps_ms - temps_ms_init > 5000){
set_position_avec_gyroscope(0);
}
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
Asser_Position(position_consigne);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
printf("lettre : %c, %d\n", lettre, lettre);
return 0;
}
/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
/// @param propulseur : 1 pour activer le propulseur toutes les secondes
/// @return
int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){
int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
struct position_t position_consigne;
position_consigne.angle_radian = 0;
position_consigne.x_mm = 0;
position_consigne.y_mm = 0;
printf("Le robot avance à 100 mm/s\n");
if(m_gyro){
printf("Init gyroscope\n");
Gyro_Init();
printf("C'est parti !\n");
}
if(propulseur){
i2c_maitre_init();
}
stdio_flush();
set_position_avec_gyroscope(m_gyro);
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
temps_ms_init = temps_ms;
multicore_launch_core1(affiche_localisation);
do{
while(temps_ms == Temps_get_temps_ms()){
if(propulseur){
i2c_gestion(i2c0);
i2c_annexe_gestion();
}
}
if(propulseur){
tempo_prop++;
if(tempo_prop>1000){
tempo_prop=0;
if(propulseur_on){
i2c_annexe_active_propulseur();
}else{
i2c_annexe_desactive_propulseur();
}
propulseur_on = !propulseur_on;
}
}
temps_ms = Temps_get_temps_ms();
temps_ms_old = temps_ms;
QEI_update();
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
if(!propulseur){
position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.;
}
Asser_Position(position_consigne);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
printf("lettre : %c, %d\n", lettre, lettre);
return 0;
}
int test_asser_position_avance(){
int lettre, _step_ms = 1, temps_ms=0;
struct position_t position;
position.angle_radian = 0;
position.x_mm = 0;
position.y_mm = 0;
printf("Le robot avance à 100 mm/s\n");
do{
QEI_update();
Localisation_gestion();
AsserMoteur_Gestion(_step_ms);
if(temps_ms < 5000){
position.y_mm = (float) temps_ms * 100. / 1000.;
}else if(temps_ms < 10000){
position.y_mm = 1000 - (float) temps_ms * 100. / 1000.;
}else{
temps_ms = 0;
}
Asser_Position(position);
temps_ms += _step_ms;
sleep_ms(_step_ms);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
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;
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;
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;
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;
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;
}
int test_avance(void){
int lettre;
int _step_ms = 1;
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;
}
void affiche_localisation(){
struct position_t position;
while(1){
position = Localisation_get();
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());
}
}
void test_asser_moteur_printf(){
int _step_ms = 1;
while(1){
printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
//sleep_ms(5);
}
}
int test_asser_moteur(){
int lettre;
int _step_ms = 1;
printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n");
AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100);
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
AsserMoteur_setConsigne_mm_s(MOTEUR_C, 100);
multicore_launch_core1(test_asser_moteur_printf);
do{
QEI_update();
AsserMoteur_Gestion(_step_ms);
sleep_ms(_step_ms);
//printf("Vitesse A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
//printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
// AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _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);
multicore_reset_core1();
return 0;
}
int test_QIE(){
int lettre;
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
do{
QEI_update();
printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n",
QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_QIE_mm(){
int lettre;
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
float a_mm=0, b_mm=0, c_mm=0;
do{
QEI_update();
a_mm += QEI_get_mm(QEI_A_NAME);
b_mm += QEI_get_mm(QEI_B_NAME);
c_mm += QEI_get_mm(QEI_C_NAME);
printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_localisation(){
int lettre;
int moteurs = 0;
struct position_t position;
uint32_t temps_ms;
uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
uint32_t m_gyro = 0;
int16_t vitesse;
int propulseur=0;
printf("A - Sans gyroscope\n");
printf("B - Avec Gyroscope\n");
printf("C - Avec Gyroscope + moteurs\n");
do{
lettre = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre == PICO_ERROR_TIMEOUT);
switch(lettre){
case 'A':
case 'a':
set_position_avec_gyroscope(0);
printf("Sans gyroscope\n");
break;
case 'c':
case 'C':
moteurs = 1;
i2c_maitre_init();
case 'B':
case 'b':
set_position_avec_gyroscope(1);
printf("Avec gyroscope, initialisation...\n");
m_gyro=1;
Gyro_Init();
break;
default:
return 0;
}
temps_ms = Temps_get_temps_ms();
multicore_launch_core1(affiche_localisation);
vitesse = (int16_t) 32766.0;
printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
while(temps_ms == Temps_get_temps_ms());
QEI_update();
if(m_gyro){
if(temps_ms % _step_ms_gyro == 0){
Gyro_Read(_step_ms_gyro);
}
}
Localisation_gestion();
position = Localisation_get();
if(moteurs){
tempo_moteur++;
if(tempo_moteur > 1000){
tempo_moteur = 0;
vitesse = -vitesse;
Moteur_SetVitesse(MOTEUR_A ,vitesse);
Moteur_SetVitesse(MOTEUR_B ,vitesse);
Moteur_SetVitesse(MOTEUR_C ,vitesse);
if(propulseur){
i2c_annexe_active_propulseur();
}else{
i2c_annexe_desactive_propulseur();
}
propulseur = !propulseur;
}
}
lettre = getchar_timeout_us(0);
temps_ms += _step_ms;
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
multicore_reset_core1();
return 0;
}
int test_moteurs(){
int lettre_moteur;
printf("Indiquez le moteurs à tester (A, B ou C):\n");
do{
lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
}while(lettre_moteur == PICO_ERROR_TIMEOUT);
printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur);
switch (lettre_moteur)
{
case 'A':
case 'a':
while(test_vitesse_moteur(MOTEUR_A));
break;
case 'B':
case 'b':
while(test_vitesse_moteur(MOTEUR_B));
break;
case 'C':
case 'c':
while(test_vitesse_moteur(MOTEUR_C));
break;
case 'Q':
case 'q':
return 0;
break;
default:
break;
}
return 1;
}
int test_vitesse_moteur(enum t_moteur moteur){
printf("Vitesse souhaitée :\n0 - 0%%\n1 - 10%%\n2 - 20%%\n...\n9 - 90%%\nA - 100%%\n");
int vitesse_moteur;
do{
vitesse_moteur = getchar_timeout_us(0);
stdio_flush();
}while(vitesse_moteur == PICO_ERROR_TIMEOUT);
switch (vitesse_moteur)
{
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
printf("Vitesse choisie : %c0%%\n", vitesse_moteur);
Moteur_SetVitesse(moteur, (vitesse_moteur - '0') * 32767.0 / 10.);
break;
case 'A':
case 'a':
printf("Vitesse choisie : 100%%\n");
Moteur_SetVitesse(moteur, (int16_t) 32766.0);
break;
case 'b':
case 'B':
printf("Vitesse choisie : -50%%\n");
Moteur_SetVitesse(moteur, (int16_t) -32766.0/2);
break;
case 'q':
case 'Q':
return 0;
break;
default:
break;
}
return 1;
}
int test_geometrie(){
float angle = 270, angle_min, angle_max;
printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
angle = 180;
printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
angle = 181;
printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
angle = 179;
printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
angle_min = -100;
angle_max = -80;
angle = -90;
printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_min, angle_max,
Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
angle = 90;
printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min,
Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
angle = -120;
printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min,
Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
angle_min = 178;
angle_max = 182;
angle = 179;
printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_min, angle_max,
Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
angle = 177;
printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min,
Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
angle = 183;
printf("Anglee %f° compris entre %f° et %f° : %d\n", angle, angle_max, angle_min,
Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
return 0;
}
void affiche_monitoring(){
float distance;
while(1){
temps_cycle_display();
printf(">distance:%f\n",Trajet_get_obstacle_mm());
sleep_ms(100);
}
}
int test_angle_balise(void){
int lettre;
float distance, angle=3.1281;
enum {
TEST_BLEU,
ATTENTE1,
TEST_VERT,
ATTENTE2,
TEST_RESET,
ATTENTE3,
TEST_ANGLE
}etat_test_led=TEST_BLEU;
i2c_maitre_init();
Balise_VL53L1X_init();
Localisation_set(1000,1500,0);
multicore_launch_core1(affiche_monitoring);
uint temps_ms, timer_ms=1000;
temps_ms = Temps_get_temps_ms();
do{
temps_cycle_check();
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
switch(etat_test_led){
case TEST_BLEU:
i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
timer_ms--;
if(timer_ms<2){
etat_test_led=TEST_VERT;
timer_ms=1000;
}
break;
case TEST_VERT:
i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
timer_ms--;
if(timer_ms<2){
etat_test_led=TEST_RESET;
timer_ms=10000;
}
break;
case TEST_RESET:
i2c_annexe_couleur_balise(0, 0x00);
timer_ms--;
if(timer_ms<2){
etat_test_led=TEST_ANGLE;
timer_ms=0;
}
break;
case TEST_ANGLE:
timer_ms++;
angle=(float)timer_ms / 1000.;
Trajet_set_obstacle_mm(Balise_VL53L1X_get_distance_obstacle_mm(angle));
break;
}
}
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
return 0;
}
Reference in New Issue View Git Blame Copy Permalink