diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino index d11eddd..68428a9 100644 --- a/Actionneurs/Actionneurs.ino +++ b/Actionneurs/Actionneurs.ino @@ -90,6 +90,7 @@ void setup() Translateur_init(); + /*while(1){ @@ -114,6 +115,7 @@ void setup() }*/ Ascenseur_init(); + } void reglage_servo(int pin_servo){ @@ -199,8 +201,8 @@ void loop() break; case 3: - // Position de pré-prise - if(Actionneur_prepare_prise() == ACTION_TERMINEE){ + // Position de prise planche + if(Actionneur_prepare_prise_planche() == ACTION_TERMINEE){ I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; } break; @@ -219,6 +221,16 @@ 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; } @@ -301,7 +313,7 @@ void loop() while(Actionneur_empile() == ACTION_EN_COURS); } if(inByte == 'n'){ - while(Actionneur_prepare_prise() == ACTION_EN_COURS); + while(Actionneur_prepare_prise_planche() == ACTION_EN_COURS); } /* if(inByte == 'u'){ @@ -341,34 +353,6 @@ 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); } @@ -406,7 +390,24 @@ enum etat_action_t Actionneur_deplacement(void){ } -enum etat_action_t Actionneur_prepare_prise(void){ +enum etat_action_t Actionneur_prepare_prise_extern(void){ + Servo_set(FOURCHE_PRISE); + // 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_TIENT); + Servo_set(AIMANT_PINCE_GAUCHE_TIENT); + // Translateur à l'arrière + Translateur_recule(); + + return ACTION_TERMINEE; + +} + +enum etat_action_t Actionneur_prepare_prise_planche(void){ Servo_set(FOURCHE_PRISE); // Ascenseur en bas Ascenseur_descend(); @@ -423,20 +424,37 @@ enum etat_action_t Actionneur_prepare_prise(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); + ledcWrite(FOURCHE_PRISE); + delay(100); + ledcWrite(FOURCHE_LEVEE); + delay(250); + ledcWrite(FOURCHE_PRISE); + delay(100); + ledcWrite(FOURCHE_LEVEE); + delay(250); + ledcWrite(FOURCHE_PRISE); + delay(100); + ledcWrite(FOURCHE_LEVEE); + + 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_DEPILE; + } etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1; 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"); @@ -444,6 +462,7 @@ enum etat_action_t Actionneur_prise_initiale(void){ Servo_set(DOIGT_PINCE_GAUCHE_OUVRE); Servo_set(DOIGT_PINCE_DROIT_OUVRE); + Servo_set(FOURCHE_LEVEE); etat_actionneur = ACTIONNEUR_PRISE_INTERNE_2; break; @@ -468,7 +487,7 @@ enum etat_action_t Actionneur_prise_initiale(void){ Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2"); if(Translateur_etat() == ACTION_TERMINEE){ Servo_set(FOURCHE_TRANSPORT); - etat_actionneur=ACTIONNEUR_DEPILE; + etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1; return ACTION_TERMINEE; } break; diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index ee4e734..02e8fee 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -425,12 +425,12 @@ void IHM_attente_match(int * couleur){ M5.update(); if(M5.BtnA.read() == 1){ affiche_msg("Couleur", " BLEU "); - //Triangulation_send_config(configuration_match_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); + Triangulation_send_config(configuration_match_jaune); m_couleur = COULEUR_JAUNE; } if(M5.BtnC.read() == 1){ @@ -449,12 +449,6 @@ enum etat_action_t Strategie(int couleur){ STRAT_ALLER_GRADINS_1_C, // Déplacement relatif STRAT_DEPOSE_GRADIN_1A, // Empile gradin STRAT_DEPOSE_GRADIN_1B, // Recule du gradin - STRAT_ALLER_GRADINS_2_A, // Tourne 80 ° - STRAT_ALLER_GRADINS_2_B, // Avance 20 cm - STRAT_ALLER_GRADINS_2_C, // Cherche gradin - STRAT_ALLER_GRADINS_2_D, - // - STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement relaif STRAT_ATTENTE_BACKSTAGE, // Attente STRAT_ALLER_BACKSTAGE, // Déplacement relatif @@ -506,7 +500,7 @@ enum etat_action_t Strategie(int couleur){ break; case STRAT_ALLER_GRADINS_1_C: - etat_action = deplacement_relatif(120, 0, 0, 0); + etat_action = deplacement_relatif(100, 0, 0, 0); if(etat_action == ACTION_TERMINEE){ etat_strategie = STRAT_DEPOSE_GRADIN_1A; } @@ -528,44 +522,6 @@ enum etat_action_t Strategie(int couleur){ etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE; } - case STRAT_ALLER_GRADINS_2_A: // Tourne 80 ° - translation_x_mm = 0; - if(couleur == COULEUR_JAUNE){ - rotation_rad = 80. * M_PI /180.; - }else{ - rotation_rad = -80. * M_PI /180.; - } - translation_y_mm = 0; - etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1); - if(etat_action == ACTION_TERMINEE){ - etat_strategie = STRAT_ALLER_GRADINS_2_B; - } - break; - - case STRAT_ALLER_GRADINS_2_B: // Avance 20 cm - translation_x_mm = 20; - rotation_rad = 0; - translation_y_mm = 0; - etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1); - if(etat_action == ACTION_TERMINEE){ - etat_strategie = STRAT_ALLER_GRADINS_2_C; - } - break; - - case STRAT_ALLER_GRADINS_2_C: // Cherche gradin - etat_action = gradin_approche(); - if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){ - etat_strategie = STRAT_ALLER_GRADINS_2_D; - } - break; - - case STRAT_ALLER_GRADINS_2_D: - etat_action = actionneur_action(ACTIONNEUR_EMPILE); - if(etat_action == ACTION_TERMINEE){ - etat_strategie = STRAT_FIN; - } - break; - case STRAT_ALLER_PREPA_BACKSTAGE: if(couleur == COULEUR_JAUNE){ @@ -585,7 +541,7 @@ enum etat_action_t Strategie(int couleur){ break; case STRAT_ALLER_BACKSTAGE: - etat_action = deplacement_relatif(-1140, 0, 0, 0); + etat_action = deplacement_relatif(-1140, 0, 0, 1); if(etat_action == ACTION_TERMINEE){ etat_strategie = STRAT_FIN; } @@ -714,7 +670,8 @@ 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){ + }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)){ angle_mem = detect_gradin.angle_rad; }else{ if(detect_gradin.centre_y_mm > 240){ @@ -743,10 +700,18 @@ enum etat_action_t gradin_approche(void){ case GA_GOTO_PREND_2: // On positionne la fourche - while(actionneur_action(ACTIONNEUR_PREPARE_PRISE)!=ACTION_TERMINEE); + while(actionneur_action(ACTIONNEUR_PREPARE_PRISE_PLANCHE)!=ACTION_TERMINEE); // On Avance de 15 cm while(deplacement_relatif(150, 0, 0, 0) != ACTION_TERMINEE); - // On attrape + // Depile les planches + while(actionneur_action(ACTIONNEUR_DEPILE)!=ACTION_TERMINEE); + // Recule de 4 cm + while(deplacement_relatif(-20, 0, 0, 0) != ACTION_TERMINEE); + // Prepa prise externe + while(actionneur_action(ACTIONNEUR_PRISE_EXTERNE)!=ACTION_TERMINEE); + // Avance de 4,4 cm + while(deplacement_relatif(45, 0, 0, 0) != ACTION_TERMINEE); + // On attrape les canettes internes while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE); statu_approche_gradin = GA_CHERCHE_GAUCHE; return ACTION_TERMINEE; diff --git a/Cerveau/Com_actionneur.h b/Cerveau/Com_actionneur.h index 7e05c3c..e0ab7cc 100644 --- a/Cerveau/Com_actionneur.h +++ b/Cerveau/Com_actionneur.h @@ -6,9 +6,11 @@ #define ACTIONNEUR_NO_ACTION 0 #define ACTIONNEUR_POS_INITIAL 1 #define ACTIONNEUR_DEPLACEMENT 2 -#define ACTIONNEUR_PREPARE_PRISE 3 +#define ACTIONNEUR_PREPARE_PRISE_PLANCHE 3 #define ACTIONNEUR_PRISE 4 #define ACTIONNEUR_EMPILE 5 +#define ACTIONNEUR_DEPILE 6 +#define ACTIONNEUR_PRISE_EXTERNE 7 struct com_actionneur_t{ char demande_action, action_terminee;