252 lines
9.1 KiB
C
252 lines
9.1 KiB
C
#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"
|
|
|
|
// 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
|
|
|
|
void commande_rotation_contacteur_longer_A();
|
|
void commande_rotation_contacteur_longer_C();
|
|
void commande_translation_longer_vers_A();
|
|
void commande_translation_longer_vers_C();
|
|
enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
|
|
|
|
|
|
float vitesse_accostage_mm_s=100;
|
|
|
|
/// @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_VERS_A);
|
|
// 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 Z 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;
|
|
break;
|
|
}
|
|
return etat_action;
|
|
}
|
|
|
|
|
|
/// @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;
|
|
|
|
}
|
|
|
|
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_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
|
|
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);
|
|
}
|
|
|
|
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;
|
|
} |