WIP - sequence empillement

This commit is contained in:
Samuel 2025-05-24 19:12:26 +02:00
parent 4e192b6449
commit a650bacca9
3 changed files with 171 additions and 23 deletions

View File

@ -9,9 +9,9 @@
#define FOURCHE_LEVEE 5, 217
#define FOURCHE_PRISE 5, 327
#define DOIGT_PINCE_GAUCHE_FERME 3, 1000
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 800
#define DOIGT_PINCE_GAUCHE_SEUIL 815
#define DOIGT_PINCE_GAUCHE_FERME 3, 1500
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1842
#define DOIGT_PINCE_GAUCHE_SEUIL 1515
/// TODO renseigner de vraies valeurs
#define DOIGT_PINCE_DROIT_FERME 4, 1000
@ -34,11 +34,7 @@ struct position_t{
uint position;
} cible_haute, cible_basse, actuelle;
enum etat_action_t{
ACTION_EN_COURS,
ACTION_TERMINEE,
ACTION_ECHEC
};
enum translateur_action_t{
TRANSLATEUR_INIT,
@ -46,19 +42,7 @@ enum translateur_action_t{
TRANSLATEUR_RECULE,
}translateur_action = TRANSLATEUR_INIT;
enum etat_actionneur_t{
ACTIONNEUR_DEPLACEMENT,
ACTIONNEUR_PRISE_EXTERNE,
ACTIONNEUR_DEPILE,
ACTIONNEUR_PRISE_INTERNE,
ACTIONNEUR_RECULE_PRISE_INTERNE,
ACTIONNEUR_LEVE,
ACTIONNEUR_AVANCE_PRISE_INTERNE,
ACTIONNEUR_DEPOSE_PRISE_INTERNE,
ACTIONNEUR_RANGE_TRANSLATEUR,
ACTIONNEUR_DESCEND
}
enum etat_action_t Actionneur_empile();
/// @brief pilote la vitesse des moteurs
@ -145,6 +129,7 @@ void setup()
ledcAttach(9, 50, 12);
ledcWrite(9, 307);
Translateur_init();
@ -211,7 +196,7 @@ void Translateur_init(void){
enum etat_action_t Translateur_etat(void){
if(translateur_action == TRANSLATEUR_AVANCE){
if(digitalRead(2) == 0){
return ACTION_TERMINEE;
return ACTION_TERMINEE;
}
}
if(translateur_action == TRANSLATEUR_RECULE){
@ -317,6 +302,12 @@ void loop()
if(inByte == 'a'){
Translateur_avance();
}
if(inByte == 'w'){
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
}
if(inByte == 'x'){
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
}
if(inByte == 'z'){
Translateur_recule();
}
@ -329,6 +320,9 @@ void loop()
if(inByte == '3'){
reglage_servo(5);
}
if(inByte == 'u'){
while(Actionneur_empile() == ACTION_EN_COURS);
}
if(inByte == 'c'){
while(Serial.available() > 0){
inByte = Serial.read();
@ -348,6 +342,10 @@ void loop()
inByte = Serial.read();
}
}
Serial.printf(">Servo3:%d\n", sms_sts.ReadPos(3));
Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
Serial.printf(">GPIO2:%d\n",digitalRead(2));
Serial.printf(">GPIO10:%d\n",digitalRead(10));
Ascenseur_gestion();
/*
@ -378,7 +376,143 @@ void loop()
Serial.println("FeedBack err");
delay(500);
}*/
delay(25);
}
void Servo_set(int servo, int position){
switch(servo){
case 5:
case 8:
case 9:
ledcWrite(servo, position);
break;
case 3:
case 4:
sms_sts.WritePosEx(servo, position, 2000);
break;
}
}
enum etat_action_t Actionneur_empile(){
static enum etat_actionneur_t{
ACTIONNEUR_DEPLACEMENT,
ACTIONNEUR_PRISE_EXTERNE,
ACTIONNEUR_DEPILE,
ACTIONNEUR_PRISE_INTERNE_1,
ACTIONNEUR_PRISE_INTERNE_2,
ACTIONNEUR_RECULE_PRISE_INTERNE_1,
ACTIONNEUR_RECULE_PRISE_INTERNE_2,
ACTIONNEUR_LEVE,
ACTIONNEUR_AVANCE_PRISE_INTERNE,
ACTIONNEUR_DEPOSE_PRISE_INTERNE,
ACTIONNEUR_RANGE_TRANSLATEUR,
ACTIONNEUR_DESCEND
} etat_actionneur=ACTIONNEUR_DEPLACEMENT;
switch(etat_actionneur){
case ACTIONNEUR_DEPLACEMENT:
Servo_set(FOURCHE_TRANSPORT);
Translateur_recule();
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
Servo_set(DOIGT_PINCE_DROIT_FERME);
Servo_set(AIMANT_PINCE_DROIT_LACHE);
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
delay(5000);
etat_actionneur = ACTIONNEUR_PRISE_EXTERNE;
break;
case ACTIONNEUR_PRISE_EXTERNE:
Servo_set(FOURCHE_PRISE);
Servo_set(AIMANT_PINCE_DROIT_LACHE);
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
// Attente avance
delay(5000);
etat_actionneur = ACTIONNEUR_DEPILE;
break;
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");
Translateur_avance();
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
etat_actionneur = ACTIONNEUR_PRISE_INTERNE_2;
break;
case ACTIONNEUR_PRISE_INTERNE_2:
Serial.println("ACTIONNEUR_PRISE_INTERNE_2");
if(Translateur_etat() == ACTION_TERMINEE){
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
Servo_set(DOIGT_PINCE_DROIT_FERME);
delay(500);
etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_1;
}
break;
case ACTIONNEUR_RECULE_PRISE_INTERNE_1:
Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_1");
Translateur_recule();
delay(1000);
etat_actionneur = ACTIONNEUR_RECULE_PRISE_INTERNE_2;
break;
case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
if(Translateur_etat() == ACTION_TERMINEE){
etat_actionneur = ACTIONNEUR_LEVE;
Ascenseur_monte();
}
break;
case ACTIONNEUR_LEVE:
Serial.println("ACTIONNEUR_LEVE");
if(Ascenseur_get_etat() == ACTION_TERMINEE){
etat_actionneur = ACTIONNEUR_AVANCE_PRISE_INTERNE;
Translateur_avance();
}
break;
case ACTIONNEUR_AVANCE_PRISE_INTERNE:
Serial.println("ACTIONNEUR_AVANCE_PRISE_INTERNE");
if(Translateur_etat() == ACTION_TERMINEE){
etat_actionneur = ACTIONNEUR_DEPOSE_PRISE_INTERNE;
}
break;
case ACTIONNEUR_DEPOSE_PRISE_INTERNE:
Servo_set(DOIGT_PINCE_DROIT_OUVRE);
Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
delay(500);
Translateur_recule();
etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR;
break;
case ACTIONNEUR_RANGE_TRANSLATEUR:
if(Translateur_etat() == ACTION_TERMINEE){
etat_actionneur = ACTIONNEUR_DESCEND;
Servo_set(DOIGT_PINCE_DROIT_FERME);
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
Ascenseur_monte();
}
break;
case ACTIONNEUR_DESCEND:
if(Ascenseur_get_etat() == ACTION_TERMINEE){
etat_actionneur = ACTIONNEUR_DEPLACEMENT;
Servo_set(FOURCHE_TRANSPORT);
return ACTION_TERMINEE;
}
break;
}
Ascenseur_gestion();
return ACTION_EN_COURS;
}

View File

@ -1,6 +1,13 @@
enum etat_action_t{
ACTION_EN_COURS,
ACTION_TERMINEE,
ACTION_ECHEC
};
void Ascenseur_init(void);
int Ascenseur_monte(void);
int Ascenseur_descend(void);
void Ascenseur_step_up(void);
void Ascenseur_step_down(void);
void Ascenseur_gestion(void);
void Ascenseur_gestion(void);
enum etat_action_t Ascenseur_get_etat(void);

View File

@ -155,6 +155,13 @@ int Ascenseur_descend(void){
return 1;
}
enum etat_action_t Ascenseur_get_etat(void){
if(etat_ascenseur == ASCENSEUR_ACTIF){
return ACTION_TERMINEE;
}
return ACTION_EN_COURS;
}
void Ascenseur_step_up(void){
step--;
Ascenseur_update_step();