Deplacement absolu fonctionnel - mais un problème d'echelle mm <=> pas le rend pas très efficace
This commit is contained in:
parent
33ff8f17b3
commit
5fb503883c
@ -92,7 +92,7 @@ bool Balises_OK = 0;
|
||||
int tolerance_position =100;
|
||||
|
||||
|
||||
char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position"};
|
||||
char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"};
|
||||
char* statu[] = {"/..","./.","../"};
|
||||
int index_statu=0;
|
||||
|
||||
@ -152,7 +152,7 @@ void loop() {
|
||||
static int64_t time;
|
||||
struct chassis_reception_t chassis_reception;
|
||||
|
||||
strategie();
|
||||
gestion_match();
|
||||
affichage_resultats();
|
||||
delay(10);
|
||||
}
|
||||
@ -256,7 +256,7 @@ void affiche_msg(char * chaine1, char * chaine2){
|
||||
M5.Lcd.print(chaine2);
|
||||
}
|
||||
|
||||
void strategie(){
|
||||
void gestion_match(){
|
||||
struct chassis_reception_t chassis_reception;
|
||||
struct chassis_emission_t chassis_emission;
|
||||
struct triangulation_reception_t triangulation_reception;
|
||||
@ -267,6 +267,8 @@ void strategie(){
|
||||
ATTENTE_ORDRE=0,
|
||||
LECTURE_TRIANGULATION=1,
|
||||
DEPLACEMENT_RELATIF=2,
|
||||
MATCH_EN_COURS=3,
|
||||
TEST_DEPLACEMENT_ABSOLU=4,
|
||||
};
|
||||
|
||||
switch(index_Maitre){
|
||||
@ -292,19 +294,21 @@ void strategie(){
|
||||
}
|
||||
if(M5.BtnA.read() == 1){
|
||||
// Déplacement en X
|
||||
translation_x_mm = 10000;
|
||||
translation_x_mm = 2000;
|
||||
translation_y_mm = 0;
|
||||
rotation_rad = 0;
|
||||
|
||||
index_Maitre = DEPLACEMENT_RELATIF;
|
||||
}
|
||||
if(M5.BtnB.read() == 1){
|
||||
Serial.println("BtnB");
|
||||
// Déplacement en Y
|
||||
|
||||
translation_x_mm = 0;
|
||||
translation_y_mm = 2000;
|
||||
rotation_rad = 0;
|
||||
|
||||
index_Maitre = DEPLACEMENT_RELATIF;
|
||||
index_Maitre = TEST_DEPLACEMENT_ABSOLU;
|
||||
|
||||
}
|
||||
if(M5.BtnC.read() == 1){
|
||||
@ -336,26 +340,28 @@ void strategie(){
|
||||
index_Maitre = ATTENTE_ORDRE;
|
||||
}
|
||||
break;
|
||||
|
||||
case MATCH_EN_COURS:
|
||||
break;
|
||||
|
||||
case TEST_DEPLACEMENT_ABSOLU:
|
||||
if(deplacement_absolu(1000, 1000, 0, 0) == ACTION_TERMINEE){
|
||||
index_Maitre = ATTENTE_ORDRE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
/// @brief : compare la position actuelle et la position lue par la balise
|
||||
/// Note : Pour l'instant, on ne déclenche un mouvment qu'en cas d'ecart sur la distance, pas sur l'orientation.
|
||||
void compar_cinematique(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad,
|
||||
struct triangulation_reception_t triangulation_reception, struct chassis_emission_t * chassis_emission){
|
||||
|
||||
void compar_cinematique(){
|
||||
|
||||
// Consigne de position à atteindre en X, Y et angle par rapport à la position actuel
|
||||
// dans le repère du terrain
|
||||
//Position à atteindre théorique
|
||||
struct triangulation_reception_t triangulation_reception;
|
||||
|
||||
X_futur = MemCmd_X;
|
||||
Y_futur = MemCmd_Y;
|
||||
|
||||
Scan_Triangulation(&triangulation_reception); //Prise de la position actuel
|
||||
if(Balises_OK == true && error == 0){ //triangulation calcul valide ************** a prendre sur I2c
|
||||
compar_X = X_futur - Position_actuelle_X; //compar de la position théoriquement atteinte avec la position actuel
|
||||
compar_Y = Y_futur - Position_actuelle_Y; //YR : position actuel Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique)
|
||||
compar_X = consigne_x_mm - triangulation_reception.pos_x_mm; //compar de la position théoriquement atteinte avec la position actuel
|
||||
compar_Y = consigne_y_mm - triangulation_reception.pos_y_mm; //YR : position actuel Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique)
|
||||
printf("compar_X:%d\ncompar_y:%d\n", compar_X, compar_Y);
|
||||
|
||||
if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){
|
||||
Mvt_tolerance_OK = true;
|
||||
chassis_emission->status = MOUVEMENT_FINI;
|
||||
}else{
|
||||
// print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue
|
||||
|
||||
@ -363,23 +369,68 @@ void compar_cinematique(){
|
||||
float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y));
|
||||
float angle_robot_vers_destination = M_PI_2 - atan2(compar_Y, compar_X);
|
||||
|
||||
float distance_Y_ref_robot = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
|
||||
float distance_X_ref_robot = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
|
||||
chassis_emission->translation_x_mm = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
|
||||
chassis_emission->translation_y_mm = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
|
||||
chassis_emission->rotation_z_rad = 0;
|
||||
chassis_emission->status = MOUVEMENT_EN_COURS;
|
||||
}
|
||||
}
|
||||
|
||||
if(corrige == false){
|
||||
mem_x = distance_X_ref_robot; // faire une memoire et travailler avec
|
||||
mem_y = distance_Y_ref_robot;
|
||||
corrige = true;
|
||||
}
|
||||
Mvt_tolerance_OK = false;
|
||||
}
|
||||
enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad, int evitement){
|
||||
static enum{
|
||||
DA_INIT,
|
||||
DA_COMPARE_POSITIONS,
|
||||
DA_MVT_EN_COUR,
|
||||
} etat_deplacement = DA_INIT;
|
||||
static int mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad;
|
||||
static struct chassis_emission_t chassis_emission;
|
||||
struct triangulation_reception_t triangulation_reception;
|
||||
enum etat_action_t etat_deplacement_relatif;
|
||||
|
||||
switch(etat_deplacement){
|
||||
case DA_INIT:
|
||||
mem_consigne_x_mm = consigne_x_mm;
|
||||
mem_consigne_y_mm = consigne_y_mm;
|
||||
mem_consigne_orientation_rad = consigne_orientation_rad;
|
||||
etat_deplacement = DA_COMPARE_POSITIONS;
|
||||
Serial.printf("DA_INIT\n");
|
||||
break;
|
||||
|
||||
case DA_COMPARE_POSITIONS:
|
||||
Serial.printf("Scan_Triangulation\n");
|
||||
Scan_Triangulation(&triangulation_reception); //Prise de la position actuel
|
||||
if(triangulation_reception.validite == true){
|
||||
Serial.printf("Compare cinematique\n");
|
||||
compar_cinematique(mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad,
|
||||
triangulation_reception, &chassis_emission);
|
||||
if(chassis_emission.status == MOUVEMENT_EN_COURS){
|
||||
// C'est que la fonction compar_cinematique indique qu'on doit se déplacer
|
||||
// Les valeurs du déplacement sont renseignées dans "chassis_emission".
|
||||
Serial.printf("DA_MVT_EN_COUR\n");
|
||||
Serial.printf("trans_x:%d\ntrans_y:%d\nrot:%d\n",chassis_emission.translation_x_mm,
|
||||
chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad);
|
||||
etat_deplacement = DA_MVT_EN_COUR;
|
||||
}else{
|
||||
compar_X =0 ; //compar de la position théoriquement atteinte avec la position actuel
|
||||
compar_Y =0 ;
|
||||
mem_x =0 ; //compar de la position théoriquement atteinte avec la position actuel
|
||||
mem_y =0 ;
|
||||
// Alors nous sommes arrivés
|
||||
// On réinitialise la mahcine à état
|
||||
etat_deplacement = DA_INIT;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case DA_MVT_EN_COUR:
|
||||
etat_deplacement_relatif = deplacement_relatif(- chassis_emission.translation_x_mm,
|
||||
-chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad, evitement);
|
||||
if(etat_deplacement_relatif == ACTION_TERMINEE){
|
||||
Serial.printf("DA_COMPARE_POSITIONS\n");
|
||||
etat_deplacement = DA_COMPARE_POSITIONS;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return ACTION_EN_COURS;
|
||||
}
|
||||
|
||||
/// @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
|
||||
@ -437,6 +488,7 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
|
||||
unsigned char tampon2[14];
|
||||
lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
|
||||
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
|
||||
triangulation_reception->validite = false;
|
||||
error = I2C_lire_registre(I2C_SLAVE_trian, 0, tampon2, 13); // si errror != de 0 alors erreur de communication
|
||||
if (error !=0){
|
||||
Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
|
||||
@ -464,8 +516,6 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti
|
||||
}
|
||||
if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
|
||||
triangulation_reception->validite = true;
|
||||
}else{
|
||||
triangulation_reception->validite = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -13,7 +13,6 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){
|
||||
affiche_erreur("Scan_Chassi", "Erreur I2C");
|
||||
while(1);
|
||||
}else{
|
||||
Serial.println("I2C OK");
|
||||
Err_Chassi_com =0;
|
||||
IndexErr = 0;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user