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

466 lines
17 KiB
C
Raw Blame History

#include "stdio.h"
#include "Strategie_prise_cerises.h"
#include "Commande_vitesse.h"
#include "Geometrie.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "math.h"
#include "i2c_annexe.h"
#include "Trajet.h"
// Rotation en rad/s pour accoster les cerises
#define ROTATION_CERISE 0.5f
// Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure.
#define MAX_LONGE_MM 240
#define MAX_ASPIRE_CERISE_MM 320
// Translation en mm/s pour aspirer les cerises
#define TRANSLATION_CERISE 70
// Seuil angulaire pour la détection de la fin de la barrette de cerises
#define SEUIL_ANGLE_CERISE (2. * DEGRE_EN_RADIAN)
void commande_rotation_contacteur_longer_A();
void commande_rotation_contacteur_longer_C();
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm);
enum etat_action_t demarre_turbine(uint32_t step_ms);
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
float vitesse_accostage_mm_s=100;
enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms){
static enum {
ATTRAPE_CERISE_DEMI_BAS,
REVENIR_CENTRE,
ATTRAPE_CERISE_DEMI_HAUT,
}etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS;
struct trajectoire_t trajectoire;
float angle_fin;
float pos_recal_x_mm = PETIT_RAYON_ROBOT_MM + 30;
switch (etat_attrappe_cerises_droite){
case ATTRAPE_CERISE_DEMI_BAS:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_droite = REVENIR_CENTRE;
}
break;
case REVENIR_CENTRE:
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30 * DEGRE_EN_RADIAN);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
2000 - 180, 1500 - 75, Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_HAUT;
}
break;
case ATTRAPE_CERISE_DEMI_HAUT:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
static enum {
ATTRAPE_CERISE_DEMI_BAS,
REVENIR_CENTRE,
ATTRAPE_CERISE_DEMI_HAUT,
}etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
struct trajectoire_t trajectoire;
float angle_fin;
float pos_recal_x_mm = PETIT_RAYON_ROBOT_MM + 30;
switch (etat_attrappe_cerises_gauche){
case ATTRAPE_CERISE_DEMI_BAS:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = REVENIR_CENTRE;
}
break;
case REVENIR_CENTRE:
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150 * DEGRE_EN_RADIAN);
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
180, 1500 - 75, Localisation_get().angle_radian, angle_fin);
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_HAUT;
}
break;
case ATTRAPE_CERISE_DEMI_HAUT:
if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){
etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm){
// 1 accoster
// Demarrer la turbine
// 2 Longer en aspirant
// 3 avancer en aspirant
static enum {
ACCOSTAGE,
DEMARRE_TURBINE,
LONGE,
TOURNE,
AVANCE,
}etat_attrappe_demi_cerise=ACCOSTAGE;
const float continu_avance_mm = 60;
static float orientation_ref;
static float pos_y_ref;
struct trajectoire_t trajectoire;
switch(etat_attrappe_demi_cerise){
case ACCOSTAGE:
if(cerise_accostage() == ACTION_TERMINEE){
Localisation_set_x(pos_recal_x_mm);
orientation_ref = Localisation_get().angle_radian;
etat_attrappe_demi_cerise = DEMARRE_TURBINE;
}
break;
case DEMARRE_TURBINE:
if(demarre_turbine(step_ms) == ACTION_TERMINEE){
etat_attrappe_demi_cerise = LONGE;
}
break;
case LONGE:
avance_puis_longe_bordure(longer_direction);
// La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
// Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
// ou 120 (MAX_LONGE_MM/2) du milieu de la bordure
// En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
/*if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){
etat_attrappe_demi_cerise = AVANCE;
}*/
if(fabs(orientation_ref - Localisation_get().angle_radian) > SEUIL_ANGLE_CERISE){
etat_attrappe_demi_cerise = TOURNE;
pos_y_ref = Localisation_get().y_mm;
}
break;
case TOURNE:
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, orientation_ref);
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
etat_attrappe_demi_cerise = AVANCE;
}
break;
case AVANCE:
if(longer_direction == LONGER_VERS_A){
commande_translation_longer_vers_A();
}else{
commande_translation_longer_vers_C();
}
if( (Localisation_get().y_mm > pos_y_ref + continu_avance_mm ) || (Localisation_get().y_mm < pos_y_ref - continu_avance_mm )){
etat_attrappe_demi_cerise = ACCOSTAGE;
i2c_annexe_desactive_turbine();
commande_vitesse_stop();
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
/// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure.
/// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
/// @param longer_direction : direction dans laquelle se trouve la bordure
/// @param step_ms : fréquence d'appel
/// @param pos_recalage_x_mm : Position de recalage contre le support de cerises
/// @param pos_recalage_y_mm : Position de recalage contre la bordure du terrain
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm, float pos_recalage_y_mm){
enum etat_action_t etat_action = ACTION_EN_COURS;
enum longer_direction_t longer_direction_aspire;
static uint32_t tempo_ms = 0;
static enum {
ATTRAPE_INIT,
ATTRAPE_VERS_BORDURE,
TURBINE_DEMARRAGE,
TURBINE_DEMARRAGE_TEMPO,
ASPIRE_LONGE,
ASPIRE_LIBRE,
ASPIRE_FIN
} etat_attrape=ATTRAPE_INIT;
switch(etat_attrape){
case ATTRAPE_INIT:
etat_attrape=ATTRAPE_VERS_BORDURE;
break;
case ATTRAPE_VERS_BORDURE:
avance_puis_longe_bordure(longer_direction);
if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
(longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
Localisation_set_x(pos_recalage_x_mm);
Localisation_set_y(pos_recalage_y_mm);
etat_attrape = TURBINE_DEMARRAGE;
}
break;
case TURBINE_DEMARRAGE:
i2c_annexe_ferme_porte();
i2c_annexe_active_turbine();
commande_vitesse_stop();
tempo_ms = 1000;
etat_attrape = TURBINE_DEMARRAGE_TEMPO;
break;
case TURBINE_DEMARRAGE_TEMPO:
if(tempo_ms < step_ms){
etat_attrape = ASPIRE_LONGE;
}else{
tempo_ms -= step_ms;
}
break;
case ASPIRE_LONGE:
longer_direction_aspire = inverser_longe_direction(longer_direction);
avance_puis_longe_bordure(longer_direction_aspire);
// La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
// Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
// En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) ||
((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){
etat_attrape = ASPIRE_LIBRE;
}
break;
case ASPIRE_LIBRE:
longer_direction_aspire = inverser_longe_direction(longer_direction);
if(longer_direction_aspire == LONGER_VERS_A){
commande_translation_longer_vers_A();
}else{
commande_translation_longer_vers_C();
}
if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_ASPIRE_CERISE_MM) )) ||
((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_ASPIRE_CERISE_MM))) ){
etat_attrape = ASPIRE_FIN;
}
break;
case ASPIRE_FIN:
commande_vitesse_stop();
i2c_annexe_desactive_turbine();
etat_action = ACTION_TERMINEE;
etat_attrape = ATTRAPE_INIT;
break;
}
return etat_action;
}
/// @brief Envoie l'ordre de démarrer la turbine puis attends 1 seconde
/// @param step_ms
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t demarre_turbine(uint32_t step_ms){
static enum {
TURBINE_DEMARRAGE,
TURBINE_DEMARRAGE_TEMPO,
} etat_demarrage_turbine=TURBINE_DEMARRAGE;
static uint32_t tempo_ms;
switch(etat_demarrage_turbine){
case TURBINE_DEMARRAGE:
i2c_annexe_ferme_porte();
i2c_annexe_active_turbine();
commande_vitesse_stop();
tempo_ms = 1000;
etat_demarrage_turbine = TURBINE_DEMARRAGE_TEMPO;
break;
case TURBINE_DEMARRAGE_TEMPO:
if(temporisation_terminee(&tempo_ms, step_ms)){
etat_demarrage_turbine = TURBINE_DEMARRAGE;
return ACTION_TERMINEE;
}
break;
}
return ACTION_EN_COURS;
}
/// @brief Fonction pour accoster et longer une bordure
/// @param longer_direction : direction dans laquelle le robot va aller une fois le long de la bordure
/// @return ACTION_EN_COURS
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction){
static enum {
LONGE_INIT,
LONGE_NORMAL,
LONGE_COLLE
} etat_longer_bord=LONGE_INIT;
switch (etat_longer_bord){
case LONGE_INIT:
etat_longer_bord=LONGE_NORMAL;
break;
case LONGE_NORMAL:
if(longer_direction==LONGER_VERS_A){
commande_translation_longer_vers_A();
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
etat_longer_bord = LONGE_COLLE;
}
}else{
commande_translation_longer_vers_C();
if( (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF) ||
(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF) ){
etat_longer_bord = LONGE_COLLE;
}
}
break;
case LONGE_COLLE:
if(cerise_accostage() == ACTION_TERMINEE){
etat_longer_bord = LONGE_NORMAL;
}
}
return ACTION_EN_COURS;
}
/// @brief Viens positionner le robot contre une bordure ou un support cerise devant lui.
enum etat_action_t cerise_accostage(void){
enum etat_action_t etat_action = ACTION_EN_COURS;
float rotation;
static enum {
CERISE_AVANCE_DROIT,
CERISE_TOURNE_CONTACTEUR_LONGER_A,
CERISE_TOURNE_CONTACTEUR_LONGER_C,
CERISE_ACCOSTE
} etat_accostage=CERISE_AVANCE_DROIT;
switch (etat_accostage)
{
case CERISE_AVANCE_DROIT:
commande_translation_avance_vers_trompe();
if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A;
}
if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_C;
}
if (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF && i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
etat_accostage=CERISE_ACCOSTE;
}
break;
case CERISE_TOURNE_CONTACTEUR_LONGER_A:
commande_rotation_contacteur_longer_A();
if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
etat_accostage = CERISE_AVANCE_DROIT;
}else{
etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_A;
}
}else if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){
etat_accostage = CERISE_ACCOSTE;
}
break;
case CERISE_TOURNE_CONTACTEUR_LONGER_C:
commande_rotation_contacteur_longer_C();
if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){
if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){
etat_accostage = CERISE_AVANCE_DROIT;
}else{
etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_C;
}
}else if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
etat_accostage = CERISE_ACCOSTE;
}
break;
case CERISE_ACCOSTE:
commande_vitesse_stop();
etat_accostage = CERISE_AVANCE_DROIT;
etat_action = ACTION_TERMINEE;
break;
default:
break;
}
return etat_action;
}
void commande_rotation_contacteur_longer_A(){
commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0);
}
void commande_rotation_contacteur_longer_C(){
commande_rotation(-ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0);
}
void commande_translation_longer_vers_A(){
// V_x : V * cos (60°) = V / 2
// V_y : V * sin (60°) = V * RACINE(3) / 2
commande_vitesse(TRANSLATION_CERISE/2., TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
}
void commande_translation_longer_vers_C(){
// V_x : -V * cos (60°) = -V / 2
// V_y : -V * sin (60°) = -V * RACINE(3) / 2
commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
}
void commande_translation_avance_vers_trompe(){
commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
}
void commande_translation_recule_vers_trompe(){
commande_vitesse(-vitesse_accostage_mm_s * cos(-M_PI/6), -vitesse_accostage_mm_s * sin(-M_PI/6), 0);
}
void commande_translation_avance_vers_A(){
// V_x : V * cos (60°) = V / 2
// V_y : V * sin (60°) = V * RACINE(3) / 2
commande_vitesse(vitesse_accostage_mm_s/2., vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0);
}
void commande_translation_avance_vers_C(){
// V_x : -V * cos (60°) = -V / 2
// V_y : -V * sin (60°) = -V * RACINE(3) / 2
commande_vitesse(-vitesse_accostage_mm_s/2., -vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0);
}
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction){
if(direction ==LONGER_VERS_A){
return LONGER_VERS_C;
}
return LONGER_VERS_A;
}
Reference in New Issue View Git Blame Copy Permalink