WIP - Calage initial du robot - Pourrait être fonctionnel
This commit is contained in:
parent
32c21947d0
commit
68e16127ba
@ -32,8 +32,6 @@ Score.c
|
||||
Servomoteur.c
|
||||
Strategie.c
|
||||
Strategie_deplacement.c
|
||||
Strategie_prise_cerises.c
|
||||
Strategie_pousse_gateau.c
|
||||
Strategie_2024_pots.c
|
||||
Temps.c
|
||||
Test.c
|
||||
|
@ -200,9 +200,10 @@ enum etat_action_t Demonstration_calage(){
|
||||
switch (etat_calage)
|
||||
{
|
||||
case CALAGE:
|
||||
if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
|
||||
// TODO: Appeler la nouvelle fonction de prise de référentiel
|
||||
/*if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
|
||||
etat_calage = DECALAGE;
|
||||
}
|
||||
}*/
|
||||
break;
|
||||
|
||||
case DECALAGE:
|
||||
|
13
Geometrie.c
13
Geometrie.c
@ -1,4 +1,5 @@
|
||||
#include "Geometrie.h"
|
||||
#include "math.h"
|
||||
|
||||
|
||||
/// @brief Retourne l'angle entre -PI et +PI
|
||||
@ -68,4 +69,14 @@ unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Déplace un point de la distance indiquée en se servant de l'angle de la position donnée.
|
||||
struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){
|
||||
struct position_t position_arrivée;
|
||||
position_arrivée.angle_radian = position_depart.angle_radian;
|
||||
position_arrivée.x_mm = cosf(position_depart.angle_radian) * distance_mm;
|
||||
position_arrivée.y_mm = sinf(position_depart.angle_radian) * distance_mm;
|
||||
|
||||
return position_arrivée;
|
||||
}
|
||||
|
@ -1,7 +1,9 @@
|
||||
#define DISTANCE_ROUES_CENTRE_MM 112
|
||||
|
||||
// Distance entre le centre du robot et un angle du robot
|
||||
#define RAYON_ROBOT 125
|
||||
#define RAYON_ROBOT 160.
|
||||
#define DISTANCE_CENTRE_CAPTEUR 80.
|
||||
#define ANGLE_PINCE (-150 * DEGRE_EN_RADIAN)
|
||||
|
||||
// Distance entre le centre du robot et un bord du robot
|
||||
#define PETIT_RAYON_ROBOT_MM 108.2
|
||||
|
@ -86,10 +86,9 @@ int main() {
|
||||
|
||||
switch(statu_match){
|
||||
case MATCH_ATTENTE_TIRETTE:
|
||||
Strategie_preparation();
|
||||
if(lire_couleur() == COULEUR_VERT){
|
||||
// Tout vert
|
||||
i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
|
||||
if(lire_couleur() == COULEUR_JAUNE){
|
||||
// Tout jaune
|
||||
i2c_annexe_couleur_balise(0b11111100, 0x0FFF);
|
||||
}else{
|
||||
// Tout bleu
|
||||
i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
|
||||
@ -115,11 +114,9 @@ int main() {
|
||||
|
||||
case MATCH_ARRET_EN_COURS:
|
||||
commande_vitesse_stop();
|
||||
i2c_annexe_active_deguisement();
|
||||
Score_set_funny_action();
|
||||
if(Robot_est_dans_zone_depose(couleur)){
|
||||
Score_set_pieds_dans_plat();
|
||||
}
|
||||
}
|
||||
|
||||
if (timer_match_ms > 100000){ // 100 secondes
|
||||
statu_match = MATCH_TERMINEE;
|
||||
|
630
Strategie.c
630
Strategie.c
@ -7,8 +7,6 @@
|
||||
#include "Monitoring.h"
|
||||
#include "Moteurs.h"
|
||||
#include "Score.h"
|
||||
#include "Strategie_prise_cerises.h"
|
||||
#include "Strategie_pousse_gateau.h"
|
||||
#include "Strategie.h"
|
||||
#include "Trajet.h"
|
||||
#include "math.h"
|
||||
@ -51,6 +49,8 @@ int Robot_est_dans_zone_cerise_droite();
|
||||
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms);
|
||||
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms);
|
||||
|
||||
enum etat_action_t Strategie_calage(enum couleur_t couleur);
|
||||
|
||||
|
||||
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
|
||||
const uint32_t temps_pre_fin_match = (97000 - 10000);
|
||||
@ -94,336 +94,27 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
|
||||
|
||||
switch(etat_strategie){
|
||||
case STRATEGIE_INIT:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
|
||||
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
|
||||
struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
|
||||
struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS};
|
||||
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE};
|
||||
objectifs[0]= objectif_1;
|
||||
objectifs[1]= objectif_2;
|
||||
objectifs[2]= objectif_3;
|
||||
objectifs[3]= objectif_4;
|
||||
}else{
|
||||
Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
|
||||
struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
|
||||
struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE};
|
||||
struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS};
|
||||
struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE};
|
||||
objectifs[0]= objectif_1;
|
||||
objectifs[1]= objectif_2;
|
||||
objectifs[2]= objectif_3;
|
||||
objectifs[3]= objectif_4;
|
||||
}
|
||||
objectif_courant = &objectifs[4];
|
||||
Strategie_set_cerise_dans_robot(10);
|
||||
etat_strategie = LANCER_PANIER;
|
||||
break;
|
||||
|
||||
case ALLER_CERISE_BAS:
|
||||
|
||||
if(couleur == COULEUR_BLEU){
|
||||
angle_fin = 30. * DEGRE_EN_RADIAN;
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||||
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
650, Localisation_get().y_mm,
|
||||
490, 0,
|
||||
857, 0,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
}else{
|
||||
angle_fin = -150. * DEGRE_EN_RADIAN;
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||||
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
2000 - 650, Localisation_get().y_mm,
|
||||
2000 - 490, 0,
|
||||
2000 - 857, 0,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
}
|
||||
Trajet_config(500,250);
|
||||
|
||||
if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == TRAJET_TERMINE){
|
||||
etat_strategie = ATTRAPER_CERISE_BAS;
|
||||
}
|
||||
break;
|
||||
|
||||
case ATTRAPER_CERISE_BAS:
|
||||
recal_pos_y = RAYON_ROBOT;
|
||||
if(couleur == COULEUR_BLEU){
|
||||
longer_direction = LONGER_VERS_C;
|
||||
recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
|
||||
}else{
|
||||
longer_direction = LONGER_VERS_A;
|
||||
recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
|
||||
}
|
||||
|
||||
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
Strategie_set_cerise_dans_robot(6);
|
||||
etat_strategie = ALLER_PANIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case ALLER_CERISE_HAUT:
|
||||
|
||||
if(couleur == COULEUR_BLEU){
|
||||
angle_fin = 30. * DEGRE_EN_RADIAN;
|
||||
point_x = 857;
|
||||
}else{
|
||||
angle_fin = -150. * DEGRE_EN_RADIAN;
|
||||
point_x = 2000 - 857;
|
||||
}
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
Trajet_config(500, 250);
|
||||
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
|
||||
etat_strategie = ATTRAPER_CERISE_HAUT;
|
||||
}
|
||||
break;
|
||||
|
||||
case ATTRAPER_CERISE_HAUT:
|
||||
recal_pos_y = 3000 - RAYON_ROBOT;
|
||||
if(couleur == COULEUR_BLEU){
|
||||
longer_direction = LONGER_VERS_A;
|
||||
recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
|
||||
}else{
|
||||
longer_direction = LONGER_VERS_C;
|
||||
recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
|
||||
}
|
||||
|
||||
etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
Strategie_set_cerise_dans_robot(6);
|
||||
etat_strategie = ALLER_PANIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case ALLER_CERISE_GAUCHE:
|
||||
Trajet_config(250, 250);
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
||||
if(couleur == COULEUR_BLEU){
|
||||
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
|
||||
etat_strategie = ATTRAPER_CERISE_GAUCHE;
|
||||
Score_ajout_gateau(3);
|
||||
}
|
||||
}else{
|
||||
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
|
||||
etat_strategie = ATTRAPER_CERISE_GAUCHE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ATTRAPER_CERISE_GAUCHE:
|
||||
if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){
|
||||
Strategie_set_cerise_dans_robot(10);
|
||||
etat_strategie = ALLER_PANIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case ALLER_CERISE_DROITE:
|
||||
Trajet_config(250, 250);
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
||||
if(couleur == COULEUR_BLEU){
|
||||
if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
|
||||
etat_strategie = ATTRAPER_CERISE_DROITE;
|
||||
}
|
||||
}else{
|
||||
if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
|
||||
Score_ajout_gateau(3);
|
||||
etat_strategie = ATTRAPER_CERISE_DROITE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ATTRAPER_CERISE_DROITE:
|
||||
if(cerises_attraper_cerises_droite(step_ms) == ACTION_TERMINEE){
|
||||
Strategie_set_cerise_dans_robot(10);
|
||||
etat_strategie = ALLER_PANIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case ALLER_PANIER:
|
||||
if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){
|
||||
etat_strategie = LANCER_PANIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case LANCER_PANIER:
|
||||
if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){
|
||||
Score_ajout_cerise(Strategie_get_cerise_dans_robot());
|
||||
Strategie_set_cerise_dans_robot(0);
|
||||
objectif_courant->etat = FAIT;
|
||||
etat_strategie = DECISION_STRATEGIE;
|
||||
}
|
||||
break;
|
||||
|
||||
case DECISION_STRATEGIE:
|
||||
// Obtenir l'objectif suivant
|
||||
for(uint m_objectif=0; m_objectif < NB_OBJECTIFS; m_objectif++){
|
||||
if(objectif_courant->etat == FAIT && objectifs[m_objectif].etat == A_FAIRE){
|
||||
objectif_courant = &(objectifs[m_objectif]);
|
||||
}else if(objectif_courant->etat == A_FAIRE && objectifs[m_objectif].etat == A_FAIRE){
|
||||
if(objectif_courant->priorite > objectifs[m_objectif].priorite){
|
||||
objectif_courant = &(objectifs[m_objectif]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(objectif_courant->etat == FAIT || pre_fin_match_active){
|
||||
if(Strategie_calage_debut(COULEUR_BLEU, step_ms) == ACTION_TERMINEE){
|
||||
etat_strategie = STRATEGIE_FIN;
|
||||
}else{
|
||||
switch(objectif_courant->cible){
|
||||
case CERISE_BAS:
|
||||
etat_strategie = ALLER_CERISE_BAS;
|
||||
break;
|
||||
case CERISE_HAUT:
|
||||
etat_strategie = ALLER_CERISE_HAUT;
|
||||
break;
|
||||
case CERISE_GAUCHE:
|
||||
etat_strategie = ALLER_CERISE_GAUCHE;
|
||||
break;
|
||||
case CERISE_DROITE:
|
||||
etat_strategie = ALLER_CERISE_DROITE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case RETOUR_MAISON:
|
||||
i2c_annexe_plie_bras();
|
||||
i2c_annexe_desactive_turbine();
|
||||
if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){
|
||||
// Si le robot est dans la zone du panier, jeter les cerises s'il en a
|
||||
if(Strategie_get_cerise_dans_robot() > 0 && Robot_est_dans_zone_depose_panier(couleur)){
|
||||
//etat_strategie=LANCER_PANIER; // Il faut orienter le robot face au panier
|
||||
etat_strategie=STRATEGIE_FIN;
|
||||
}else{
|
||||
etat_strategie=STRATEGIE_FIN;
|
||||
}
|
||||
etat_strategie=STRATEGIE_FIN;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case STRATEGIE_FIN:
|
||||
pre_fin_match_active=true;
|
||||
i2c_annexe_desactive_propulseur();
|
||||
commande_vitesse_stop();
|
||||
i2c_annexe_couleur_balise(0xE0, 0x0FFF);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){
|
||||
return Gateau_pousse_proche(couleur, step_ms);
|
||||
|
||||
/*if(couleur == COULEUR_BLEU){
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
||||
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
740, 3000 - 550,
|
||||
510, 3000 - 1580,
|
||||
500, 1400,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
}else{
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
|
||||
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
2000 - 740, 3000 - 550,
|
||||
2000 - 510, 3000 - 1580,
|
||||
2000 - 500, 1400,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
}
|
||||
return parcourt_trajet_simple(trajectoire, step_ms);*/
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms){
|
||||
struct trajectoire_t trajectoire;
|
||||
float angle_fin;
|
||||
if(couleur == COULEUR_BLEU){
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
|
||||
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
785, 3000 - 550,
|
||||
1550, 3000 - 785,
|
||||
1775, 1500,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
}else{
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
|
||||
Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
2000 - 785, 3000 - 550,
|
||||
2000 - 1550, 3000 - 785,
|
||||
2000 - 1775, 1500,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
}
|
||||
return parcourt_trajet_simple(trajectoire, step_ms);
|
||||
}
|
||||
|
||||
|
||||
enum etat_action_t Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
|
||||
enum etat_action_t etat_action = ACTION_EN_COURS;
|
||||
struct trajectoire_t trajectoire;
|
||||
Trajet_config(TRAJECT_CONFIG_STD);
|
||||
|
||||
// Definition des trajectoires
|
||||
if(couleur == COULEUR_BLEU){
|
||||
// Si le robot est déjà dans la zone cerise haut
|
||||
// On va en ligne droite
|
||||
if(Robot_est_dans_zone_cerise_haut(couleur)){
|
||||
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
180,2800,
|
||||
Localisation_get().angle_radian,
|
||||
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
|
||||
}else if (Robot_est_dans_zone_cerise_gauche()){
|
||||
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
Localisation_get().x_mm + 330, Localisation_get().y_mm - 100,
|
||||
745, 3000 - 475,
|
||||
180, 3000 - 200,
|
||||
Localisation_get().angle_radian,
|
||||
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
|
||||
}else{
|
||||
// Sinon, on a une courbe de bézier
|
||||
Trajet_config(500,250);
|
||||
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
418, Localisation_get().y_mm,
|
||||
655, 2800,
|
||||
180, 2800,
|
||||
Localisation_get().angle_radian,
|
||||
Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
|
||||
}
|
||||
}else{ // COULEUR_VERT
|
||||
// Si le robot est déjà dans le quart haut droit,
|
||||
// On va en ligne droite
|
||||
if(Robot_est_dans_quart_haut_droit()){
|
||||
Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
2000-180, 2800,
|
||||
Localisation_get().angle_radian ,Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
|
||||
}else if (Robot_est_dans_zone_cerise_droite()){
|
||||
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
Localisation_get().x_mm + 330, Localisation_get().y_mm - 100,
|
||||
2000 - 745, 3000 - 475,
|
||||
2000 - 180, 3000 - 200,
|
||||
Localisation_get().angle_radian,
|
||||
Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
|
||||
}else{
|
||||
// Sinon, on a une courbe de bézier
|
||||
Trajet_config(500,250);
|
||||
Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
2000 - 418, Localisation_get().y_mm,
|
||||
2000 - 655, 2800,
|
||||
2000 - 180, 2800,
|
||||
Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
|
||||
}
|
||||
}
|
||||
|
||||
// parcours du trajet
|
||||
if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == ACTION_TERMINEE){
|
||||
etat_action = ACTION_TERMINEE;
|
||||
}
|
||||
return etat_action;
|
||||
|
||||
}
|
||||
|
||||
/// Fonction de localisation approximatives pour la stratégie.
|
||||
|
||||
@ -551,194 +242,6 @@ int Robot_est_dans_zone_depose(enum couleur_t couleur){
|
||||
}
|
||||
}
|
||||
|
||||
enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){
|
||||
static enum {
|
||||
CALAGE_PANIER_1,
|
||||
RECULE_PANIER,
|
||||
LANCE_DANS_PANIER,
|
||||
}etat_lance_balles_dans_panier;
|
||||
float recal_pos_x, recal_pos_y;
|
||||
float angle_depart, angle_arrivee;
|
||||
enum longer_direction_t longer_direction;
|
||||
float point_x, point_y;
|
||||
|
||||
enum etat_action_t etat_action = ACTION_EN_COURS;
|
||||
|
||||
struct trajectoire_t trajectoire;
|
||||
|
||||
switch(etat_lance_balles_dans_panier){
|
||||
case CALAGE_PANIER_1:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
recal_pos_x = RAYON_ROBOT;
|
||||
longer_direction = LONGER_VERS_A;
|
||||
}else{
|
||||
recal_pos_x = 2000- RAYON_ROBOT;
|
||||
longer_direction = LONGER_VERS_C;
|
||||
}
|
||||
|
||||
if(calage_angle(longer_direction, recal_pos_x, 3000 - (PETIT_RAYON_ROBOT_MM), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
|
||||
etat_lance_balles_dans_panier = RECULE_PANIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case RECULE_PANIER:
|
||||
Trajet_config(120, 250);
|
||||
if(couleur == COULEUR_BLEU){
|
||||
point_x = 180;
|
||||
point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
|
||||
}else{
|
||||
point_x = 1735;
|
||||
point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
|
||||
}
|
||||
angle_depart = Localisation_get().angle_radian;
|
||||
angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN);
|
||||
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
|
||||
point_x, point_y,
|
||||
angle_depart, angle_arrivee);
|
||||
|
||||
if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
|
||||
etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
|
||||
Trajet_config(250, 250);
|
||||
}
|
||||
break;
|
||||
|
||||
case LANCE_DANS_PANIER:
|
||||
Asser_Position_maintien();
|
||||
if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){
|
||||
etat_action = ACTION_TERMINEE;
|
||||
etat_lance_balles_dans_panier = CALAGE_PANIER_1;
|
||||
}
|
||||
break;
|
||||
}
|
||||
return etat_action;
|
||||
|
||||
}
|
||||
|
||||
/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
|
||||
/// @param step_ms : pas de temps.
|
||||
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
|
||||
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises){
|
||||
enum etat_action_t etat_action = ACTION_EN_COURS;
|
||||
static uint32_t tempo_ms;
|
||||
static uint32_t nb_iteration;
|
||||
|
||||
static enum{
|
||||
LANCE_PROPULSEUR_ON,
|
||||
LANCE_TEMPO_PROP_ON,
|
||||
LANCE_PORTE_OUVERTE,
|
||||
LANCE_OUVERTURE_INITIALE,
|
||||
} etat_lance_balle = LANCE_PROPULSEUR_ON;
|
||||
|
||||
switch(etat_lance_balle){
|
||||
case LANCE_PROPULSEUR_ON:
|
||||
i2c_annexe_active_propulseur();
|
||||
tempo_ms = 1000;
|
||||
nb_iteration=0;
|
||||
etat_lance_balle = LANCE_OUVERTURE_INITIALE;
|
||||
break;
|
||||
|
||||
case LANCE_OUVERTURE_INITIALE:
|
||||
if(temporisation_terminee(&tempo_ms, step_ms)){
|
||||
i2c_annexe_ouvre_porte();
|
||||
etat_lance_balle = LANCE_PORTE_OUVERTE;
|
||||
tempo_ms = 250;
|
||||
}
|
||||
break;
|
||||
|
||||
case LANCE_TEMPO_PROP_ON:
|
||||
if(temporisation_terminee(&tempo_ms, step_ms)){
|
||||
i2c_annexe_ouvre_porte();
|
||||
nb_iteration++;
|
||||
if(nb_iteration > nb_cerises){
|
||||
nb_iteration=0;
|
||||
etat_action = ACTION_TERMINEE;
|
||||
etat_lance_balle = LANCE_PROPULSEUR_ON;
|
||||
i2c_annexe_desactive_propulseur();
|
||||
}else{
|
||||
etat_lance_balle = LANCE_PORTE_OUVERTE;
|
||||
tempo_ms = 150;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case LANCE_PORTE_OUVERTE:
|
||||
if(temporisation_terminee(&tempo_ms, step_ms)){
|
||||
i2c_annexe_mi_ferme_porte();
|
||||
etat_lance_balle = LANCE_TEMPO_PROP_ON;
|
||||
tempo_ms = 500;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
return etat_action;
|
||||
}
|
||||
|
||||
/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
|
||||
/// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites
|
||||
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){
|
||||
enum etat_action_t etat_action = ACTION_EN_COURS;
|
||||
struct position_t position;
|
||||
struct trajectoire_t trajectoire;
|
||||
static float y_pos_ref;
|
||||
|
||||
static enum {
|
||||
CONTACT_AVANT,
|
||||
RECULE_BORDURE,
|
||||
CONTACT_LATERAL,
|
||||
RECALE_X
|
||||
}etat_calage_angle;
|
||||
|
||||
switch(etat_calage_angle){
|
||||
case CONTACT_AVANT:
|
||||
if(cerise_accostage() == ACTION_TERMINEE){
|
||||
position = Localisation_get();
|
||||
//if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
|
||||
Localisation_set_y(y_mm);
|
||||
//}
|
||||
//if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
|
||||
Localisation_set_angle(angle_radian);
|
||||
//}
|
||||
y_pos_ref = Localisation_get().y_mm;
|
||||
etat_calage_angle = RECULE_BORDURE;
|
||||
}
|
||||
break;
|
||||
|
||||
case RECULE_BORDURE:
|
||||
commande_translation_recule_vers_trompe();
|
||||
position = Localisation_get();
|
||||
if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){
|
||||
etat_calage_angle = CONTACT_LATERAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case CONTACT_LATERAL:
|
||||
if(longer_direction == LONGER_VERS_A){
|
||||
commande_translation_avance_vers_A();
|
||||
if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){
|
||||
etat_calage_angle = RECALE_X;
|
||||
}
|
||||
}else{
|
||||
commande_translation_avance_vers_C();
|
||||
if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){
|
||||
etat_calage_angle = RECALE_X;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case RECALE_X:
|
||||
etat_action = ACTION_TERMINEE;
|
||||
commande_vitesse_stop();
|
||||
position = Localisation_get();
|
||||
//if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
|
||||
Localisation_set_x(x_mm);
|
||||
//}
|
||||
etat_calage_angle = CONTACT_AVANT;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
return etat_action;
|
||||
}
|
||||
|
||||
/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
|
||||
uint attente_tirette(void){
|
||||
@ -748,7 +251,7 @@ uint attente_tirette(void){
|
||||
/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
|
||||
enum couleur_t lire_couleur(void){
|
||||
if (gpio_get(COULEUR))
|
||||
return COULEUR_VERT;
|
||||
return COULEUR_JAUNE;
|
||||
return COULEUR_BLEU;
|
||||
|
||||
}
|
||||
@ -766,51 +269,6 @@ int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Gère le chargement des balles au début du match
|
||||
enum etat_action_t Strategie_preparation(){
|
||||
|
||||
static enum{
|
||||
PREP_INIT_V,
|
||||
PREP_INIT_B,
|
||||
PREP_ASPIRE,
|
||||
PREP_ARRET_ASPIRATION,
|
||||
PREP_TEMPO,
|
||||
} etat_prep=PREP_INIT_V;
|
||||
|
||||
switch(etat_prep){
|
||||
case PREP_INIT_V:
|
||||
if(lire_couleur()== COULEUR_VERT){
|
||||
etat_prep = PREP_INIT_B;
|
||||
}
|
||||
break;
|
||||
case PREP_INIT_B:
|
||||
if(lire_couleur()== COULEUR_BLEU){
|
||||
etat_prep = PREP_ASPIRE;
|
||||
}
|
||||
break;
|
||||
|
||||
case PREP_ASPIRE:
|
||||
if(lire_couleur()== COULEUR_VERT){
|
||||
i2c_annexe_ferme_porte();
|
||||
i2c_annexe_active_turbine();
|
||||
etat_prep=PREP_ARRET_ASPIRATION;
|
||||
}
|
||||
break;
|
||||
|
||||
case PREP_ARRET_ASPIRATION:
|
||||
if(lire_couleur()== COULEUR_BLEU){
|
||||
i2c_annexe_desactive_turbine();
|
||||
etat_prep=PREP_TEMPO;
|
||||
}
|
||||
break;
|
||||
|
||||
case PREP_TEMPO:
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
return ACTION_EN_COURS;
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms){
|
||||
struct trajectoire_t trajectoire;
|
||||
|
||||
@ -936,3 +394,81 @@ enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_
|
||||
}
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms){
|
||||
// 1 Envoyer la commande pour détecter la bordure
|
||||
// 2 Si la valeur de la bordure est valide, lire l'angle et la distance.
|
||||
// Recaler la distance Y
|
||||
// 3 Se tourner du bon angle (en fonction de la couleur et de l'angle lu)
|
||||
// 4 Si la valeur de la bordure est valide, lire l'angle et la distance.
|
||||
// 5 Se positionner à (250, 250), PINCE orientée à 45°.
|
||||
static enum{
|
||||
CD_ENVOI_CDE_BORDURE,
|
||||
CD_LECTURE_BORDURE_Y,
|
||||
CD_ROTATION_VERS_X,
|
||||
CD_LECTURE_BORDURE_X,
|
||||
CD_ALLER_POSITION_INIT,
|
||||
}etat_calage_debut=CD_ENVOI_CDE_BORDURE;
|
||||
enum validite_vl53l8_t validite;
|
||||
struct trajectoire_t trajectoire;
|
||||
|
||||
float angle, distance;
|
||||
|
||||
switch(etat_calage_debut){
|
||||
case CD_ENVOI_CDE_BORDURE:
|
||||
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
|
||||
etat_calage_debut = CD_LECTURE_BORDURE_Y;
|
||||
break;
|
||||
|
||||
case CD_LECTURE_BORDURE_Y:
|
||||
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
|
||||
if(validite){
|
||||
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
||||
commande_vitesse_stop();
|
||||
Localisation_set_y(distance + DISTANCE_CENTRE_CAPTEUR);
|
||||
Localisation_set_angle((-90 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
|
||||
etat_calage_debut = CD_ROTATION_VERS_X;
|
||||
}
|
||||
break;
|
||||
|
||||
case CD_ROTATION_VERS_X:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
|
||||
(-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
|
||||
}else{
|
||||
Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian,
|
||||
(0 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
|
||||
}
|
||||
if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){
|
||||
i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
|
||||
etat_calage_debut = CD_LECTURE_BORDURE_Y;
|
||||
}
|
||||
break;
|
||||
|
||||
case CD_LECTURE_BORDURE_X:
|
||||
i2c_annexe_get_VL53L8(&validite, &angle, &distance);
|
||||
if(validite){
|
||||
i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
|
||||
commande_vitesse_stop();
|
||||
if(couleur == COULEUR_BLEU){
|
||||
Localisation_set_x(distance + DISTANCE_CENTRE_CAPTEUR);
|
||||
Localisation_set_angle((-180 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
|
||||
}else{
|
||||
Localisation_set_x(3000 - (distance + DISTANCE_CENTRE_CAPTEUR));
|
||||
Localisation_set_angle((0 * DEGRE_EN_RADIAN) - ANGLE_PINCE);
|
||||
}
|
||||
|
||||
etat_calage_debut = CD_ALLER_POSITION_INIT;
|
||||
}
|
||||
break;
|
||||
|
||||
case CD_ALLER_POSITION_INIT:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
return Strategie_aller_a(250, 250, step_ms);
|
||||
}else{
|
||||
return Strategie_aller_a(3000 - 250, 250, step_ms);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return ACTION_EN_COURS;
|
||||
}
|
@ -22,7 +22,7 @@ enum longer_direction_t{
|
||||
|
||||
enum couleur_t{
|
||||
COULEUR_BLEU=0,
|
||||
COULEUR_VERT,
|
||||
COULEUR_JAUNE,
|
||||
COULEUR_INCONNUE,
|
||||
};
|
||||
|
||||
@ -45,14 +45,10 @@ enum etat_action_t cerise_accostage(void);
|
||||
enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
|
||||
enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
|
||||
enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms);
|
||||
enum etat_action_t depose_cerises(uint32_t step_ms);
|
||||
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
|
||||
|
||||
void Homologation(uint32_t step_ms);
|
||||
enum couleur_t lire_couleur(void);
|
||||
uint attente_tirette(void);
|
||||
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
|
||||
int test_panier(void);
|
||||
void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);
|
||||
|
||||
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
|
||||
@ -66,6 +62,8 @@ enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_
|
||||
enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms);
|
||||
enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms);
|
||||
|
||||
enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms);
|
||||
|
||||
extern float distance_obstacle;
|
||||
|
||||
// STRATEGIE_H
|
||||
|
@ -1,6 +1,20 @@
|
||||
#include "math.h"
|
||||
#include "Strategie.h"
|
||||
#include "Geometrie_robot.h"
|
||||
#include "Strategie_2024_pots.h"
|
||||
|
||||
#define DISTANCE_APPROCHE_POT
|
||||
|
||||
float angle_bras[6] =
|
||||
{
|
||||
180 * DEGRE_EN_RADIAN,
|
||||
120 * DEGRE_EN_RADIAN,
|
||||
60 * DEGRE_EN_RADIAN,
|
||||
0,
|
||||
-60 * DEGRE_EN_RADIAN,
|
||||
-120 * DEGRE_EN_RADIAN
|
||||
};
|
||||
|
||||
struct position_t position_pots_dans_groupe_pot[5] =
|
||||
{
|
||||
{.x_mm = -40, .y_mm = 69.2, .angle_radian = 120 * DEGRE_EN_RADIAN},
|
||||
@ -42,4 +56,17 @@ struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_p
|
||||
position_pot.angle_radian = my_position_groupe_pot.angle_radian + angle_groupe_pot;
|
||||
|
||||
return position_pot;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Fonction qui déplace le robot jusqu'à la zone pour attraper les pots et qui attrape les 5 pots
|
||||
enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot){
|
||||
// Parcourir la trajectoire pour aller jusqu'au premier pot
|
||||
|
||||
// Pour chaque pot
|
||||
// Baisser le bras correspondant
|
||||
// Aller jusqu'au point de prise de pot en s'orientant pour prendre le pot
|
||||
// Avancer de X cm en direction du pot
|
||||
// Reculer dans l'axe de prise et rejoindre le point de prise suivant
|
||||
// Pendant le mouvement, apres 1 sec (à confirmer) Lever le bras
|
||||
|
||||
}
|
||||
|
@ -13,4 +13,11 @@
|
||||
#define GROUPE_POT_R2 4
|
||||
#define GROUPE_POT_R1 5
|
||||
|
||||
#define BRAS_1 0
|
||||
#define BRAS_2 1
|
||||
#define BRAS_3 2
|
||||
#define BRAS_4 3
|
||||
#define BRAS_5 4
|
||||
#define BRAS_6 5
|
||||
|
||||
struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot);
|
||||
|
@ -1,84 +0,0 @@
|
||||
#include "i2c_annexe.h"
|
||||
#include "Localisation.h"
|
||||
#include "Monitoring.h"
|
||||
#include "Strategie.h"
|
||||
#include "Trajectoire.h"
|
||||
#include "Trajet.h"
|
||||
#include "Geometrie.h"
|
||||
|
||||
enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){
|
||||
static enum {
|
||||
ALLER_1,
|
||||
ALLER_2,
|
||||
ALLER_3
|
||||
} etat=ALLER_1;
|
||||
float point_x, point_y;
|
||||
float angle_fin;
|
||||
struct trajectoire_t trajectoire;
|
||||
|
||||
|
||||
switch(etat){
|
||||
case ALLER_1:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
angle_fin = 90. * DEGRE_EN_RADIAN;
|
||||
point_x = 310;
|
||||
}else{
|
||||
angle_fin = 205. * DEGRE_EN_RADIAN;
|
||||
point_x = 1700;
|
||||
}
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
Trajet_config(100, 250);
|
||||
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
|
||||
etat=ALLER_2;
|
||||
i2c_annexe_deplie_bras();
|
||||
}
|
||||
break;
|
||||
|
||||
case ALLER_2:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
angle_fin = 90. * DEGRE_EN_RADIAN;
|
||||
point_x = 310;
|
||||
}else{
|
||||
angle_fin = 205. * DEGRE_EN_RADIAN;
|
||||
point_x = 1700;
|
||||
}
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
Trajet_config(500,250);
|
||||
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1700,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
|
||||
i2c_annexe_plie_bras();
|
||||
etat=ALLER_3;
|
||||
}
|
||||
break;
|
||||
|
||||
case ALLER_3:
|
||||
if(couleur == COULEUR_BLEU){
|
||||
angle_fin = -150. * DEGRE_EN_RADIAN;
|
||||
point_x = 225;
|
||||
}else{
|
||||
angle_fin = 30. * DEGRE_EN_RADIAN;
|
||||
point_x = 1775;
|
||||
}
|
||||
angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
Trajet_config(250, 250);
|
||||
Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1600,
|
||||
Localisation_get().angle_radian, angle_fin);
|
||||
|
||||
if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
|
||||
etat=ALLER_1;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
return ACTION_EN_COURS;
|
||||
|
||||
}
|
@ -1,3 +0,0 @@
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms);
|
@ -1,466 +0,0 @@
|
||||
#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 = 2000 - (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, 3000 - 1600, 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;
|
||||
}
|
@ -1,10 +0,0 @@
|
||||
#include "Strategie.h"
|
||||
enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);
|
||||
enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms);
|
||||
enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms);
|
||||
void commande_translation_longer_vers_A();
|
||||
void commande_translation_longer_vers_C();
|
||||
void commande_translation_avance_vers_trompe();
|
||||
void commande_translation_recule_vers_trompe();
|
||||
void commande_translation_avance_vers_A();
|
||||
void commande_translation_avance_vers_C();
|
37
Test.c
37
Test.c
@ -44,7 +44,7 @@
|
||||
|
||||
int test_APDS9960(void);
|
||||
int test_asser_position_avance(void);
|
||||
int test_asser_position_avance_et_tourne(int, int);
|
||||
int test_asser_position_avance_et_tourne(int);
|
||||
int test_transition_gyro_pas_gyro(void);
|
||||
void affiche_localisation(void);
|
||||
int test_capteurs_balise(void);
|
||||
@ -66,7 +66,6 @@ int mode_test(){
|
||||
printf("J - Asser Position - avance et tourne (sans gyro)\n");
|
||||
printf("N - Fonctions geometrique\n");
|
||||
printf("O - Analyse obstacle\n");
|
||||
printf("P - Asser Position - perturbation\n");
|
||||
printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
|
||||
printf("R - Test des logs...\n");
|
||||
printf("T - Trajectoire\n");
|
||||
@ -111,12 +110,12 @@ int mode_test(){
|
||||
|
||||
case 'I':
|
||||
case 'i':
|
||||
while(test_asser_position_avance_et_tourne(1, 0));
|
||||
while(test_asser_position_avance_et_tourne(1));
|
||||
break;
|
||||
|
||||
case 'J':
|
||||
case 'j':
|
||||
while(test_asser_position_avance_et_tourne(0, 0));
|
||||
while(test_asser_position_avance_et_tourne(0));
|
||||
break;
|
||||
|
||||
|
||||
@ -130,11 +129,6 @@ int mode_test(){
|
||||
while(test_angle_balise());
|
||||
break;
|
||||
|
||||
case 'P':
|
||||
case 'p':
|
||||
while(test_asser_position_avance_et_tourne(1, 1));
|
||||
break;
|
||||
|
||||
case 'Q':
|
||||
case 'q':
|
||||
while(test_transition_gyro_pas_gyro());
|
||||
@ -298,7 +292,7 @@ int test_transition_gyro_pas_gyro(void){
|
||||
/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
|
||||
/// @param propulseur : 1 pour activer le propulseur toutes les secondes
|
||||
/// @return
|
||||
int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){
|
||||
int test_asser_position_avance_et_tourne(int m_gyro){
|
||||
int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2, propulseur_on=0;
|
||||
uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old, tempo_prop=0;
|
||||
struct position_t position_consigne;
|
||||
@ -313,9 +307,6 @@ int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){
|
||||
Gyro_Init();
|
||||
printf("C'est parti !\n");
|
||||
}
|
||||
if(propulseur){
|
||||
i2c_maitre_init();
|
||||
}
|
||||
stdio_flush();
|
||||
|
||||
set_position_avec_gyroscope(m_gyro);
|
||||
@ -327,22 +318,7 @@ int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){
|
||||
do{
|
||||
|
||||
while(temps_ms == Temps_get_temps_ms()){
|
||||
if(propulseur){
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
}
|
||||
}
|
||||
if(propulseur){
|
||||
tempo_prop++;
|
||||
if(tempo_prop>1000){
|
||||
tempo_prop=0;
|
||||
if(propulseur_on){
|
||||
i2c_annexe_active_propulseur();
|
||||
}else{
|
||||
i2c_annexe_desactive_propulseur();
|
||||
}
|
||||
propulseur_on = !propulseur_on;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
@ -356,9 +332,6 @@ int test_asser_position_avance_et_tourne(int m_gyro, int propulseur){
|
||||
AsserMoteur_Gestion(_step_ms);
|
||||
|
||||
position_consigne.angle_radian = (float) (temps_ms - temps_ms_init) /1000.;
|
||||
if(!propulseur){
|
||||
position_consigne.y_mm = (float) (temps_ms - temps_ms_init) * 100. / 1000.;
|
||||
}
|
||||
|
||||
Asser_Position(position_consigne);
|
||||
|
||||
|
481
Test_strategie.c
481
Test_strategie.c
@ -14,7 +14,6 @@
|
||||
#include "QEI.h"
|
||||
#include "Robot_config.h"
|
||||
#include "Strategie.h"
|
||||
#include "Strategie_prise_cerises.h"
|
||||
#include "Temps.h"
|
||||
#include "Trajet.h"
|
||||
#include "Trajectoire.h"
|
||||
@ -73,59 +72,24 @@ void affichage_test_strategie(){
|
||||
|
||||
|
||||
int test_strategie(){
|
||||
printf("A - Accoster.\n");
|
||||
printf("C - Couleur et tirette.\n");
|
||||
printf("D - Attraper cerises laterales.\n");
|
||||
printf("E - Evitement\n");
|
||||
printf("H - Homologation.\n");
|
||||
printf("L - Longer.\n");
|
||||
printf("P - Panier.\n");
|
||||
|
||||
int lettre;
|
||||
do{
|
||||
lettre = getchar_timeout_us(0);
|
||||
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
|
||||
switch(lettre){
|
||||
case 'a':
|
||||
case 'A':
|
||||
while(test_accostage());
|
||||
break;
|
||||
|
||||
case 'c':
|
||||
case 'C':
|
||||
while(test_tirette_et_couleur());
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
case 'D':
|
||||
while(test_cerise_laterales_droite());
|
||||
break;
|
||||
|
||||
case 'e':
|
||||
case 'E':
|
||||
while(test_evitement());
|
||||
break;
|
||||
|
||||
case 'g':
|
||||
case 'G':
|
||||
while(test_cerise_laterales_gauche());
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
case 'H':
|
||||
//while(test_homologation());
|
||||
break;
|
||||
|
||||
case 'l':
|
||||
case 'L':
|
||||
while(test_longe());
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
case 'P':
|
||||
while(test_panier());
|
||||
break;
|
||||
|
||||
case 'q':
|
||||
case 'Q':
|
||||
return 0;
|
||||
@ -135,246 +99,6 @@ int test_strategie(){
|
||||
}
|
||||
|
||||
|
||||
int test_cerise_laterales_gauche(){
|
||||
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
|
||||
uint32_t temps_cycle_old;
|
||||
enum etat_action_t etat_action;
|
||||
printf("Attaper cerise latérales\n");
|
||||
|
||||
i2c_maitre_init();
|
||||
Trajet_init();
|
||||
Balise_VL53L1X_init();
|
||||
//printf("Init gyroscope\n");
|
||||
set_position_avec_gyroscope(1);
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Init();
|
||||
}
|
||||
|
||||
stdio_flush();
|
||||
|
||||
multicore_launch_core1(affichage_test_strategie);
|
||||
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
temps_ms_init = temps_ms;
|
||||
temps_cycle_old= time_us_32();
|
||||
|
||||
uint32_t tempo_ms=1000;
|
||||
|
||||
Localisation_set(250, 1500, -150 * DEGRE_EN_RADIAN);
|
||||
do{
|
||||
etat_action = ACTION_EN_COURS;
|
||||
|
||||
temps_cycle_check();
|
||||
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
Balise_VL53L1X_gestion();
|
||||
|
||||
// Routines à 1 ms
|
||||
|
||||
if(temps_ms != Temps_get_temps_ms()){
|
||||
static enum {
|
||||
TEMPO_AVANT,
|
||||
TEST,
|
||||
TEMPO_APRES
|
||||
}etat_test;
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
QEI_update();
|
||||
Localisation_gestion();
|
||||
AsserMoteur_Gestion(_step_ms);
|
||||
|
||||
|
||||
// Routine à 2 ms
|
||||
if(temps_ms % _step_ms_gyro == 0){
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Read(_step_ms_gyro);
|
||||
}
|
||||
}
|
||||
|
||||
switch(etat_test){
|
||||
case TEMPO_AVANT:
|
||||
if(temporisation_terminee(&tempo_ms, _step_ms)){
|
||||
etat_test = TEST;
|
||||
}
|
||||
break;
|
||||
case TEST:
|
||||
if(cerises_attraper_cerises_gauches(_step_ms) == ACTION_TERMINEE){
|
||||
tempo_ms = 1000;
|
||||
etat_test = TEMPO_APRES;
|
||||
}
|
||||
break;
|
||||
case TEMPO_APRES:
|
||||
if(temporisation_terminee(&tempo_ms, _step_ms)){
|
||||
etat_test = TEMPO_AVANT;
|
||||
etat_action = ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
//lettre = getchar_timeout_us(0);
|
||||
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
|
||||
}while(etat_action == ACTION_EN_COURS);
|
||||
Moteur_Stop();
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
int test_cerise_laterales_droite(){
|
||||
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
|
||||
uint32_t temps_cycle_old;
|
||||
enum etat_action_t etat_action;
|
||||
printf("Attaper cerise latérales\n");
|
||||
|
||||
i2c_maitre_init();
|
||||
Trajet_init();
|
||||
Balise_VL53L1X_init();
|
||||
//printf("Init gyroscope\n");
|
||||
set_position_avec_gyroscope(1);
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Init();
|
||||
}
|
||||
|
||||
stdio_flush();
|
||||
|
||||
multicore_launch_core1(affichage_test_strategie);
|
||||
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
temps_ms_init = temps_ms;
|
||||
temps_cycle_old= time_us_32();
|
||||
|
||||
uint32_t tempo_ms=1000;
|
||||
|
||||
Localisation_set(1775, 1500, 30 * DEGRE_EN_RADIAN);
|
||||
do{
|
||||
etat_action = ACTION_EN_COURS;
|
||||
|
||||
temps_cycle_check();
|
||||
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
Balise_VL53L1X_gestion();
|
||||
|
||||
// Routines à 1 ms
|
||||
|
||||
if(temps_ms != Temps_get_temps_ms()){
|
||||
static enum {
|
||||
TEMPO_AVANT,
|
||||
TEST,
|
||||
TEMPO_APRES
|
||||
}etat_test;
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
QEI_update();
|
||||
Localisation_gestion();
|
||||
AsserMoteur_Gestion(_step_ms);
|
||||
|
||||
|
||||
// Routine à 2 ms
|
||||
if(temps_ms % _step_ms_gyro == 0){
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Read(_step_ms_gyro);
|
||||
}
|
||||
}
|
||||
|
||||
switch(etat_test){
|
||||
case TEMPO_AVANT:
|
||||
if(temporisation_terminee(&tempo_ms, _step_ms)){
|
||||
etat_test = TEST;
|
||||
}
|
||||
break;
|
||||
case TEST:
|
||||
if(cerises_attraper_cerises_droite(_step_ms) == ACTION_TERMINEE){
|
||||
tempo_ms = 1000;
|
||||
etat_test = TEMPO_APRES;
|
||||
}
|
||||
break;
|
||||
case TEMPO_APRES:
|
||||
if(temporisation_terminee(&tempo_ms, _step_ms)){
|
||||
etat_test = TEMPO_AVANT;
|
||||
etat_action = ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
//lettre = getchar_timeout_us(0);
|
||||
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
|
||||
}while(etat_action == ACTION_EN_COURS);
|
||||
Moteur_Stop();
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
int test_homologation(){
|
||||
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
|
||||
uint32_t temps_cycle[10], temps_cycle_old, index_temps_cycle=0;
|
||||
printf("Homologation\n");
|
||||
|
||||
i2c_maitre_init();
|
||||
Trajet_init();
|
||||
Balise_VL53L1X_init();
|
||||
//printf("Init gyroscope\n");
|
||||
set_position_avec_gyroscope(1);
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Init();
|
||||
}
|
||||
|
||||
stdio_flush();
|
||||
|
||||
multicore_launch_core1(affichage_test_strategie);
|
||||
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
temps_ms_init = temps_ms;
|
||||
temps_cycle_old= time_us_32();
|
||||
do{
|
||||
/*temps_cycle[index_temps_cycle++]= time_us_32() - temps_cycle_old;
|
||||
if(index_temps_cycle >= 10){
|
||||
for(int i=0; i<10; i++){
|
||||
printf("t_cycle:%d\n", temps_cycle[i]);
|
||||
}
|
||||
index_temps_cycle=0;
|
||||
}
|
||||
temps_cycle_old=time_us_32();*/
|
||||
|
||||
temps_cycle_check();
|
||||
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
Balise_VL53L1X_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){
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Read(_step_ms_gyro);
|
||||
}
|
||||
}
|
||||
|
||||
//Homologation(_step_ms);
|
||||
|
||||
}
|
||||
//lettre = getchar_timeout_us(0);
|
||||
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
|
||||
}while(1);
|
||||
printf("STRATEGIE_LOOP_2\n");
|
||||
printf("Lettre : %d; %c\n", lettre, lettre);
|
||||
|
||||
if(lettre == 'q' && lettre == 'Q'){
|
||||
return 0;
|
||||
}
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
void affichage_test_evitement(){
|
||||
while(1){
|
||||
@ -471,201 +195,6 @@ int test_evitement(){
|
||||
}
|
||||
|
||||
|
||||
int test_panier(){
|
||||
int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
|
||||
printf("Test panier\n");
|
||||
|
||||
static enum{
|
||||
TEST_PANIER_INIT,
|
||||
TEST_PANIER_ASPIRE,
|
||||
TEST_PANIER_TEMPO,
|
||||
TEST_PANIER_LANCE_BALLES,
|
||||
TEST_PANIER_PORTE_OUVERTE,
|
||||
} etat_test_panier=TEST_PANIER_INIT;
|
||||
static uint32_t tempo_ms;
|
||||
|
||||
i2c_maitre_init();
|
||||
Trajet_init();
|
||||
//printf("Init gyroscope\n");
|
||||
set_position_avec_gyroscope(0);
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Init();
|
||||
}
|
||||
|
||||
stdio_flush();
|
||||
|
||||
multicore_launch_core1(affichage_test_strategie);
|
||||
|
||||
temps_ms = Temps_get_temps_ms();
|
||||
temps_ms_init = temps_ms;
|
||||
do{
|
||||
i2c_gestion(i2c0);
|
||||
i2c_annexe_gestion();
|
||||
Balise_VL53L1X_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){
|
||||
if(get_position_avec_gyroscope()){
|
||||
Gyro_Read(_step_ms_gyro);
|
||||
}
|
||||
}
|
||||
|
||||
switch(etat_test_panier){
|
||||
case TEST_PANIER_INIT:
|
||||
if(lire_couleur()== COULEUR_VERT){
|
||||
i2c_annexe_ferme_porte();
|
||||
i2c_annexe_active_turbine();
|
||||
etat_test_panier=TEST_PANIER_ASPIRE;
|
||||
}
|
||||
break;
|
||||
|
||||
case TEST_PANIER_ASPIRE:
|
||||
if(lire_couleur()== COULEUR_BLEU){
|
||||
i2c_annexe_desactive_turbine();
|
||||
tempo_ms = 3000;
|
||||
etat_test_panier=TEST_PANIER_TEMPO;
|
||||
}
|
||||
break;
|
||||
case TEST_PANIER_TEMPO:
|
||||
if(temporisation_terminee(&tempo_ms, _step_ms)){
|
||||
etat_test_panier=TEST_PANIER_LANCE_BALLES;
|
||||
}
|
||||
break;
|
||||
|
||||
case TEST_PANIER_LANCE_BALLES:
|
||||
if(lance_balles(_step_ms, 10) == ACTION_TERMINEE){
|
||||
etat_test_panier=TEST_PANIER_PORTE_OUVERTE;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case TEST_PANIER_PORTE_OUVERTE:
|
||||
if(temporisation_terminee(&tempo_ms, _step_ms)){
|
||||
i2c_annexe_ouvre_porte();
|
||||
}
|
||||
if(lire_couleur()== COULEUR_BLEU){
|
||||
etat_test_panier=TEST_PANIER_INIT;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
lettre = getchar_timeout_us(0);
|
||||
//}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
|
||||
}while(1);
|
||||
printf("STRATEGIE_LOOP_2\n");
|
||||
printf("Lettre : %d; %c\n", lettre, lettre);
|
||||
|
||||
if(lettre == 'q' && lettre == 'Q'){
|
||||
return 0;
|
||||
}
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
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(avance_puis_longe_bordure(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;
|
||||
}
|
||||
|
||||
|
||||
int test_tirette_et_couleur(){
|
||||
int lettre;
|
||||
@ -683,8 +212,8 @@ int test_tirette_et_couleur(){
|
||||
i2c_annexe_gestion();
|
||||
|
||||
printf(">tirette:%d\n", attente_tirette());
|
||||
if(lire_couleur() == COULEUR_VERT){
|
||||
printf(">couleur:Vert|t\n");
|
||||
if(lire_couleur() == COULEUR_JAUNE){
|
||||
printf(">couleur:Jaune|t\n");
|
||||
}else{
|
||||
printf(">couleur:Bleu|t\n");
|
||||
}
|
||||
@ -693,9 +222,9 @@ int test_tirette_et_couleur(){
|
||||
if(couleur_old != lire_couleur() || tirette != attente_tirette()){
|
||||
tirette = attente_tirette();
|
||||
couleur_old = lire_couleur();
|
||||
if(couleur_old == COULEUR_VERT){
|
||||
// Tout vert
|
||||
i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
|
||||
if(couleur_old == COULEUR_JAUNE){
|
||||
// Tout Jaune
|
||||
i2c_annexe_couleur_balise(0b11111100, 0x0FFF);
|
||||
}else{
|
||||
// Tout bleu
|
||||
i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
|
||||
|
@ -1,4 +1,5 @@
|
||||
#include <stdio.h>
|
||||
#include "pico/stdio.h"
|
||||
#include "pico/error.h"
|
||||
#include "Geometrie.h"
|
||||
#include "Strategie_2024_pots.h"
|
||||
@ -17,6 +18,7 @@ int test_strategie_2024(){
|
||||
case 'A':
|
||||
while(test_calcul_position_pot());
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -268,8 +268,6 @@ int test_localisation(){
|
||||
uint32_t _step_ms_gyro = 2, _step_ms=1, tempo_moteur=0;
|
||||
uint32_t m_gyro = 0;
|
||||
int16_t vitesse;
|
||||
int propulseur=0;
|
||||
|
||||
|
||||
Localisation_init();
|
||||
|
||||
@ -329,12 +327,6 @@ int test_localisation(){
|
||||
Moteur_SetVitesse(MOTEUR_A ,vitesse);
|
||||
Moteur_SetVitesse(MOTEUR_B ,vitesse);
|
||||
Moteur_SetVitesse(MOTEUR_C ,vitesse);
|
||||
if(propulseur){
|
||||
i2c_annexe_active_propulseur();
|
||||
}else{
|
||||
i2c_annexe_desactive_propulseur();
|
||||
}
|
||||
propulseur = !propulseur;
|
||||
}
|
||||
}
|
||||
|
||||
|
81
i2c_annexe.c
81
i2c_annexe.c
@ -6,15 +6,18 @@
|
||||
#define ADRESSE_DEBUT_W 0x05
|
||||
#define ADRESSE_DEBUT_R 0x0A
|
||||
#define ADRESSE_SCORE 0x09
|
||||
#define ADRESSE_TURBINE_PORTE 0x0A
|
||||
#define ADRESSE_VL53L8_MODE 0x0A
|
||||
#define ADRESSE_CONTACTEURS 0x0B
|
||||
#define ADRESSE_VL53L1X 0x0C
|
||||
#define ADRESSE_TENSION_BATTERIE 0x18
|
||||
#define ADRESSE_COULEUR 0x05
|
||||
#define ADRESSE_MASQUE_LED_1 0x06
|
||||
#define ADRESSE_MASQUE_LED_2 0x07
|
||||
#define ADRESSE_VL53L8_VALIDITE 0x19
|
||||
#define ADRESSE_VL53L8_ANGLE 0x1A
|
||||
#define ADRESSE_VL53L8_DISTANCE 0x1E
|
||||
#define TAILLE_DONNEES_EMISSION 6
|
||||
#define TAILLE_DONNEES_RECEPTION 15
|
||||
#define TAILLE_DONNEES_RECEPTION 24
|
||||
|
||||
#define ADRESSE_PIC18F4550 0x31
|
||||
#define ADRESSE_PIC18F4550_DEBUT_W 0x00
|
||||
@ -26,6 +29,11 @@ uint8_t donnees_emission_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION];
|
||||
uint8_t donnees_reception_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION];
|
||||
uint8_t donnees_reception[TAILLE_DONNEES_RECEPTION];
|
||||
|
||||
union float_or_octet_t
|
||||
{
|
||||
float f_value;
|
||||
unsigned char octet[4];
|
||||
};
|
||||
|
||||
uint donnees_a_envoyer=0;
|
||||
uint donnees_a_envoyer_pic=0;
|
||||
@ -97,55 +105,8 @@ void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){
|
||||
donnees_emission[ADRESSE_MASQUE_LED_2 - ADRESSE_DEBUT_W] = masque_led & 0xFF;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
|
||||
void i2c_annexe_active_turbine(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
void i2c_annexe_desactive_turbine(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
|
||||
void i2c_annexe_ouvre_porte(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
void i2c_annexe_ferme_porte(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9;
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= (1 << 1);
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
void i2c_annexe_mi_ferme_porte(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9;
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= (2 << 1);
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
|
||||
void i2c_annexe_active_propulseur(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x08;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
void i2c_annexe_desactive_propulseur(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF7;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
|
||||
void i2c_annexe_active_deguisement(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x10;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
void i2c_annexe_desactive_deguisement(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xEF;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
|
||||
void i2c_annexe_deplie_bras(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x20;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
void i2c_annexe_plie_bras(void){
|
||||
donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xDF;
|
||||
void i2c_annexe_set_mode_VL53L8(enum validite_vl53l8_t mode){
|
||||
donnees_emission[ADRESSE_VL53L8_MODE - ADRESSE_DEBUT_W] = mode;
|
||||
donnees_a_envoyer=1;
|
||||
}
|
||||
|
||||
@ -173,6 +134,24 @@ uint8_t i2c_annexe_get_tension_batterie(void){
|
||||
return donnees_reception[ADRESSE_TENSION_BATTERIE - ADRESSE_DEBUT_R];
|
||||
}
|
||||
|
||||
void i2c_annexe_get_VL53L8(enum validite_vl53l8_t *validite, float * angle, float * distance){
|
||||
union float_or_octet_t donnee;
|
||||
*validite = donnees_reception[ADRESSE_VL53L8_VALIDITE];
|
||||
|
||||
donnee.octet[0]= donnees_reception[ADRESSE_VL53L8_ANGLE];
|
||||
donnee.octet[1]= donnees_reception[ADRESSE_VL53L8_ANGLE+1];
|
||||
donnee.octet[2]= donnees_reception[ADRESSE_VL53L8_ANGLE+2];
|
||||
donnee.octet[3]= donnees_reception[ADRESSE_VL53L8_ANGLE+3];
|
||||
*angle = donnee.f_value;
|
||||
|
||||
donnee.octet[0]= donnees_reception[ADRESSE_VL53L8_DISTANCE];
|
||||
donnee.octet[1]= donnees_reception[ADRESSE_VL53L8_DISTANCE+1];
|
||||
donnee.octet[2]= donnees_reception[ADRESSE_VL53L8_DISTANCE+2];
|
||||
donnee.octet[3]= donnees_reception[ADRESSE_VL53L8_DISTANCE+3];
|
||||
*distance = donnee.f_value;
|
||||
}
|
||||
|
||||
|
||||
void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
|
||||
for (uint8_t capteur = 0; capteur < 12; capteur++){
|
||||
distance_capteur_cm[capteur] = donnees_reception[capteur + ADRESSE_VL53L1X - ADRESSE_DEBUT_R];
|
||||
|
23
i2c_annexe.h
23
i2c_annexe.h
@ -18,16 +18,13 @@
|
||||
#define PLANTE_BRAS_1 '1'
|
||||
#define PLANTE_BRAS_6 '6'
|
||||
|
||||
enum validite_vl53l8_t{
|
||||
VL53L8_INVALIDE=0,
|
||||
VL53L8_BORDURE=1,
|
||||
VL53L8_PLANTE=2
|
||||
};
|
||||
|
||||
void i2c_annexe_gestion(void);
|
||||
void i2c_annexe_active_turbine(void);
|
||||
void i2c_annexe_desactive_turbine(void);
|
||||
|
||||
void i2c_annexe_ouvre_porte(void);
|
||||
void i2c_annexe_ferme_porte(void);
|
||||
void i2c_annexe_mi_ferme_porte(void);
|
||||
|
||||
void i2c_annexe_active_propulseur(void);
|
||||
void i2c_annexe_desactive_propulseur(void);
|
||||
|
||||
void i2c_annexe_envoie_score(uint8_t score);
|
||||
|
||||
@ -40,13 +37,9 @@ uint8_t i2c_annexe_get_contacteur_longer_C(void);
|
||||
void i2c_annexe_get_distances(uint8_t *distance_capteur_cm);
|
||||
void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led);
|
||||
|
||||
void i2c_annexe_active_deguisement(void);
|
||||
void i2c_annexe_desactive_deguisement(void);
|
||||
|
||||
void i2c_annexe_plie_bras(void);
|
||||
void i2c_annexe_deplie_bras(void);
|
||||
|
||||
void i2c_annexe_set_mode_VL53L8(enum validite_vl53l8_t mode);
|
||||
uint8_t i2c_annexe_get_tension_batterie(void);
|
||||
void i2c_annexe_get_VL53L8(enum validite_vl53l8_t *validite, float * angle, float * distance);
|
||||
|
||||
void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt);
|
||||
void i2c_annexe_attrape_plante(uint8_t action);
|
||||
|
Loading…
Reference in New Issue
Block a user