153 lines
3.9 KiB
C
153 lines
3.9 KiB
C
#include "pico/multicore.h"
|
|
#include "stdio.h"
|
|
#include "hardware/i2c.h"
|
|
|
|
#include "Asser_Moteurs.h"
|
|
#include "i2c_annexe.h"
|
|
#include "i2c_maitre.h"
|
|
#include "gyro.h"
|
|
#include "Localisation.h"
|
|
#include "QEI.h"
|
|
#include "Robot_config.h"
|
|
#include "Strategie.h"
|
|
#include "Temps.h"
|
|
#include "Trajet.h"
|
|
#include "Trajectoire.h"
|
|
#include "Test.h"
|
|
|
|
int test_accostage();
|
|
int test_longe();
|
|
|
|
void affichage_test_strategie(){
|
|
uint32_t temps;
|
|
temps = time_us_32()/1000;
|
|
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));
|
|
|
|
sleep_ms(10);
|
|
}
|
|
|
|
|
|
int test_strategie(){
|
|
printf("L - longer.\n");
|
|
printf("A - Accoster.\n");
|
|
int lettre;
|
|
do{
|
|
lettre = getchar_timeout_us(0);
|
|
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
|
|
switch(lettre){
|
|
case 'l':
|
|
case 'L':
|
|
while(test_longe());
|
|
break;
|
|
|
|
case 'a':
|
|
case 'A':
|
|
while(test_accostage());
|
|
break;
|
|
|
|
case 'q':
|
|
case 'Q':
|
|
return 0;
|
|
default:
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
|
|
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(cerise_longer_bord(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;
|
|
} |