diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino index 9bc9b80..cd1b76d 100644 --- a/Actionneurs/Actionneurs.ino +++ b/Actionneurs/Actionneurs.ino @@ -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); - } + }*/ } diff --git a/Actionneurs/Ascenseur.h b/Actionneurs/Ascenseur.h new file mode 100644 index 0000000..0c67953 --- /dev/null +++ b/Actionneurs/Ascenseur.h @@ -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); \ No newline at end of file diff --git a/Actionneurs/Ascenseur.ino b/Actionneurs/Ascenseur.ino index 8992482..b7b2952 100644 --- a/Actionneurs/Ascenseur.ino +++ b/Actionneurs/Ascenseur.ino @@ -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;