Gestion des configurations des balises + début d'un traitement des données de la detections de l'adversaire
This commit is contained in:
parent
ebcadc1af5
commit
3e37992ee2
@ -21,6 +21,9 @@
|
||||
#define VITESSE_STANDARD 1000
|
||||
#define ACCELERATION_STANDARD 500
|
||||
|
||||
#define COULEUR_BLEU 0
|
||||
#define COULEUR_JAUNE 1
|
||||
|
||||
#define gst_server;
|
||||
|
||||
struct triangulation_reception_t {
|
||||
@ -41,7 +44,8 @@ IPAddress subnet(255, 255, 255, 0);
|
||||
// IPAddress gateway(192, 168, 1, 1);
|
||||
// IPAddress subnet(255, 255, 255, 0);
|
||||
|
||||
|
||||
const unsigned char configuration_match_jaune = 0;
|
||||
const unsigned char configuration_match_bleu = 1;
|
||||
|
||||
const int16_t I2C_MASTER = 0x42;
|
||||
const int16_t I2C_SLAVE_trian = 0x30;
|
||||
@ -265,6 +269,7 @@ void gestion_match(){
|
||||
struct triangulation_reception_t triangulation_reception;
|
||||
static int translation_x_mm, translation_y_mm;
|
||||
static float rotation_rad;
|
||||
static int couleur;
|
||||
|
||||
enum etat_strategie{
|
||||
ATTENTE_ORDRE=0,
|
||||
@ -302,7 +307,7 @@ void gestion_match(){
|
||||
translation_y_mm = 0;
|
||||
rotation_rad = 0;
|
||||
|
||||
//index_Maitre = DEPLACEMENT_RELATIF;
|
||||
index_Maitre = DEPLACEMENT_RELATIF;
|
||||
Scan_Triangulation(&triangulation_reception);
|
||||
}
|
||||
if(M5.BtnB.read() == 1){
|
||||
@ -319,6 +324,11 @@ void gestion_match(){
|
||||
translation_y_mm = 0;
|
||||
rotation_rad = 100;
|
||||
Triangulation_send_immobile(1);
|
||||
while(M5.BtnC.read()){
|
||||
M5.update();
|
||||
}
|
||||
delay(200);
|
||||
IHM_attente_match(&couleur);
|
||||
|
||||
index_Maitre = MATCH_EN_COURS;
|
||||
|
||||
@ -345,7 +355,7 @@ void gestion_match(){
|
||||
break;
|
||||
|
||||
case MATCH_EN_COURS:
|
||||
if(Strategie() == ACTION_TERMINEE){
|
||||
if(Strategie(couleur) == ACTION_TERMINEE){
|
||||
index_Maitre = ATTENTE_ORDRE;
|
||||
}
|
||||
break;
|
||||
@ -358,7 +368,30 @@ void gestion_match(){
|
||||
}
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie(void){
|
||||
void IHM_attente_match(int * couleur){
|
||||
int pret = 0;
|
||||
int m_couleur = COULEUR_BLEU;
|
||||
affiche_msg("Choix couleur", "En attente");
|
||||
while(!pret){
|
||||
M5.update();
|
||||
if(M5.BtnA.read() == 1){
|
||||
affiche_msg("Couleur", " BLEU ");
|
||||
Triangulation_send_config(configuration_match_bleu);
|
||||
m_couleur = COULEUR_BLEU;
|
||||
}
|
||||
if(M5.BtnB.read() == 1){
|
||||
affiche_msg("Couleur", " JAUNE ");
|
||||
Triangulation_send_config(configuration_match_jaune);
|
||||
m_couleur = COULEUR_JAUNE;
|
||||
}
|
||||
if(M5.BtnC.read() == 1){
|
||||
pret = 1;
|
||||
*couleur = m_couleur;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
enum etat_action_t Strategie(int couleur){
|
||||
static enum {
|
||||
STRAT_RECULE_BANDEROLE, // Deplacement relatif
|
||||
STRAT_ALLER_GRADINS_1_A, // Déplacement absolu
|
||||
@ -378,14 +411,18 @@ enum etat_action_t Strategie(void){
|
||||
translation_x_mm = -450;
|
||||
translation_y_mm = 0;
|
||||
rotation_rad = 0;
|
||||
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 0);
|
||||
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
etat_strategie = STRAT_ALLER_GRADINS_1_A;
|
||||
}
|
||||
break;
|
||||
|
||||
case STRAT_ALLER_GRADINS_1_A:
|
||||
etat_action = deplacement_absolu(800, 800, -M_PI/2., 0);
|
||||
if(couleur == COULEUR_JAUNE){
|
||||
etat_action = deplacement_absolu(800, 800, -M_PI/2., 1);
|
||||
}else{
|
||||
etat_action = deplacement_absolu(3000 - 800, 800, -M_PI/2., 1);
|
||||
}
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
etat_strategie = STRAT_ALLER_GRADINS_1_B;
|
||||
}
|
||||
@ -406,7 +443,11 @@ enum etat_action_t Strategie(void){
|
||||
break;
|
||||
|
||||
case STRAT_ALLER_PREPA_BACKSTAGE:
|
||||
if(couleur == COULEUR_JAUNE){
|
||||
etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
|
||||
}else{
|
||||
etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0);
|
||||
}
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
etat_strategie = STRAT_ALLER_BACKSTAGE;
|
||||
}
|
||||
@ -532,6 +573,36 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, floa
|
||||
return ACTION_EN_COURS;
|
||||
}
|
||||
|
||||
/// @brief Récupère les données de la carte de détection de l'advesraire et renoi 1 s'il y aun adversaire
|
||||
/// Test les capteurs dans la direction d'avancement
|
||||
/// @param angle_deplacement direction dans laquelle avance le robot, dans le référentiel du robot
|
||||
int detection_adversaire(float angle_deplacement){
|
||||
int capteur_central, capteur_precedant, capteur_suivant;
|
||||
// On ramène l'angle entre 0 et 2 PI.
|
||||
while(angle_deplacement < 0){
|
||||
angle_deplacement += 2 * M_PI;
|
||||
}
|
||||
while(angle_deplacement > 2 * M_PI){
|
||||
angle_deplacement -= 2 * M_PI;
|
||||
}
|
||||
// On obtient le capteur central.
|
||||
capteur_central = angle_deplacement / (M_PI * 2) * 12;
|
||||
capteur_precedant = capteur_central - 1;
|
||||
if(capteur_precedant < 0){
|
||||
capteur_precedant = 11;
|
||||
}
|
||||
capteur_suivant = capteur_central + 1;
|
||||
if(capteur_suivant > 11){
|
||||
capteur_suivant = 0;
|
||||
}
|
||||
if(detect_adv_reception.distance_cm[capteur_central] < 50 ||
|
||||
detect_adv_reception.distance_cm[capteur_precedant] < 50 ||
|
||||
detect_adv_reception.distance_cm[capteur_suivant] < 50 ){
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
|
||||
/// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
|
||||
enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
|
||||
@ -563,11 +634,19 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
|
||||
// On lit les capteurs
|
||||
Detect_adv_lire(&detect_adv_reception);
|
||||
// On analyse les valeurs - TODO : créer une fonction plus évoluée
|
||||
if(distance_x_mm > 0){
|
||||
if(detect_adv_reception.distance_cm[0] < 50){
|
||||
// Arrêt du mouvement
|
||||
chassis_emission.status = MOUVEMENT_INTERRUPTION;
|
||||
send_Chassis(&chassis_emission);
|
||||
}
|
||||
}else if(distance_x_mm < 0){
|
||||
if(detect_adv_reception.distance_cm[6] < 50){
|
||||
// Arrêt du mouvement
|
||||
chassis_emission.status = MOUVEMENT_INTERRUPTION;
|
||||
send_Chassis(&chassis_emission);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Scan_chassis(&chassis_reception);
|
||||
|
||||
@ -48,3 +48,12 @@ void Triangulation_send_immobile(int immobile){
|
||||
while(1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Triangulation_send_config(unsigned char configuration){
|
||||
error = I2C_ecrire_registre(I2C_SLAVE_trian, 14, &configuration, 1); // si errror != de 0 alors erreur de communication
|
||||
if (error !=0){
|
||||
affiche_erreur("Send_Triangulation", "Erreur I2C");
|
||||
while(1);
|
||||
}
|
||||
}
|
||||
@ -20,7 +20,7 @@ String Lecture;
|
||||
#define pi2 6.283185307179586
|
||||
#define ALIGNEMENT_RAD (245. / 180. *M_PI)
|
||||
|
||||
uint8_t * data_i2C;
|
||||
uint8_t * data_i2c;
|
||||
int Balise[4][2]; // 4 balises potentielles i de 0 a 3
|
||||
// Frequence modulation individuelle balises
|
||||
// Balise[i][0] est la frequence de la balise
|
||||
@ -51,6 +51,8 @@ int Frequence_0 = 5000; // frequence balise 0
|
||||
int Frequence_1 = 6000; // frequence balise 1
|
||||
int Frequence_2 = 4000; // frequence balise 2
|
||||
int Bande_P = 200; // bande passante balises
|
||||
|
||||
// Attention, les absisces des balises sont redéfines plus bas (dans loop)
|
||||
int X1 = -90; // abcisse balise 0
|
||||
int X2 = -90; // abcisse balise 1
|
||||
int X3 = 3090; // abcisse balise 2
|
||||
@ -194,8 +196,8 @@ void setup() {
|
||||
WiFi.begin(ssid, password);
|
||||
|
||||
// Pour accéder aux données de l'I2C
|
||||
data_i2C = get_i2c_data();
|
||||
data_i2C[13] = 1; // On dit que le robot est immobile.
|
||||
data_i2c = get_i2c_data();
|
||||
data_i2c[13] = 1; // On dit que le robot est immobile.
|
||||
|
||||
int test_wifi = 0;
|
||||
|
||||
@ -277,8 +279,9 @@ void setup() {
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//Effectue a chaque tour ........................
|
||||
if(data_i2C[13] != 1){
|
||||
|
||||
// On invalide les balises si le robot bouge
|
||||
if(data_i2c[13] != 1){
|
||||
Nb_Balises = 0;
|
||||
Balise_0 = false;
|
||||
Balise_1 = false;
|
||||
@ -287,6 +290,35 @@ void loop() {
|
||||
uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
|
||||
I2C_envoi_8bits(etat_balises,0);
|
||||
}
|
||||
|
||||
// Configuration des balises
|
||||
if(data_i2c[14] == 0){
|
||||
// Match côté Jaune
|
||||
X1 = -90; // abcisse balise 0
|
||||
X2 = -90; // abcisse balise 1
|
||||
X3 = 3090; // abcisse balise 2
|
||||
Y1 = 1950; // ordonnee balise 0
|
||||
Y2 = 50; // ordonnee balise 1
|
||||
Y3 = 1000; // ordonnee balise 2
|
||||
Xp1 = X1 - X2;
|
||||
Yp1 = Y1 - Y2;
|
||||
Xp3 = X3 - X2;
|
||||
Yp3 = Y3 - Y2;
|
||||
}else if(data_i2c[14] == 1){
|
||||
// Match côté Jaune
|
||||
X1 = 3090; // abcisse balise 0
|
||||
X2 = 3090; // abcisse balise 1
|
||||
X3 = -90; // abcisse balise 2
|
||||
Y1 = 1950; // ordonnee balise 0
|
||||
Y2 = 50; // ordonnee balise 1
|
||||
Y3 = 1000; // ordonnee balise 2
|
||||
Xp1 = X1 - X2;
|
||||
Yp1 = Y1 - Y2;
|
||||
Xp3 = X3 - X2;
|
||||
Yp3 = Y3 - Y2;
|
||||
}
|
||||
|
||||
//Effectue a chaque tour ........................
|
||||
if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){
|
||||
Old_Nb_tours = Nb_tours;
|
||||
if (!Balise_Valide){
|
||||
|
||||
Loading…
Reference in New Issue
Block a user