436 lines
14 KiB
C
436 lines
14 KiB
C
#include "Holonome2023.h"
|
|
#include "Demonstration.h"
|
|
|
|
#define TEST_TIMEOUT_US 10000000
|
|
#define CAPTEUR_POUR_ATTENTE 11
|
|
|
|
|
|
int Demonstration_init(void);
|
|
enum etat_action_t Demonstration_calage();
|
|
enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm);
|
|
enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees);
|
|
enum etat_action_t Demonstration_bezier();
|
|
enum etat_action_t Demonstration_attente();
|
|
|
|
|
|
uint32_t temps_ms_demo = 0, temps_ms_old;
|
|
|
|
void demo_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() / 10.);
|
|
printf(">capteur:%d\n", Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE));
|
|
}
|
|
}
|
|
|
|
int Demonstration_menu(void){
|
|
static int iteration = 2;
|
|
int rep;
|
|
printf("Mode demo - init\n");
|
|
Demonstration_init();
|
|
while(1){
|
|
do{
|
|
printf("A - Calage dans l'angle\n");
|
|
printf("B - Trajets unitaires - Rectangle\n");
|
|
printf("C - Trajets unitaires - Droit puis tourne\n");
|
|
printf("D - Trajets unitaires - Bezier\n");
|
|
printf("E - Test attente\n");
|
|
/*printf("C - Trajets enchaines - manuels\n");
|
|
printf("D - Trajets enchaines - auto\n");
|
|
printf("E - Asservissement angulaire\n");*/
|
|
printf("Q - Quitter\n");
|
|
rep = getchar_timeout_us(TEST_TIMEOUT_US);
|
|
}while(rep == 0 || rep == PICO_ERROR_TIMEOUT);
|
|
|
|
switch (rep)
|
|
{
|
|
case 'a':
|
|
case 'A':
|
|
Demonstration_calage();
|
|
break;
|
|
|
|
case 'b':
|
|
case 'B':
|
|
printf("Demonstration rectangle\n");
|
|
Demonstration_rectangle(550, 1000);
|
|
printf("Recalage\n");
|
|
Demonstration_calage();
|
|
break;
|
|
|
|
case 'c':
|
|
case 'C':
|
|
Demonstration_avance_puis_tourne(300, 1000, 720.);
|
|
break;
|
|
|
|
case 'd':
|
|
case 'D':
|
|
Demonstration_bezier();
|
|
break;
|
|
|
|
case 'E':
|
|
printf("Début attente\n");
|
|
Demonstration_attente();
|
|
printf("Fin attente\n");
|
|
break;
|
|
|
|
case 'q':
|
|
case 'Q':
|
|
return 0;
|
|
break;
|
|
|
|
case 'z':
|
|
case 'Z':
|
|
Demonstration_semiauto();
|
|
break;
|
|
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
int Demonstration_init(){
|
|
|
|
Holonome2023_init();
|
|
temps_ms_demo = Temps_get_temps_ms();
|
|
temps_ms_old = temps_ms_demo;
|
|
multicore_launch_core1(demo_affiche_localisation);
|
|
}
|
|
|
|
void Demonstration_semiauto(){
|
|
Demonstration_calage();
|
|
Demonstration_attente();
|
|
|
|
while(true){
|
|
Demonstration_rectangle(550, 1000);
|
|
Demonstration_calage();
|
|
Demonstration_attente();
|
|
|
|
Demonstration_avance_puis_tourne(300, 1000, 720.);
|
|
Demonstration_calage();
|
|
Demonstration_attente();
|
|
|
|
Demonstration_bezier();
|
|
Demonstration_calage();
|
|
Demonstration_attente();
|
|
}
|
|
}
|
|
|
|
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,
|
|
DETECTION_PROCHE,
|
|
FIN_ATTENTE
|
|
} etat_attente = ATTENTE_DETECTION;
|
|
uint32_t temps_debut_tempo, duree_tempo_ms = 50;
|
|
|
|
while(true){
|
|
Holonome_cyclique(PARAM_NO_MOTORS);
|
|
|
|
switch(etat_attente){
|
|
case ATTENTE_DETECTION:
|
|
if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 15 && Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 1){
|
|
/// Sans obstacle, le capteur peut renvoyer 0;
|
|
etat_attente=DETECTION_PROCHE;
|
|
temps_debut_tempo = time_us_32();
|
|
}
|
|
break;
|
|
|
|
case DETECTION_PROCHE:
|
|
if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
|
|
// On a perdu la detection avant le temps écoulé
|
|
etat_attente=ATTENTE_DETECTION;
|
|
}
|
|
if((temps_debut_tempo + (duree_tempo_ms * 1000)) < time_us_32()){
|
|
// temps écoulé
|
|
etat_attente=FIN_ATTENTE;
|
|
}
|
|
break;
|
|
|
|
case FIN_ATTENTE:
|
|
if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
|
|
// On a perdu la detection après le temps écoulé
|
|
etat_attente=ATTENTE_DETECTION;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
}
|
|
//sleep_ms(20);
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
enum etat_action_t Demonstration_calage(){
|
|
enum {
|
|
CALAGE,
|
|
DECALAGE,
|
|
CALAGE_TERMINE
|
|
} etat_calage = CALAGE;
|
|
while(true){
|
|
Holonome_cyclique(PARAM_DEFAULT);
|
|
struct trajectoire_t trajectoire;
|
|
|
|
// Toutes les 1 ms.
|
|
if(temps_ms_demo != Temps_get_temps_ms()){
|
|
temps_ms_demo = Temps_get_temps_ms();
|
|
|
|
|
|
switch (etat_calage)
|
|
{
|
|
case CALAGE:
|
|
if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
|
|
etat_calage = DECALAGE;
|
|
}
|
|
break;
|
|
|
|
case DECALAGE:
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
RAYON_ROBOT + 150, PETIT_RAYON_ROBOT_MM + 150,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_calage = CALAGE_TERMINE;
|
|
}
|
|
break;
|
|
|
|
|
|
case CALAGE_TERMINE:
|
|
etat_calage = CALAGE;
|
|
Moteur_Stop();
|
|
return ACTION_TERMINEE;
|
|
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
return ACTION_ECHEC;
|
|
}
|
|
|
|
|
|
/// @brief Deplacement suivant un rectangle.
|
|
/// @param avance_x_mm : Distance en X (conseillé 620)
|
|
/// @param avance_y_mm : Distance en Y (conseillé 1000)
|
|
/// @return ACTION_TERMINEE
|
|
enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
|
|
enum {
|
|
AVANCE_Y,
|
|
AVANCE_X,
|
|
RECULE_Y,
|
|
RECULE_X,
|
|
RECTANGLE_TERMINE
|
|
} etat_rectangle = AVANCE_Y;
|
|
while(true){
|
|
struct trajectoire_t trajectoire;
|
|
|
|
Holonome_cyclique(PARAM_DEFAULT);
|
|
|
|
// Toutes les 1 ms.
|
|
if(temps_ms_demo != Temps_get_temps_ms()){
|
|
temps_ms_demo = Temps_get_temps_ms();
|
|
|
|
|
|
switch (etat_rectangle)
|
|
{
|
|
case AVANCE_Y:
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm, Localisation_get().y_mm + avance_y_mm,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_rectangle = AVANCE_X;
|
|
}
|
|
break;
|
|
|
|
case AVANCE_X:
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm + avance_x_mm, Localisation_get().y_mm,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_rectangle = RECULE_Y;
|
|
}
|
|
break;
|
|
|
|
case RECULE_Y:
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm, Localisation_get().y_mm - avance_y_mm,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_rectangle = RECULE_X;
|
|
}
|
|
break;
|
|
|
|
case RECULE_X:
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_rectangle = RECTANGLE_TERMINE;
|
|
}
|
|
break;
|
|
|
|
case RECTANGLE_TERMINE:
|
|
etat_rectangle = AVANCE_Y;
|
|
Moteur_Stop();
|
|
return ACTION_TERMINEE;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Deplacement suivant une droite, rotation sur lui-même du robot une fois arrivée à destination,
|
|
/// puis retour en ligne droite
|
|
/// @param avance_x_mm : Distance en X (conseillé 300)
|
|
/// @param avance_y_mm : Distance en Y (conseillé 1000)
|
|
/// @param angle_degrees : Rotation du robot sur lui-même
|
|
/// @return ACTION_TERMINEE
|
|
enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees){
|
|
enum {
|
|
APT_AVANCE,
|
|
APT_TOURNE,
|
|
APT_RECULE,
|
|
APT_TERMINE
|
|
} etat_avance_puis_tourne = APT_AVANCE;
|
|
int pos_x_init_mm, pos_y_init_mm;
|
|
while(true){
|
|
struct trajectoire_t trajectoire;
|
|
|
|
Holonome_cyclique(PARAM_DEFAULT);
|
|
|
|
// Toutes les 1 ms.
|
|
if(temps_ms_demo != Temps_get_temps_ms()){
|
|
temps_ms_demo = Temps_get_temps_ms();
|
|
|
|
|
|
switch (etat_avance_puis_tourne)
|
|
{
|
|
case APT_AVANCE:
|
|
pos_x_init_mm = Localisation_get().x_mm;
|
|
pos_y_init_mm = Localisation_get().y_mm;
|
|
Trajectoire_droite(&trajectoire, pos_x_init_mm, pos_y_init_mm,
|
|
pos_x_init_mm + avance_x_mm, pos_y_init_mm + avance_y_mm,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_avance_puis_tourne = APT_TOURNE;
|
|
}
|
|
break;
|
|
|
|
case APT_TOURNE:
|
|
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
|
|
Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_avance_puis_tourne = APT_RECULE;
|
|
Trajet_config(TRAJECT_CONFIG_STD);
|
|
}
|
|
break;
|
|
|
|
case APT_RECULE:
|
|
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm - avance_y_mm,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_avance_puis_tourne = APT_TERMINE;
|
|
}
|
|
break;
|
|
|
|
case APT_TERMINE:
|
|
etat_avance_puis_tourne = APT_AVANCE;
|
|
Moteur_Stop();
|
|
return ACTION_TERMINEE;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
return ACTION_ECHEC;
|
|
}
|
|
|
|
/// @brief Déplacement suivant deux courbes de Bézier. Recommandé pour une démo sur une planche de 1m x 1,5m
|
|
/// @return ACTION_TERMINEE
|
|
enum etat_action_t Demonstration_bezier(){
|
|
enum {
|
|
BEZIER_ALLER,
|
|
BEZIER_RETOUR,
|
|
BEZIER_TERMINE
|
|
} etat_bezier = BEZIER_ALLER;
|
|
while(true){
|
|
struct trajectoire_t trajectoire;
|
|
static int pos_x_init_mm, pos_y_init_mm;
|
|
|
|
Holonome_cyclique(PARAM_DEFAULT);
|
|
Trajet_config(200, 200);
|
|
|
|
// Toutes les 1 ms.
|
|
if(temps_ms_demo != Temps_get_temps_ms()){
|
|
temps_ms_demo = Temps_get_temps_ms();
|
|
|
|
|
|
switch (etat_bezier)
|
|
{
|
|
case BEZIER_ALLER:
|
|
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
|
1386, Localisation_get().y_mm,
|
|
-576, 1500 - 276,
|
|
545, 1500 - 276,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_bezier = BEZIER_RETOUR;
|
|
}
|
|
break;
|
|
|
|
case BEZIER_RETOUR:
|
|
Trajectoire_bezier(&trajectoire,
|
|
Localisation_get().x_mm, Localisation_get().y_mm,
|
|
1391, Localisation_get().y_mm,
|
|
-76, 1500 - 788,
|
|
242, 1500 - 1289,
|
|
Localisation_get().angle_radian, Localisation_get().angle_radian);
|
|
if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
|
|
etat_bezier = BEZIER_TERMINE;
|
|
}
|
|
break;
|
|
|
|
case BEZIER_TERMINE:
|
|
etat_bezier = BEZIER_ALLER;
|
|
Trajet_config(TRAJECT_CONFIG_STD);
|
|
return ACTION_TERMINEE;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
return ACTION_ECHEC;
|
|
}
|