WIP - sequence empillement
This commit is contained in:
parent
4e192b6449
commit
a650bacca9
@ -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;
|
||||
}
|
||||
|
@ -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);
|
@ -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();
|
||||
|
Loading…
Reference in New Issue
Block a user