Secoue les planches, v1
This commit is contained in:
parent
aa9ec10e42
commit
73adee0cbe
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user