Compare commits
No commits in common. "22d47c804c7a05feadbad3c08a6991c5630db631" and "8bdffe821dafbed3f768d1098eaeabd38fca2094" have entirely different histories.
22d47c804c
...
8bdffe821d
@ -5,7 +5,7 @@
|
||||
#define SERVO_PINCE_GAUCHE 8
|
||||
#define SERVO_PINCE_DROITE 9
|
||||
|
||||
#define FOURCHE_TRANSPORT 5, 277
|
||||
#define FOURCHE_TRANSPORT 5, 266
|
||||
#define FOURCHE_LEVEE 5, 206
|
||||
#define FOURCHE_PRISE 5, 308
|
||||
|
||||
@ -13,7 +13,7 @@
|
||||
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
|
||||
#define DOIGT_PINCE_GAUCHE_SEUIL 1545
|
||||
|
||||
#define DOIGT_PINCE_DROIT_FERME 4, 2500
|
||||
#define DOIGT_PINCE_DROIT_FERME 4, 2475
|
||||
#define DOIGT_PINCE_DROIT_OUVRE 4, 2365
|
||||
#define DOIGT_PINCE_DROIT_SEUIL 1575
|
||||
|
||||
@ -90,7 +90,6 @@ void setup()
|
||||
|
||||
|
||||
Translateur_init();
|
||||
|
||||
|
||||
|
||||
/*while(1){
|
||||
@ -115,7 +114,6 @@ void setup()
|
||||
|
||||
}*/
|
||||
Ascenseur_init();
|
||||
|
||||
}
|
||||
|
||||
void reglage_servo(int pin_servo){
|
||||
@ -201,8 +199,8 @@ void loop()
|
||||
break;
|
||||
|
||||
case 3:
|
||||
// Position de prise planche
|
||||
if(Actionneur_prepare_prise_planche() == ACTION_TERMINEE){
|
||||
// Position de pré-prise
|
||||
if(Actionneur_prepare_prise() == ACTION_TERMINEE){
|
||||
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
||||
}
|
||||
break;
|
||||
@ -221,21 +219,7 @@ void loop()
|
||||
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
||||
}
|
||||
break;
|
||||
|
||||
case 6:
|
||||
while(Actionneur_depile_planche() != ACTION_TERMINEE);
|
||||
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
||||
break;
|
||||
|
||||
case 7:
|
||||
while(Actionneur_prepare_prise_extern() != ACTION_TERMINEE);
|
||||
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
||||
break;
|
||||
|
||||
case 8:
|
||||
while(Actionneur_fourche_transport() != ACTION_TERMINEE);
|
||||
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
@ -298,9 +282,6 @@ void loop()
|
||||
if(inByte == 'f'){
|
||||
Servo_set(FOURCHE_LEVEE);
|
||||
}
|
||||
if(inByte == 'y'){
|
||||
while(Actionneur_depile_planche() == ACTION_EN_COURS);
|
||||
}
|
||||
if(inByte == '1'){
|
||||
reglage_servo(8);
|
||||
}
|
||||
@ -320,7 +301,7 @@ void loop()
|
||||
while(Actionneur_empile() == ACTION_EN_COURS);
|
||||
}
|
||||
if(inByte == 'n'){
|
||||
while(Actionneur_prepare_prise_planche() == ACTION_EN_COURS);
|
||||
while(Actionneur_prepare_prise() == ACTION_EN_COURS);
|
||||
}
|
||||
/*
|
||||
if(inByte == 'u'){
|
||||
@ -360,6 +341,34 @@ void loop()
|
||||
Serial.printf(">OUT4:%d\n",digitalRead(4));
|
||||
|
||||
Ascenseur_gestion();
|
||||
/*
|
||||
if(sms_sts.FeedBack(ID)!=-1){
|
||||
Pos = sms_sts.ReadPos(-1);
|
||||
Speed = sms_sts.ReadSpeed(-1);
|
||||
Load = sms_sts.ReadLoad(-1);
|
||||
Voltage = sms_sts.ReadVoltage(-1);
|
||||
Temper = sms_sts.ReadTemper(-1);
|
||||
Move = sms_sts.ReadMove(-1);
|
||||
Current = sms_sts.ReadCurrent(-1);
|
||||
Serial.print(">Position:");
|
||||
Serial.println(Pos);
|
||||
Serial.print(">Speed:");
|
||||
Serial.println(Speed);
|
||||
Serial.print(">Load:");
|
||||
Serial.println(Load);
|
||||
Serial.print(">Voltage:");
|
||||
Serial.println(Voltage);
|
||||
Serial.print(">Temper:");
|
||||
Serial.println(Temper);
|
||||
Serial.print(">Move:");
|
||||
Serial.println(Move);
|
||||
Serial.print(">Current:");
|
||||
Serial.println(Current);
|
||||
delay(50);
|
||||
}else{
|
||||
Serial.println("FeedBack err");
|
||||
delay(500);
|
||||
}*/
|
||||
|
||||
delay(25);
|
||||
}
|
||||
@ -397,24 +406,7 @@ enum etat_action_t Actionneur_deplacement(void){
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Actionneur_prepare_prise_extern(void){
|
||||
Servo_set(FOURCHE_TRANSPORT);
|
||||
// Ascenseur en bas
|
||||
Ascenseur_descend();
|
||||
// Pinces internes fermées
|
||||
Servo_set(DOIGT_PINCE_DROIT_FERME);
|
||||
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
|
||||
// Aimant sortis
|
||||
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
||||
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
||||
// Translateur à l'arrière
|
||||
Translateur_recule();
|
||||
|
||||
return ACTION_TERMINEE;
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Actionneur_prepare_prise_planche(void){
|
||||
enum etat_action_t Actionneur_prepare_prise(void){
|
||||
Servo_set(FOURCHE_PRISE);
|
||||
// Ascenseur en bas
|
||||
Ascenseur_descend();
|
||||
@ -431,32 +423,20 @@ enum etat_action_t Actionneur_prepare_prise_planche(void){
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Actionneur_depile_planche(void){
|
||||
Servo_set(AIMANT_PINCE_DROIT_LACHE);
|
||||
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
|
||||
delay(1000);
|
||||
ledcWrite(FOURCHE_LEVEE);
|
||||
delay(1000);
|
||||
for(int i=0;i<20;i++){
|
||||
ledcWrite(FOURCHE_PRISE);
|
||||
delay(50);
|
||||
ledcWrite(FOURCHE_LEVEE);
|
||||
delay(200);
|
||||
}
|
||||
|
||||
return ACTION_TERMINEE;
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Actionneur_prise_initiale(void){
|
||||
static enum etat_actionneur_t{
|
||||
ACTIONNEUR_DEPILE,
|
||||
ACTIONNEUR_PRISE_INTERNE_1,
|
||||
ACTIONNEUR_PRISE_INTERNE_2,
|
||||
ACTIONNEUR_RECULE_PRISE_INTERNE_1,
|
||||
ACTIONNEUR_RECULE_PRISE_INTERNE_2
|
||||
} etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
|
||||
} etat_actionneur=ACTIONNEUR_DEPILE;
|
||||
|
||||
switch(etat_actionneur){
|
||||
case ACTIONNEUR_DEPILE:
|
||||
Servo_set(FOURCHE_LEVEE);
|
||||
etat_actionneur = ACTIONNEUR_PRISE_INTERNE_1;
|
||||
break;
|
||||
|
||||
case ACTIONNEUR_PRISE_INTERNE_1:
|
||||
Serial.println("ACTIONNEUR_PRISE_INTERNE_1");
|
||||
@ -487,8 +467,8 @@ enum etat_action_t Actionneur_prise_initiale(void){
|
||||
case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
|
||||
Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
|
||||
if(Translateur_etat() == ACTION_TERMINEE){
|
||||
//Servo_set(FOURCHE_TRANSPORT);
|
||||
etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
|
||||
Servo_set(FOURCHE_TRANSPORT);
|
||||
etat_actionneur=ACTIONNEUR_DEPILE;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
@ -498,11 +478,6 @@ enum etat_action_t Actionneur_prise_initiale(void){
|
||||
|
||||
}
|
||||
|
||||
enum etat_action_t Actionneur_fourche_transport(){
|
||||
Servo_set(FOURCHE_TRANSPORT);
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
|
||||
enum etat_action_t Actionneur_empile(){
|
||||
static enum etat_actionneur_t{
|
||||
ACTIONNEUR_LEVE,
|
||||
|
||||
@ -338,15 +338,11 @@ void gestion_match(){
|
||||
translation_x_mm = 0;
|
||||
translation_y_mm = 0;
|
||||
rotation_rad = 100;
|
||||
Serial.println("C1");
|
||||
Triangulation_send_immobile(1);
|
||||
Serial.println("C2");
|
||||
while(M5.BtnC.read()){
|
||||
M5.update();
|
||||
}
|
||||
Serial.println("C3");
|
||||
delay(200);
|
||||
Serial.println("C4");
|
||||
IHM_attente_match(&couleur);
|
||||
tirette_enlevee = 0;
|
||||
|
||||
@ -442,49 +438,39 @@ void IHM_attente_match(int * couleur){
|
||||
|
||||
enum etat_action_t Strategie(int couleur){
|
||||
static enum {
|
||||
STRAT_INIT, // On attend de ne pas voir la tirette dans la detection de l'adversaire
|
||||
STRAT_RECULE_BANDEROLE, // Deplacement relatif
|
||||
STRAT_ALLER_GRADINS_1_A, // Déplacement relatif
|
||||
STRAT_ALLER_GRADINS_1_B, // Cherche gradin
|
||||
STRAT_ALLER_GRADINS_1_C, // Déplacement relatif
|
||||
STRAT_DEPOSE_GRADIN_1A, // Empile gradin
|
||||
STRAT_DEPOSE_GRADIN_1B, // Recule du gradin
|
||||
STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement relaif
|
||||
STRAT_ATTENTE_BACKSTAGE, // Attente
|
||||
STRAT_ALLER_BACKSTAGE, // Déplacement relatif
|
||||
STRAT_FIN
|
||||
STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
|
||||
STRAT_ALLER_BACKSTAGE // Déplacement relatif
|
||||
|
||||
}etat_strategie = STRAT_INIT;
|
||||
}etat_strategie = STRAT_RECULE_BANDEROLE;
|
||||
enum etat_action_t etat_action;
|
||||
int translation_x_mm, translation_y_mm;
|
||||
static unsigned long temps_timer;
|
||||
float rotation_rad;
|
||||
|
||||
switch(etat_strategie){
|
||||
case STRAT_INIT:
|
||||
temps_timer = millis();
|
||||
etat_strategie = STRAT_RECULE_BANDEROLE;
|
||||
break;
|
||||
case STRAT_RECULE_BANDEROLE:
|
||||
if(millis() - temps_timer >300){
|
||||
// Déplacement en X
|
||||
translation_x_mm = -300;
|
||||
translation_y_mm = 0;
|
||||
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;
|
||||
}
|
||||
// Déplacement en X
|
||||
translation_x_mm = -300;
|
||||
translation_y_mm = 0;
|
||||
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:
|
||||
translation_x_mm = -450;
|
||||
translation_x_mm = -500;
|
||||
rotation_rad = 0;
|
||||
if(couleur == COULEUR_JAUNE){
|
||||
translation_y_mm = -450;
|
||||
translation_y_mm = -500;
|
||||
}else{
|
||||
translation_y_mm = 450;
|
||||
translation_y_mm = 500;
|
||||
}
|
||||
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
@ -494,13 +480,13 @@ enum etat_action_t Strategie(int couleur){
|
||||
|
||||
case STRAT_ALLER_GRADINS_1_B:
|
||||
etat_action = gradin_approche();
|
||||
if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
etat_strategie = STRAT_ALLER_GRADINS_1_C;
|
||||
}
|
||||
break;
|
||||
|
||||
case STRAT_ALLER_GRADINS_1_C:
|
||||
etat_action = deplacement_relatif(100, 0, 0, 0);
|
||||
etat_action = deplacement_relatif(120, 0, 0, 0);
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
etat_strategie = STRAT_DEPOSE_GRADIN_1A;
|
||||
}
|
||||
@ -525,31 +511,22 @@ enum etat_action_t Strategie(int couleur){
|
||||
|
||||
case STRAT_ALLER_PREPA_BACKSTAGE:
|
||||
if(couleur == COULEUR_JAUNE){
|
||||
etat_action = deplacement_relatif(0, 0, -15. / 180 * M_PI, 0);
|
||||
etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
|
||||
}else{
|
||||
etat_action = deplacement_relatif(0, 0, 15. / 180 * M_PI, 0);
|
||||
etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0);
|
||||
}
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
etat_strategie = STRAT_ATTENTE_BACKSTAGE;
|
||||
}
|
||||
break;
|
||||
|
||||
case STRAT_ATTENTE_BACKSTAGE:
|
||||
if (millis() - temps_timer > 95000){
|
||||
etat_strategie = STRAT_ALLER_BACKSTAGE;
|
||||
}
|
||||
break;
|
||||
|
||||
case STRAT_ALLER_BACKSTAGE:
|
||||
etat_action = deplacement_relatif(-1140, 0, 0, 1);
|
||||
etat_action = deplacement_relatif(500, 0, 0, 0);
|
||||
if(etat_action == ACTION_TERMINEE){
|
||||
etat_strategie = STRAT_FIN;
|
||||
etat_strategie = STRAT_RECULE_BANDEROLE;
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
break;
|
||||
|
||||
case STRAT_FIN:
|
||||
return ACTION_TERMINEE;
|
||||
break;
|
||||
}
|
||||
|
||||
return ACTION_EN_COURS;
|
||||
@ -633,6 +610,7 @@ enum etat_action_t gradin_approche(void){
|
||||
|
||||
case GA_GOTO_EN_FACE:
|
||||
case GA_GOTO_LARGE:
|
||||
Detect_gradin(&detect_gradin);
|
||||
if(detect_gradin.status != 2){
|
||||
nb_erreur++;
|
||||
if(nb_erreur > 100){
|
||||
@ -670,8 +648,7 @@ enum etat_action_t gradin_approche(void){
|
||||
statu_approche_gradin = GA_CHERCHE_GAUCHE;
|
||||
}else if(detect_gradin.centre_y_mm < 180){
|
||||
statu_approche_gradin = GA_RECULE;
|
||||
}else if((fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD * 6) && (detect_gradin.centre_y_mm >= 240) ||
|
||||
(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD) && (detect_gradin.centre_y_mm < 240)){
|
||||
}else if(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD){
|
||||
angle_mem = detect_gradin.angle_rad;
|
||||
}else{
|
||||
if(detect_gradin.centre_y_mm > 240){
|
||||
@ -700,24 +677,11 @@ enum etat_action_t gradin_approche(void){
|
||||
|
||||
case GA_GOTO_PREND_2:
|
||||
// On positionne la fourche
|
||||
while(actionneur_action(ACTIONNEUR_PREPARE_PRISE_PLANCHE)!=ACTION_TERMINEE);
|
||||
while(actionneur_action(ACTIONNEUR_PREPARE_PRISE)!=ACTION_TERMINEE);
|
||||
// On Avance de 15 cm
|
||||
while(deplacement_relatif(150, 0, 0, 0) != ACTION_TERMINEE);
|
||||
// Depile les planches
|
||||
while(actionneur_action(ACTIONNEUR_DEPILE)!=ACTION_TERMINEE);
|
||||
// Recule de 4 cm
|
||||
while(deplacement_relatif(-40, 0, 0, 0) != ACTION_TERMINEE);
|
||||
// Prepa prise externe
|
||||
while(actionneur_action(ACTIONNEUR_PRISE_EXTERNE)!=ACTION_TERMINEE);
|
||||
// Avance de 4,4 cm
|
||||
while(deplacement_relatif(65, 0, 0, 0) != ACTION_TERMINEE);
|
||||
// On attrape les canettes internes
|
||||
// On attrape
|
||||
while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE);
|
||||
// On recule pour ne pas faire tomber la fourche sur les cannettes
|
||||
//while(deplacement_relatif(-20, 0, 0, 0) != ACTION_TERMINEE);
|
||||
// On baisse la fourche
|
||||
//while(actionneur_action(ACTIONNEUR_FOURCHE_TRANSPORT)!=ACTION_TERMINEE);
|
||||
|
||||
statu_approche_gradin = GA_CHERCHE_GAUCHE;
|
||||
return ACTION_TERMINEE;
|
||||
break;
|
||||
|
||||
@ -6,12 +6,9 @@
|
||||
#define ACTIONNEUR_NO_ACTION 0
|
||||
#define ACTIONNEUR_POS_INITIAL 1
|
||||
#define ACTIONNEUR_DEPLACEMENT 2
|
||||
#define ACTIONNEUR_PREPARE_PRISE_PLANCHE 3
|
||||
#define ACTIONNEUR_PREPARE_PRISE 3
|
||||
#define ACTIONNEUR_PRISE 4
|
||||
#define ACTIONNEUR_EMPILE 5
|
||||
#define ACTIONNEUR_DEPILE 6
|
||||
#define ACTIONNEUR_PRISE_EXTERNE 7
|
||||
#define ACTIONNEUR_FOURCHE_TRANSPORT 8
|
||||
|
||||
struct com_actionneur_t{
|
||||
char demande_action, action_terminee;
|
||||
|
||||
@ -8,23 +8,23 @@ int Scan_chassis(struct chassis_reception_t * chassis_reception){
|
||||
}
|
||||
|
||||
/// @brief Lit l'état du chassis en I2C
|
||||
int Scan_chassis(struct chassis_reception_t * chassis_reception, bool continuous_try){
|
||||
int Scan_chassis(struct chassis_reception_t * chassis_reception, bool blocking){
|
||||
unsigned char tampon2[14];
|
||||
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
|
||||
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
|
||||
while (error !=0 && continuous_try){
|
||||
if (error !=0){
|
||||
Err_Chassi_com =1;IndexErr = 2;
|
||||
affiche_erreur("Scan_Chassi", "Erreur I2C");
|
||||
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
|
||||
}
|
||||
if(error == 0){
|
||||
if(blocking){
|
||||
while(1);
|
||||
}
|
||||
}else{
|
||||
Err_Chassi_com =0;
|
||||
IndexErr = 0;
|
||||
|
||||
Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
|
||||
chassis_reception->status = tampon2[0];
|
||||
}
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
@ -55,11 +55,8 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
|
||||
Mot[10] = chassis_emission->acceleration;
|
||||
|
||||
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
|
||||
while (error !=0){
|
||||
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
|
||||
Err_Cha_send =1; IndexErr = 5;
|
||||
}
|
||||
Err_Cha_send =0;IndexErr = 0;
|
||||
if (error !=0){Err_Cha_send =1; IndexErr = 5;}
|
||||
else{Err_Cha_send =0;IndexErr = 0;}
|
||||
if(error==0){ //si pas d'erreur de transmission alors RAZ des valeurs
|
||||
nbr_essai ++;
|
||||
cmd_chassi_x = 0;
|
||||
|
||||
@ -6,14 +6,17 @@ int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){
|
||||
return Detect_adv_lire(detect_adv_reception, true);
|
||||
}
|
||||
/// @brief Lit les capteurs VL53L1X
|
||||
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool continuous_try){
|
||||
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool blocking){
|
||||
unsigned char tampon2[14];
|
||||
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
|
||||
error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
|
||||
while (error !=0 && continuous_try){
|
||||
if (error !=0){
|
||||
affiche_erreur("Scan_Detect_adversaire", "Erreur I2C");
|
||||
error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
|
||||
|
||||
if(blocking){
|
||||
while(1);
|
||||
}
|
||||
}else{
|
||||
Serial.println("I2C OK");
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user