Rotation Fonctionnel + début de la séparation des tests unitaire

This commit is contained in:
Samuel 2024-03-11 20:33:37 +01:00
parent baf22acf62
commit d93db3cefb
12 changed files with 277 additions and 185 deletions

View File

@ -21,6 +21,8 @@ float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
float commande_I[3]; // Terme integral
void AsserMoteur_Init(){
QEI_init();
Moteur_Init();
for(unsigned int i =0; i< 3; i ++){
commande_I[i]=0;
consigne_mm_s[i]=0;

View File

@ -40,6 +40,7 @@ Test_gyro.c
Test_i2c.c
Test_log.c
Test_strategie.c
Tests_unitaires.c
Trajet.c
Trajectoire.c
Trajectoire_bezier.c

View File

@ -117,6 +117,24 @@ void Demonstration_semiauto(){
}
}
void Demonstration_auto(){
Demonstration_calage();
Demonstration_attente();
while(true){
Demonstration_rectangle(550, 1000);
Demonstration_calage();
Demonstration_avance_puis_tourne(300, 1000, 720.);
Demonstration_calage();
Demonstration_bezier();
Demonstration_calage();
Demonstration_attente();
}
}
enum etat_action_t Demonstration_attente(){
enum {
ATTENTE_DETECTION,

View File

@ -1,3 +1,4 @@
int Demonstration_menu(void);
void Demonstration_semiauto(void);
void Demonstration_auto(void);
int Demonstration_init(void);

View File

@ -40,7 +40,7 @@ int main() {
stdio_init_all();
Demonstration_init();Demonstration_semiauto();
//Demonstration_init();Demonstration_auto();
while(mode_test());
Holonome2023_init();

1
Log.c
View File

@ -32,6 +32,7 @@ void Log_init(void){
/// @param message : string, without '\n' at the end.
/// @param log_level : can be in TELEPLOT, TRACE, DEBUG, INFO, WARN, ERROR, FATAL
void Log_message(char * message, enum Log_level log_level){
printf("ERROR:%s\n",message);
if(log_error != LOG_ERROR_OK){
return;
}

View File

@ -92,6 +92,11 @@ void set_debug_varf(float variable){
void Monitoring_Error(char * msg){
gpio_put(LED_PIN_ROUGE, 1);
Log_message(msg, ERROR);
while(1){
sleep_ms(500);
printf(msg);
printf("\n");
}
}

6
QEI.c
View File

@ -23,6 +23,8 @@
struct QEI_t QEI_A, QEI_B, QEI_C;
bool QEI_est_init = false;
PIO pio_QEI = pio0;
const uint CODEUR_1_A = 26;
@ -31,7 +33,7 @@ const uint CODEUR_1_B = 27;
void QEI_init(){
// Initialisation des 3 modules QEI
// Chaque module QEI sera dans une machine à état du PIO 0
if(!QEI_est_init){
// Offset le début du programme
// Si ce n'est pas 0, le programme ne marchera pas
@ -59,6 +61,8 @@ void QEI_init(){
QEI_A.value=0;
QEI_B.value=0;
QEI_C.value=0;
QEI_est_init=true;
}
}

192
Test.c
View File

@ -34,17 +34,13 @@
#include "Test_i2c.h"
#include "Test_log.h"
#include "Test_strategie.h"
#include "Tests_unitaires.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);
@ -71,10 +67,8 @@ int continuous_printf = 1;
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("A - Tests unitaires\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");
@ -83,7 +77,6 @@ int mode_test(){
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");
@ -102,22 +95,12 @@ int mode_test(){
{
case 'a':
case 'A':
while(test_asser_moteur());
while(mode_test_unitaire());
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':
@ -159,11 +142,6 @@ int mode_test(){
while(test_localisation());
break;
case 'M':
case 'm':
while(test_moteurs());
break;
case 'N':
case 'n':
while(test_geometrie());
@ -390,7 +368,7 @@ int test_aller_retour(){
Trajectoire_rotation(&trajectoire, 0, 0, 0, 700);
trajectoire.orientation_debut_rad = 0;
trajectoire.orientation_fin_rad = M_PI;
printf("Trajectoire droite avec rotation\n");
printf("Trajectoire rotation pure\n");
break;
default: return 0;
@ -398,7 +376,7 @@ int test_aller_retour(){
printf("Init gyroscope\n");
Gyro_Init();
//printf("C'est parti !\n");
printf("C'est parti !\n");
stdio_flush();
set_position_avec_gyroscope(1);
@ -976,73 +954,6 @@ void affiche_localisation(){
}
}
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;
@ -1054,6 +965,10 @@ int test_localisation(){
int16_t vitesse;
int propulseur=0;
Temps_init();
QEI_init();
Localisation_init();
printf("A - Sans gyroscope\n");
printf("B - Avec Gyroscope\n");
printf("C - Avec Gyroscope + moteurs\n");
@ -1131,95 +1046,6 @@ int test_localisation(){
}
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);

228
Tests_unitaires.c Normal file
View File

@ -0,0 +1,228 @@
#include <stdio.h>
#include "pico/multicore.h"
#include "QEI.h"
#include "Moteurs.h"
#include "Asser_Moteurs.h"
#define TEST_TIMEOUT_US 10000000
int test_QIE(void);
int test_QIE_mm(void);
int test_asser_moteur(void);
int test_vitesse_moteur(enum t_moteur moteur);
int test_moteurs(void);
// Mode test : renvoie 0 pour quitter le mode test
int mode_test_unitaire(){
static int iteration = 2;
printf("Appuyez sur une touche pour entrer en mode test :\n");
printf("A - pour les codeurs\n");
printf("B - pour les codeurs (somme en mm)\n");
printf("C - pour les moteurs\n");
printf("D - pour asser_moteurs (rotation)\n");
stdio_flush();
int rep = getchar_timeout_us(TEST_TIMEOUT_US);
stdio_flush();
switch (rep)
{
case 'a':
case 'A':
while(test_QIE());
break;
case 'b':
case 'B':
while(test_QIE_mm());
break;
case 'C':
case 'c':
while(test_moteurs());
break;
case 'D':
case 'd':
while(test_asser_moteur());
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_QIE(){
int lettre;
printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
QEI_init();
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));
printf(">CodeurA:%.3f\n>CodeurB:%.3f\n>CodeurC:%.3f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_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");
QEI_init();
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);
printf(">CodeurA:%.3f\n>CodeurB:%.3f\n>CodeurC:%.3f\n", a_mm, b_mm, c_mm);
sleep_ms(100);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT);
return 0;
}
int test_moteurs(){
int lettre_moteur;
Moteur_Init();
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;
}
void test_asser_moteur_printf(){
int _step_ms = 1;
while(1){
printf(">Vitesse_A:%.0f\n>Vitesse_B:%.0f\n>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;
AsserMoteur_Init();
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;
}

2
Tests_unitaires.h Normal file
View File

@ -0,0 +1,2 @@
int mode_test_unitaire(void);

View File

@ -108,6 +108,10 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
case TRAJECTOIRE_BEZIER:
Trajectoire_bezier_get_longueur(trajectoire);
break;
case TRAJECTOIRE_ROTATION:
Trajectoire_rotation_get_longueur(trajectoire);
break;
}
}
return trajectoire->longueur;