286 lines
11 KiB
C
286 lines
11 KiB
C
#include "Commande_vitesse.h"
|
|
#include "Strategie_2024_plante.h"
|
|
#include "Geometrie_robot.h"
|
|
#include "Localisation.h"
|
|
#include "i2c_annexe.h"
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
|
|
#define ASSER_ANGLE_GAIN_PLANTE_P 1.5
|
|
#define ASSER_DISTANCE_GAIN_PLANTE_P 10
|
|
|
|
#define RAYON_ZONE_PLANTE_MM 180
|
|
|
|
struct position_t liste_zone_plante[]=
|
|
{
|
|
{.x_mm = 1500, .y_mm = 1500 },
|
|
{.x_mm = 1000, .y_mm = 1300 },
|
|
{.x_mm = 1000, .y_mm = 700 },
|
|
{.x_mm = 1500, .y_mm = 500 },
|
|
{.x_mm = 2000, .y_mm = 700 },
|
|
{.x_mm = 2000, .y_mm = 1300 }
|
|
};
|
|
|
|
|
|
|
|
enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms){
|
|
struct position_t position_robot, position_zone_plante;
|
|
float angle;
|
|
|
|
position_zone_plante = liste_zone_plante[zone_plante];
|
|
position_robot = Localisation_get();
|
|
angle = atan2f(position_zone_plante.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm);
|
|
return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms);
|
|
}
|
|
/// @brief Indique si la plante se trouve dans la zone de récole (en comptant une marge)
|
|
/// @param position_plante : position de la plante
|
|
/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
|
|
/// @return 1 si la plante est dans la zone, 0 si le robot ne trouve pas la plante ou qu'elle est hors zone.
|
|
bool est_dans_zone(struct position_t position_plante, enum zone_plante_t zone_plante){
|
|
struct position_t position_zone_plante;
|
|
float distance_plante_zone;
|
|
|
|
position_zone_plante = liste_zone_plante[zone_plante];
|
|
|
|
distance_plante_zone = sqrtf( powf(position_zone_plante.x_mm - position_zone_plante.x_mm, 2) +
|
|
powf(position_zone_plante.y_mm - position_zone_plante.y_mm, 2));
|
|
|
|
if(distance_plante_zone < RAYON_ZONE_PLANTE_MM){
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Déplace le robot vers une plante, vérifie que la plante est bien dans la zone plante qu'on vise
|
|
/// @param zone_plante : ID de la zone dans laquelle on pense trouver la plante
|
|
/// @return ACTION_SUCCESS si le robot est prêt à attraper la plante, ACTION_ECHEC si la plante
|
|
enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
|
|
static enum {
|
|
SAAP_INIT_DETECTION,
|
|
SAAP_ASSERV
|
|
} etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
enum validite_vl53l8_t validite;
|
|
float angle_rad, distance_mm, distance_obstacle, commande_vitesse_plante;
|
|
float distance_contrainte_obstacle, vitesse_max_contrainte_obstacle;
|
|
const float acceleration_mm_ss_obstacle=500;
|
|
static float distance_min_mm;
|
|
static int tempo_ms, tempo_asserv;
|
|
static bool entree_dans_zone;
|
|
|
|
switch(etat_aller_a_plante){
|
|
case SAAP_INIT_DETECTION:
|
|
i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
|
|
i2c_annexe_mi_ferme_doigt_plante();
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
|
|
tempo_ms = 2000;
|
|
distance_min_mm = 2000;
|
|
tempo_asserv = 500;
|
|
entree_dans_zone=false;
|
|
etat_aller_a_plante = SAAP_ASSERV;
|
|
break;
|
|
|
|
case SAAP_ASSERV:
|
|
i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
|
|
if(validite == VL53L8_PLANTE){
|
|
tempo_ms = 2000;
|
|
// 1 on s'assure que la plante est dans la zone qu'on recherche !
|
|
if(zone_plante != ZONE_PLANTE_AUCUNE){
|
|
// Cas où on checher une plante dans une zone
|
|
struct position_t position_robot, position_plante;
|
|
bool robot_dans_zone;
|
|
position_robot = Localisation_get();
|
|
position_robot.angle_radian += ANGLE_PINCE + angle_rad;
|
|
position_plante = Geometrie_deplace(position_robot, distance_mm);
|
|
if( !est_dans_zone(position_plante, zone_plante)){
|
|
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
return ACTION_ECHEC;
|
|
}
|
|
robot_dans_zone = est_dans_zone(Localisation_get(), zone_plante);
|
|
if(entree_dans_zone == true){
|
|
if(robot_dans_zone == false){
|
|
// Le robot est sorti de la zone
|
|
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
return ACTION_ECHEC;
|
|
}
|
|
}
|
|
if(robot_dans_zone == true){
|
|
entree_dans_zone = true;
|
|
}
|
|
}
|
|
|
|
// 2 on s'assure qu'il n'y a pas de robot en face (TODO)
|
|
distance_contrainte_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(ANGLE_PINCE);
|
|
if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
|
|
vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
|
|
}
|
|
|
|
// 2 bis, on s'assure que le robot se rapproche de la plante. Si ce n'est pas le cas, on arrête
|
|
if(distance_mm < distance_min_mm){
|
|
distance_min_mm = distance_mm;
|
|
tempo_asserv = 500;
|
|
}else{
|
|
tempo_asserv--;
|
|
if(tempo_asserv <= 0){
|
|
commande_vitesse(0, 0, 0);
|
|
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
return ACTION_ECHEC;
|
|
}
|
|
}
|
|
|
|
// 3 on asservi
|
|
commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P;
|
|
if(commande_vitesse_plante > 150){
|
|
commande_vitesse_plante = 150;
|
|
}
|
|
if(commande_vitesse_plante <= 0){
|
|
commande_vitesse_stop();
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
|
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
// On limite la vitesse avec l'obstacle
|
|
if (commande_vitesse_plante > vitesse_max_contrainte_obstacle){
|
|
commande_vitesse_plante = vitesse_max_contrainte_obstacle;
|
|
}
|
|
|
|
commande_vitesse(cosf(ANGLE_PINCE + 0.04) * commande_vitesse_plante ,
|
|
sinf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , (angle_rad - 0.04) * ASSER_ANGLE_GAIN_PLANTE_P);
|
|
}else{
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
etat_aller_a_plante = SAAP_INIT_DETECTION;
|
|
return ACTION_ECHEC;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
return ACTION_EN_COURS;
|
|
}
|
|
|
|
/// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé
|
|
/// @param step_ms
|
|
/// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6
|
|
/// @return ACTION_EN_COURS ou ACTION_TERMINEE ou ACTION_ECHEC (plante tombée devant le robot)
|
|
enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante){
|
|
static enum{
|
|
PDP_INIT,
|
|
PDP_ALLER_PLANTE,
|
|
PDP_PRE_PRISE_PLANTE,
|
|
PDP_PRE_PRISE_TEMPO,
|
|
PDP_PRE_PRISE_RECULE,
|
|
PDP_ATTRAPE_PLANTE,
|
|
PDP_RECULE,
|
|
PDP_TEMPO,
|
|
} etat_plante_dans_pot = PDP_ALLER_PLANTE;
|
|
static int tempo_ms;
|
|
struct position_t position_recule, position_initiale;
|
|
enum validite_vl53l8_t validite;
|
|
enum etat_action_t etat_action;
|
|
float angle_rad, distance_mm;
|
|
static int nb_essai_prise;
|
|
|
|
switch (etat_plante_dans_pot)
|
|
{
|
|
case PDP_INIT:
|
|
nb_essai_prise=0;
|
|
// Pas besoin de break ici
|
|
case PDP_ALLER_PLANTE:
|
|
etat_action = Strat_2024_aller_a_plante(zone_plante);
|
|
if (etat_action == ACTION_TERMINEE){
|
|
etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
|
|
}else if (etat_action == ACTION_ECHEC){
|
|
i2c_annexe_ouvre_doigt_plante();
|
|
return ACTION_ECHEC;
|
|
}
|
|
break;
|
|
|
|
case PDP_PRE_PRISE_PLANTE:
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
|
|
i2c_annexe_ferme_doigt_plante();
|
|
tempo_ms = 250;
|
|
etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO;
|
|
break;
|
|
|
|
case PDP_PRE_PRISE_TEMPO:
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
tempo_ms=500;
|
|
etat_plante_dans_pot=PDP_PRE_PRISE_RECULE;
|
|
}
|
|
break;
|
|
|
|
case PDP_PRE_PRISE_RECULE:
|
|
i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
|
|
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
|
|
}
|
|
position_initiale = Localisation_get();
|
|
position_initiale.angle_radian += ANGLE_PINCE;
|
|
position_recule = Geometrie_deplace(position_initiale, -100);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
|
|
if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
|
|
if(validite == VL53L8_PLANTE){
|
|
if(distance_mm > 90){
|
|
// Nous n'avons pas attrapé la plante avec le doigt, on recommence:
|
|
etat_plante_dans_pot = PDP_ALLER_PLANTE;
|
|
i2c_annexe_ouvre_doigt_plante();
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
|
nb_essai_prise++;
|
|
if(nb_essai_prise >= 3){
|
|
return ACTION_ECHEC;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
commande_vitesse_stop();
|
|
i2c_annexe_ouvre_doigt_plante();
|
|
i2c_annexe_attrape_plante(bras_depose_pot);
|
|
tempo_ms = 5000;
|
|
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
|
etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
|
|
}
|
|
break;
|
|
|
|
|
|
case PDP_ATTRAPE_PLANTE:
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
etat_plante_dans_pot=PDP_TEMPO;
|
|
if(bras_depose_pot == PLANTE_BRAS_1){
|
|
i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
|
|
}else{
|
|
i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
|
|
}
|
|
tempo_ms = 250;
|
|
}
|
|
break;
|
|
|
|
case PDP_TEMPO:
|
|
tempo_ms--;
|
|
if(tempo_ms <= 0){
|
|
etat_plante_dans_pot=PDP_RECULE;
|
|
}
|
|
break;
|
|
|
|
case PDP_RECULE:
|
|
tempo_ms--;
|
|
position_initiale = Localisation_get();
|
|
position_initiale.angle_radian += ANGLE_PINCE;
|
|
position_recule = Geometrie_deplace(position_initiale, -100);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT);
|
|
if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, EVITEMENT_PAUSE_DEVANT_OBSTACLE, step_ms) == ACTION_TERMINEE){
|
|
commande_vitesse_stop();
|
|
etat_plante_dans_pot=PDP_ALLER_PLANTE;
|
|
return ACTION_TERMINEE;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
return ACTION_EN_COURS;
|
|
} |