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

Déplacement des tests des fonctions déplacements (pas encore 100% testé) + réalisation des fonctions I2C pour piloter les actionneurs des pots

Browse Source
This commit is contained in:
Samuel 2024-03-17 22:47:43 +01:00
parent 79d51fbc56
commit c1e11676c2
9 changed files with 504 additions and 401 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Geometrie_robot.h
View File

@ -1,4 +1,4 @@
#define DISTANCE_ROUES_CENTRE_MM 84.25
#define DISTANCE_ROUES_CENTRE_MM 112
// Distance entre le centre du robot et un angle du robot
#define RAYON_ROBOT 125

4
QEI.c
View File

@ -86,8 +86,8 @@ void QEI_update(void){
}
/// @brief Renvoi le nombre d'impulsion du module QEI depuis la lecture précédente
/// Les signe sont inversés (sauf A) car le reducteur inverse le sens de rotation.
/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électornique
/// Les signes sont inversés (sauf A) car le réducteur inverse le sens de rotation.
/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électronique
/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
/// @return Nombre d'impulsion calculé lors du dernier appel de la function QEI_Update()
int QEI_get(enum QEI_name_t qei){

36
Readme.md
View File

@ -77,4 +77,38 @@ Le fichier [i2c_maitre.c](i2c_maitre.c) propose une implémentation non-bloquant
- La fonction *i2c_gestion* doit être appelée régulièrement.
- La fonction *i2c_transmission* (ou une fonction englobant celle-ci, telle que *i2c_lire_registre_nb*) doit être appelée jusqu'à ce qu'elle renvoie I2C_SUCCES.
Pour un exemple concret lisant une valeur dans une mémoire i2c, voir *test_i2c_lecture_pico_annex_nb2* de [Test.c](Test.c)
Pour un exemple concret lisant une valeur dans une mémoire i2c, voir *test_i2c_lecture_pico_annex_nb2* de [Test.c](Test.c)
Communication
=====================
Carte Servomoteur
-----------------
Discussion en I2C avec la carte servomoteur, celle-ci étant esclave.
Registres:
Adresse, RW, Fonction
* 0x00 (R) : Position du bras 1
* 0x01 (R) : Position du bras 2
* 0x02 (R) : Position du bras 3
* 0x03 (R) : Position du bras 4
* 0x04 (R) : Position du bras 5
* 0x05 (R) : Position du bras 5
* 0x06 (RW) : Action demandée / action en cours
'G' : Demande d'attraper plante et dépose à gauche
'g' : Action en cours: attraper plante et dépose à gauche
'D' : Demande d'attraper plante et dépose à droite
'd' : Action en cours: attraper plante et dépose à droite
't' : Action terminée
Position du bras : 1 octet, découpé ainsi :
- 4 premiers bits : position de l'aimant
- 4 derniers bits : hauteur du bras

379
Test.c
View File

@ -42,21 +42,14 @@
#define TEST_TIMEOUT_US 10000000
int test_APDS9960(void);
int test_localisation(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();
int test_endurance_aller_retour();
void test_trajectoire_teleplot();
int test_capteurs_balise(void);
int test_geometrie(void);
int test_angle_balise(void);
int continuous_printf = 1;
// Mode test : renvoie 0 pour quitter le mode test
int mode_test(){
@ -69,8 +62,6 @@ int mode_test(){
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("N - Fonctions geometrique\n");
printf("O - Analyse obstacle\n");
printf("P - Asser Position - perturbation\n");
@ -120,16 +111,7 @@ int mode_test(){
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 'N':
case 'n':
@ -156,11 +138,6 @@ int mode_test(){
while(test_log());
break;
case 'T':
case 't':
while(test_trajectoire());
break;
case 'U':
case 'u':
while(test_i2c());
@ -170,11 +147,6 @@ int mode_test(){
case 'v':
while(test_APDS9960());
break;
case 'W':
case 'w':
while(test_endurance_aller_retour());
break;
case 'Z':
case 'z':
@ -255,357 +227,6 @@ int test_APDS9960(){
return 1;
}
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();
}
}
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 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;
}
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

19
Test_i2c.c
View File

@ -290,7 +290,8 @@ int test_i2c_ecriture_pico_annex_nb_2(){
enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
printf("A - Aimant tient pot\n");
printf("C - Aimant lache pot\n");
printf("D - Deguisement On\n");
printf("E - Deguisement Off\n");
printf("F - Ferme porte\n");
@ -321,6 +322,22 @@ int test_i2c_ecriture_pico_annex_nb_2(){
if(lettre != PICO_ERROR_TIMEOUT && lettre != '\0'){
printf("lettre !\n");
switch(lettre){
case 'a':
case 'A':
for(int i=1; i<7; i++){
i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE);
}
printf("=> Lache pot\n");
break;
case 'c':
case 'C':
for(int i=1; i<7; i++){
i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_TIENT);
}
printf("=> Tient pot\n");
break;
case 'd':
case 'D':
i2c_annexe_active_deguisement();

387
Tests_deplacement.c
View File

@ -2,11 +2,18 @@
#include <math.h>
#include "pico/multicore.h"
#include "QEI.h"
#include "Moteurs.h"
#include "Asser_Moteurs.h"
#include "Geometrie_robot.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);
@ -15,6 +22,12 @@ 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);
bool continuous_printf=true;
#define TEST_TIMEOUT_US 10000000
// Mode test : renvoie 0 pour quitter le mode test
@ -23,6 +36,9 @@ int mode_test_deplacement(){
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("D - Aller-retour - Endurance - avec gyroscope...\n");
stdio_flush();
@ -39,6 +55,21 @@ int mode_test_deplacement(){
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 PICO_ERROR_TIMEOUT:
iteration--;
if(iteration == 0){
@ -204,4 +235,354 @@ int test_cde_vitesse_cercle(){
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, 700, 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_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 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;
}

4
Trajet.c
View File

@ -3,7 +3,9 @@
#include "Trajectoire.h"
#include "Trajet.h"
#include "Asser_Position.h"
#include "Asser_Moteurs.h"
#include "Monitoring.h"
#include "Temps.h"
float Trajet_calcul_vitesse(float temps_s);
int Trajet_terminee(float abscisse);
@ -25,6 +27,8 @@ float vitesse_max_contrainte_obstacle;
/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
void Trajet_init(){
Temps_init();
AsserMoteur_Init();
abscisse = 0;
vitesse_mm_s = 0;
position_mm = 0;

60
i2c_annexe.c
View File

@ -16,52 +16,73 @@
#define TAILLE_DONNEES_EMISSION 6
#define TAILLE_DONNEES_RECEPTION 15
#define ADRESSE_PIC18F4550 0x31
#define ADRESSE_PIC18F4550_DEBUT_W 0x00
#define TAILLE_DONNEES_PIC18F4550_EMISSION 7
uint8_t donnees_emission[TAILLE_DONNEES_EMISSION];
uint8_t donnees_emission_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION];
uint8_t donnees_reception[TAILLE_DONNEES_RECEPTION];
uint donnees_a_envoyer=0;
uint donnees_a_envoyer_pic=0;
void i2c_annexe_gestion(){
static enum {
EMISSION_DONNEES,
EMISSION_TEMPO,
RECEPTION_DONNEES
} etat_i2c_annexe=EMISSION_DONNEES;
EMISSION_DONNEES_RPi,
EMISSION_TEMPO_RPi,
RECEPTION_DONNEES_RPi,
EMISSION_PIC18F,
} etat_i2c_annexe=EMISSION_PIC18F;
enum i2c_resultat_t retour_i2c;
static uint32_t temps;
const uint32_t tempo = 1000;
switch(etat_i2c_annexe){
case EMISSION_DONNEES:
case EMISSION_DONNEES_RPi:
if(donnees_a_envoyer){
retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_W, donnees_emission, TAILLE_DONNEES_EMISSION);
if(retour_i2c == I2C_SUCCES){
etat_i2c_annexe = EMISSION_TEMPO;
etat_i2c_annexe = EMISSION_TEMPO_RPi;
donnees_a_envoyer=0;
temps = time_us_32();
}
}else{
etat_i2c_annexe = EMISSION_TEMPO;
etat_i2c_annexe = EMISSION_TEMPO_RPi;
temps = time_us_32();
}
break;
case EMISSION_TEMPO:
case EMISSION_TEMPO_RPi:
if(temps + tempo < time_us_32() ){
etat_i2c_annexe = RECEPTION_DONNEES;
etat_i2c_annexe = RECEPTION_DONNEES_RPi;
}
break;
case RECEPTION_DONNEES:
case RECEPTION_DONNEES_RPi:
retour_i2c = i2c_lire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_R, donnees_reception, TAILLE_DONNEES_RECEPTION);
if(retour_i2c == I2C_SUCCES){
etat_i2c_annexe = EMISSION_DONNEES;
etat_i2c_annexe = EMISSION_PIC18F;
temps = time_us_32();
}
break;
case EMISSION_PIC18F:
if(donnees_a_envoyer_pic){
retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PIC18F4550, ADRESSE_PIC18F4550_DEBUT_W,
donnees_emission_pic18f4550, TAILLE_DONNEES_PIC18F4550_EMISSION);
if(retour_i2c == I2C_SUCCES){
etat_i2c_annexe = EMISSION_DONNEES_RPi;
temps = time_us_32();
donnees_a_envoyer_pic=0;
}
}else{
etat_i2c_annexe = EMISSION_DONNEES_RPi;
}
break;
}
}
@ -152,4 +173,17 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
for (uint8_t capteur = 0; capteur < 12; capteur++){
distance_capteur_cm[capteur] = donnees_reception[capteur + ADRESSE_VL53L1X - ADRESSE_DEBUT_R];
}
}
}
/// @brief Envoie la consigne de position du servomoteur à la carte des servomoteurs
/// @param actionneur de 1 à 6, pour le "bras" correspondant".
/// @param pos_bras Code de position du bras, voir les #define BRAS_PLIE, ... définis plus haut ou dans le .h
/// @param pos_doigt Code de position du doigt, voir les #define DOIGT_LACHE, ... définis plus haut ou dans le .h
void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt){
if(actionneur < 1 || actionneur > 6){
printf("Erreur: i2c_annexe_actionneur_pot\n" );
return;
}
donnees_emission_pic18f4550[actionneur-1] = (pos_bras << 2) | pos_doigt;
donnees_a_envoyer_pic=1;
}

14
i2c_annexe.h
View File

@ -3,6 +3,16 @@
#define CONTACTEUR_ACTIF 0
#define CONTACTEUR_INACTIF 1
#define DOIGT_TIENT 1
#define DOIGT_LACHE 0
#define BRAS_PLIE 0
#define BRAS_POT_SOL 1
#define BRAS_POT_POT 2
#define BRAS_ECARTE 3
#define BRAS_DEPOSE_JARDINIERE 4
#define BRAS_HAUT 5
void i2c_annexe_gestion(void);
void i2c_annexe_active_turbine(void);
void i2c_annexe_desactive_turbine(void);
@ -31,4 +41,6 @@ void i2c_annexe_desactive_deguisement(void);
void i2c_annexe_plie_bras(void);
void i2c_annexe_deplie_bras(void);
uint8_t i2c_annexe_get_tension_batterie(void);
uint8_t i2c_annexe_get_tension_batterie(void);
void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt);