Translateur et Ascenseur : encapsulation et relecture des butées du translateur
This commit is contained in:
parent
df7a90d5a5
commit
ef108cf44d
@ -13,9 +13,6 @@
|
||||
#define DOIGT_PINCE_GAUCHE_OUVRE 2367
|
||||
#define DOIGT_PINCE_GAUCHE_PARKING 1550
|
||||
|
||||
#define ID_FEETECH_ASC_G 11
|
||||
#define ID_FEETECH_ASC_D 10
|
||||
|
||||
SMS_STS sms_sts;
|
||||
|
||||
int ID_Feetech = 4;
|
||||
@ -31,6 +28,12 @@ enum etat_action_t{
|
||||
ACTION_ECHEC
|
||||
};
|
||||
|
||||
enum translateur_action_t{
|
||||
TRANSLATEUR_INIT,
|
||||
TRANSLATEUR_AVANCE,
|
||||
TRANSLATEUR_RECULE,
|
||||
}translateur_action = TRANSLATEUR_INIT;
|
||||
|
||||
|
||||
/// @brief pilote la vitesse des moteurs
|
||||
/// @param vitesse vitesse signée, sera saturée à 255
|
||||
@ -52,19 +55,22 @@ void Moteur_set(int vitesse){
|
||||
}
|
||||
|
||||
void Translateur_avance(){
|
||||
Moteur_set(128);
|
||||
Moteur_set(-255);
|
||||
translateur_action = TRANSLATEUR_AVANCE;
|
||||
}
|
||||
|
||||
void Translateur_recule(){
|
||||
Moteur_set(-128);
|
||||
Moteur_set(255);
|
||||
translateur_action = TRANSLATEUR_RECULE;
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
int liste_servo_id[4]= {3, 4, 10, 11};
|
||||
Serial.begin(115200);//sms舵机波特率115200
|
||||
Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
|
||||
sms_sts.pSerial = &Serial1;
|
||||
delay(1000);
|
||||
delay(5000);
|
||||
|
||||
cible_haute.nb_tour = 0;
|
||||
cible_haute.position = 500;
|
||||
@ -93,6 +99,18 @@ void setup()
|
||||
delay(5000);
|
||||
|
||||
}*/
|
||||
int error = 0;
|
||||
do{
|
||||
error = 0;
|
||||
for (int i =0; i<4; i++){
|
||||
int ID = sms_sts.Ping(liste_servo_id[i]);
|
||||
printf("Servo %d: %d\n", liste_servo_id[i], ID );
|
||||
if( ID != liste_servo_id[i]){
|
||||
error = 1;
|
||||
delay(100);
|
||||
}
|
||||
}
|
||||
}while(error);
|
||||
|
||||
|
||||
// Servo pinces à aimant
|
||||
@ -102,11 +120,8 @@ void setup()
|
||||
ledcAttach(9, 50, 12);
|
||||
ledcWrite(9, 307);
|
||||
|
||||
// Moteur
|
||||
ledcAttach(3, 500, 8);
|
||||
ledcWrite(3, 0);
|
||||
ledcAttach(4, 500, 8);
|
||||
ledcWrite(4, 0);
|
||||
Translateur_init();
|
||||
|
||||
|
||||
/*while(1){
|
||||
|
||||
@ -129,6 +144,15 @@ void setup()
|
||||
delay(5000);
|
||||
|
||||
}*/
|
||||
/*
|
||||
sms_sts.WheelMode(10);
|
||||
sms_sts.WriteSpe(10, 2000, 50);
|
||||
while(1){
|
||||
printf(">load_d:%d\n", sms_sts.ReadLoad(10));
|
||||
delay(25);
|
||||
}*/
|
||||
|
||||
|
||||
|
||||
|
||||
if(sms_sts.FeedBack(ID_Feetech)!=-1){
|
||||
@ -145,6 +169,45 @@ void setup()
|
||||
}
|
||||
|
||||
|
||||
void Translateur_init(void){
|
||||
// Moteur
|
||||
ledcAttach(3, 500, 8);
|
||||
ledcWrite(3, 0);
|
||||
ledcAttach(4, 500, 8);
|
||||
ledcWrite(4, 0);
|
||||
// Etat des contacteurs de fin de course
|
||||
// relecture de 3 sur 2 (Avant)
|
||||
// relecture de 4 sur 10 (Arrière)
|
||||
pinMode(2, INPUT);
|
||||
pinMode(10, INPUT);
|
||||
}
|
||||
|
||||
enum etat_action_t Translateur_etat(void){
|
||||
if(translateur_action == TRANSLATEUR_AVANCE){
|
||||
if(digitalRead(2) == 0){
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}
|
||||
if(translateur_action == TRANSLATEUR_RECULE){
|
||||
if(digitalRead(10) == 0){
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}
|
||||
return ACTION_EN_COURS;
|
||||
}
|
||||
|
||||
void Translateur_cycle(void){
|
||||
if(Translateur_etat() == ACTION_TERMINEE){
|
||||
if(translateur_action == TRANSLATEUR_RECULE){
|
||||
Translateur_avance();
|
||||
}else{
|
||||
Translateur_recule();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void loop()
|
||||
{
|
||||
static int m_pos=0;
|
||||
@ -152,22 +215,14 @@ void loop()
|
||||
static position_t cible;
|
||||
|
||||
|
||||
Serial.print(">millis:");
|
||||
Serial.println(millis());
|
||||
if(millis() > myTime + 4000){
|
||||
|
||||
|
||||
if(millis() > myTime + 100){
|
||||
myTime = millis();
|
||||
|
||||
Serial.print(">millis:");
|
||||
Serial.println(millis());
|
||||
}
|
||||
|
||||
int ID = sms_sts.Ping(ID_Feetech);
|
||||
if(ID!=-1){
|
||||
Serial.print("Servo ID:");
|
||||
Serial.println(ID, DEC);
|
||||
delay(100);
|
||||
}else{
|
||||
Serial.println("Ping servo ID error!");
|
||||
delay(2000);
|
||||
}
|
||||
|
||||
int Pos;
|
||||
int Speed;
|
||||
@ -194,13 +249,34 @@ void loop()
|
||||
if(inByte == 'l'){
|
||||
Ascenseur_descend();
|
||||
}
|
||||
if(inByte == 'a'){
|
||||
Translateur_avance();
|
||||
}
|
||||
if(inByte == 'z'){
|
||||
Translateur_recule();
|
||||
}
|
||||
if(inByte == 'c'){
|
||||
while(Serial.available() > 0){
|
||||
inByte = Serial.read();
|
||||
}
|
||||
while(1){
|
||||
Translateur_cycle();
|
||||
if(Serial.available() > 0){
|
||||
break;
|
||||
}
|
||||
printf(">GPIO2:%d\n",digitalRead(2));
|
||||
printf(">GPIO10:%d\n",digitalRead(10));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
while(Serial.available() > 0){
|
||||
inByte = Serial.read();
|
||||
}
|
||||
}
|
||||
|
||||
Ascenseur_gestion();
|
||||
|
||||
/*
|
||||
if(sms_sts.FeedBack(ID)!=-1){
|
||||
Pos = sms_sts.ReadPos(-1);
|
||||
Speed = sms_sts.ReadSpeed(-1);
|
||||
@ -227,6 +303,6 @@ void loop()
|
||||
}else{
|
||||
Serial.println("FeedBack err");
|
||||
delay(500);
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
|
||||
6
Actionneurs/Ascenseur.h
Normal file
6
Actionneurs/Ascenseur.h
Normal file
@ -0,0 +1,6 @@
|
||||
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);
|
||||
@ -52,6 +52,9 @@ void Ascenseur_config_servo(int id){
|
||||
|
||||
enum etat_action_t Ascenseur_cherche_butees(void){
|
||||
static bool butee_g = 0, butee_d = 0;
|
||||
static int old_pos_d, old_pos_g;
|
||||
int pos_d, pos_g, my_load;
|
||||
|
||||
static enum {
|
||||
CHERCHE_BUTEE_INIT,
|
||||
CHERCHE_BUTEE,
|
||||
@ -66,33 +69,49 @@ enum etat_action_t Ascenseur_cherche_butees(void){
|
||||
tab_vitesses[1] = 500 * ASC_G_SIGNE;
|
||||
sms_sts.WheelMode(ID_FEETECH_ASC_D);
|
||||
sms_sts.WheelMode(ID_FEETECH_ASC_G);
|
||||
old_pos_d = sms_sts.ReadPos(ID_FEETECH_ASC_D);
|
||||
old_pos_g = sms_sts.ReadPos(ID_FEETECH_ASC_G);
|
||||
sms_sts.SyncWriteSpe(asc_ID, 2, tab_vitesses, tab_acc);
|
||||
delay(200);
|
||||
etat_cherche_butees = CHERCHE_BUTEE;
|
||||
Serial.printf("CHERCHE_BUTEE_INIT\n");
|
||||
break;
|
||||
|
||||
case CHERCHE_BUTEE:
|
||||
{
|
||||
int my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
|
||||
if(abs(my_load) > 150 ){
|
||||
|
||||
/*pos_d = sms_sts.ReadPos(ID_FEETECH_ASC_D);
|
||||
printf(">delta_pos_d:%d\n",pos_d - old_pos_d);
|
||||
if(abs(pos_d - old_pos_d) < 10 ){*/
|
||||
my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
|
||||
printf(">load_d:%d\n", my_load);
|
||||
if(abs(my_load) > 250 ){
|
||||
sms_sts.EnableTorque(ID_FEETECH_ASC_D, 0);
|
||||
position_basse_droit = sms_sts.ReadPos(ID_FEETECH_ASC_D);
|
||||
Serial.printf("position_basse_droit:%d\n", position_basse_droit);
|
||||
butee_d = true;
|
||||
}
|
||||
|
||||
//old_pos_d = pos_d;
|
||||
|
||||
/*
|
||||
pos_g = sms_sts.ReadPos(ID_FEETECH_ASC_G);
|
||||
printf(">delta_pos_g:%d\n",pos_g - old_pos_g);*/
|
||||
my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
|
||||
if(abs(my_load) > 150 ){
|
||||
printf(">load_g:%d\n", my_load);
|
||||
if(abs(my_load) > 250 ){
|
||||
sms_sts.EnableTorque(ID_FEETECH_ASC_G, 0);
|
||||
position_basse_gauche = sms_sts.ReadPos(ID_FEETECH_ASC_G);
|
||||
Serial.printf("position_basse_gauche:%d\n", position_basse_gauche);
|
||||
butee_g = true;
|
||||
}
|
||||
//old_pos_g = pos_g;
|
||||
|
||||
if(butee_d && butee_g){
|
||||
|
||||
etat_cherche_butees = EA_POS_INIT;
|
||||
}
|
||||
Serial.printf("CHERCHE_BUTEE\n");
|
||||
delay(100);
|
||||
break;}
|
||||
|
||||
case EA_POS_INIT:
|
||||
@ -106,6 +125,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
|
||||
tab_vitesses_u[1] = 2000;
|
||||
sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
|
||||
etat_cherche_butees = CHERCHE_BUTEE_INIT;
|
||||
Serial.printf("EA_POS_INIT\n");
|
||||
return ACTION_TERMINEE;
|
||||
break;
|
||||
}
|
||||
@ -177,10 +197,12 @@ void Ascenseur_gestion(void){
|
||||
case ASCENSEUR_INIT:
|
||||
// On vérifie que les deux servomoteurs répondent
|
||||
do{
|
||||
erreur = 0;
|
||||
int ID = sms_sts.Ping(asc_ID[0]);
|
||||
erreur = sms_sts.getLastError();
|
||||
ID = sms_sts.Ping(asc_ID[1]);
|
||||
erreur += sms_sts.getLastError();
|
||||
Serial.printf("ASCENSEUR_INIT erreur:%d\n",erreur);
|
||||
}while(erreur != 0);
|
||||
etat_ascenseur = ASCENSEUR_ACTIF;
|
||||
break;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user