diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino index 4eeee3a..2d8fd1e 100644 --- a/Actionneurs/Actionneurs.ino +++ b/Actionneurs/Actionneurs.ino @@ -9,24 +9,22 @@ #define FOURCHE_LEVEE 5, 217 #define FOURCHE_PRISE 5, 327 -#define DOIGT_PINCE_GAUCHE_FERME 3, 1500 -#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1842 -#define DOIGT_PINCE_GAUCHE_SEUIL 1515 +#define DOIGT_PINCE_GAUCHE_FERME 3, 1530 +#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790 +#define DOIGT_PINCE_GAUCHE_SEUIL 1545 -/// TODO renseigner de vraies valeurs -#define DOIGT_PINCE_DROIT_FERME 4, 1000 -#define DOIGT_PINCE_DROIT_OUVRE 4, 800 -#define DOIGT_PINCE_DROIT_SEUIL 815 -/// FIN TODO +#define DOIGT_PINCE_DROIT_FERME 4, 2475 +#define DOIGT_PINCE_DROIT_OUVRE 4, 2365 +#define DOIGT_PINCE_DROIT_SEUIL 1575 #define AIMANT_PINCE_DROIT_TIENT 8, 147 #define AIMANT_PINCE_DROIT_LACHE 8, 337 -#define AIMANT_PINCE_GAUCHE_TIENT 9, 500 -#define AIMANT_PINCE_GAUCHE_LACHE 9, 400 +#define AIMANT_PINCE_GAUCHE_TIENT 9, 417 +#define AIMANT_PINCE_GAUCHE_LACHE 9, 217 #define I2C_CDE_DEMANDE 0x00 -#define I2C_DCE_REALISE 0x01 +#define I2C_CDE_REALISE 0x01 volatile byte * I2C_memory; @@ -34,14 +32,10 @@ SMS_STS sms_sts; int ID_Feetech = 4; -struct position_t{ - int nb_tour; - uint position; -} cible_haute, cible_basse, actuelle; - enum etat_action_t Actionneur_deplacement(void); enum etat_action_t Actionneur_prise_initiale(void); enum etat_action_t Actionneur_empile(void); +enum etat_action_t Actionneur_deplacement(void); void setup() { @@ -49,15 +43,8 @@ void setup() Serial.begin(115200);//sms舵机波特率115200 Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000 sms_sts.pSerial = &Serial1; - delay(5000); + //delay(5000); - cible_haute.nb_tour = 0; - cible_haute.position = 500; - - cible_basse.nb_tour = 2; - cible_basse.position = 1500; - - actuelle.nb_tour = 0; //Servo fourche (12V) ledcAttach(5, 50, 12); @@ -94,7 +81,6 @@ void setup() } }while(error); - // Servo pinces à aimant ledcAttach(8, 50, 12); ledcWrite(8, 307); @@ -127,27 +113,6 @@ 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){ - actuelle.position = sms_sts.ReadPos(-1); - }else{ - do{ - Serial.println("Erreur lecture"); - delay(1000); - }while(sms_sts.FeedBack(ID_Feetech) ==-1); - actuelle.position = sms_sts.ReadPos(-1); - } - Ascenseur_init(); } @@ -196,10 +161,6 @@ void loop() { static int m_pos=0; static unsigned long myTime=0; - static position_t cible; - - - if(millis() > myTime + 100){ myTime = millis(); @@ -216,11 +177,14 @@ void loop() int Move; int Current; - if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_DCE_REALISE]){ + //Serial.printf("I2C: %d - %d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]); + + if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_CDE_REALISE]){ + switch (I2C_memory[I2C_CDE_DEMANDE]){ case 0: // Aucune commande - I2C_memory[I2C_DCE_REALISE] = 0; + I2C_memory[I2C_CDE_REALISE] = 0; break; case 1: @@ -231,26 +195,29 @@ void loop() // Position de déplacement // Fourche levée pour ne pas géner les capteurs if(Actionneur_deplacement() == ACTION_TERMINEE){ - I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; + I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; } break; case 3: - // Prise initiale - if(Actionneur_prise_initiale() == ACTION_TERMINEE){ - I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; + // Position de pré-prise + if(Actionneur_prepare_prise() == ACTION_TERMINEE){ + I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; } break; case 4: - // Position de transport - // Utile ? - n'est-ce pas la position après la prise initiale ? + // Prise initiale + if(Actionneur_prise_initiale() == ACTION_TERMINEE){ + I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; + } break; case 5: // Empile if(Actionneur_empile() == ACTION_TERMINEE){ - I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; + Serial.println("I2C : ACTION_TERMINEE !!!"); + I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE]; } break; @@ -280,10 +247,12 @@ void loop() Serial.println("v: Cycle ascenseur"); Serial.println("c: Cycle translateur"); Serial.println("h: Actionneur, position de déplacement"); + Serial.println("n: Actionneur, prepare prise"); Serial.println("j: Actionneur, prise initiale"); Serial.println("k: Actionneur, empile"); + } if(inByte == 'd'){ Ascenseur_step_down(); @@ -305,9 +274,11 @@ void loop() } if(inByte == 'w'){ Servo_set(DOIGT_PINCE_GAUCHE_OUVRE); + Servo_set(DOIGT_PINCE_DROIT_OUVRE); } if(inByte == 'x'){ Servo_set(DOIGT_PINCE_GAUCHE_FERME); + Servo_set(DOIGT_PINCE_DROIT_FERME); } if(inByte == '1'){ reglage_servo(8); @@ -327,6 +298,9 @@ void loop() if(inByte == 'k'){ while(Actionneur_empile() == ACTION_EN_COURS); } + if(inByte == 'n'){ + while(Actionneur_prepare_prise() == ACTION_EN_COURS); + } /* if(inByte == 'u'){ while(Actionneur_deplacement() == ACTION_EN_COURS); @@ -430,6 +404,23 @@ enum etat_action_t Actionneur_deplacement(void){ } +enum etat_action_t Actionneur_prepare_prise(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 rentrés + 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_prise_initiale(void){ static enum etat_actionneur_t{ ACTIONNEUR_DEPILE, diff --git a/Actionneurs/Ascenseur.ino b/Actionneurs/Ascenseur.ino index a499272..bfc6162 100644 --- a/Actionneurs/Ascenseur.ino +++ b/Actionneurs/Ascenseur.ino @@ -120,8 +120,8 @@ enum etat_action_t Ascenseur_cherche_butees(void){ sms_sts.ServoMode(ID_FEETECH_ASC_G); tab_position[0] = position_basse_droit + (-250 * ASC_D_SIGNE); // Droit tab_position[1] = position_basse_gauche + (-250 * ASC_G_SIGNE); // Gauche - position_haute_droit = position_basse_droit - (8100 * ASC_D_SIGNE); - position_haute_gauche = position_basse_gauche - (8100 * ASC_G_SIGNE); + position_haute_droit = position_basse_droit - (9400 * ASC_D_SIGNE); + position_haute_gauche = position_basse_gauche - (9400 * ASC_G_SIGNE); tab_vitesses_u[0] = 2000; tab_vitesses_u[1] = 2000; sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc); @@ -169,8 +169,8 @@ int Ascenseur_descend(void){ /// @return 1 si l'ascenseur n'est pas prêt int Ascenseur_depose(void){ if(etat_ascenseur == ASCENSEUR_ACTIF){ - tab_position[0] = position_haute_droit*0.9 + position_basse_droit * 0.1; - tab_position[1] = position_haute_gauche * 0.9 + position_basse_gauche * 0.1; + tab_position[0] = position_haute_droit*0.8 + position_basse_droit * 0.2; + tab_position[1] = position_haute_gauche * 0.8 + position_basse_gauche * 0.2; tab_vitesses_u[0] = 2000; tab_vitesses_u[1] = 2000; sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc); @@ -207,13 +207,13 @@ void Ascenseur_update_step(void){ } if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) < 0){ tab_position[0] = position_basse_droit; - } + }/* if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) > 1){ tab_position[1] = position_haute_gauche; } if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) < 0){ tab_position[1] = position_basse_gauche; - } + }*/ tab_vitesses_u[0] = 2000; tab_vitesses_u[1] = 2000; sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc); diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino index a647d31..c5bae67 100644 --- a/Cerveau/Cerveau.ino +++ b/Cerveau/Cerveau.ino @@ -6,6 +6,7 @@ #include "Com_chassis.h" #include "Com_detection_adversaire.h" #include "Com_gradins.h" +#include "Com_actionneur.h" #include "ServerWeb.h" @@ -319,8 +320,10 @@ void gestion_match(){ translation_x_mm = 500; translation_y_mm = 0; rotation_rad = 0; + + actionneur_action_generique(5); - index_Maitre = TEST_APPROCHE_GRADIN; + //index_Maitre = TEST_APPROCHE_GRADIN; } if(M5.BtnB.read() == 1){ Serial.println("BtnB"); @@ -850,6 +853,43 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo return ACTION_EN_COURS; } +enum etat_action_t actionneur_pos_initiale(){ + +} + +enum etat_action_t actionneur_action_generique(int action){ + struct com_actionneur_t com_actionneur_env, com_actionneur_rec; + int action_terminee=0; + + com_actionneur_env.demande_action = action; + com_actionneur_rec.action_terminee = 0; + Serial.printf("Demande action %d\n", com_actionneur_env.demande_action); + Actionneur_send(&com_actionneur_env); + + Serial.printf("Lecture action...\n"); + do{ + Actionneur_scan(&com_actionneur_rec); + Serial.printf("Lecture action... %d %d\n", com_actionneur_rec.action_terminee, com_actionneur_env.demande_action); + }while(com_actionneur_env.action_terminee != com_actionneur_env.demande_action); + + // On réinitialise la machine à état + com_actionneur_env.demande_action = 0; + Serial.printf("Demande action %d\n", com_actionneur_env.demande_action); + Actionneur_send(&com_actionneur_env); + + Serial.printf("Lecture action...\n"); + do{ + Actionneur_scan(&com_actionneur_rec); + }while(com_actionneur_rec.action_terminee != com_actionneur_env.demande_action); + + return ACTION_TERMINEE; + + + + +} + + void scan_I2C_bus(){ char error, address; diff --git a/Cerveau/ServerWeb.ino b/Cerveau/ServerWeb.ino index 9ccc9bd..483e2c6 100644 --- a/Cerveau/ServerWeb.ino +++ b/Cerveau/ServerWeb.ino @@ -1,6 +1,7 @@ #include #include + //#include "Chassis.h" //#include "ServerWeb.h" @@ -101,23 +102,26 @@ void handleForm() { } void showStatus() { - char chassis[50], gradin[50], triangulation[50], detection_adversaire[50]; + char chassis[50], gradin[50], triangulation[50], detection_adversaire[50], actionneur[50]; char message_statu[500]; const char message_statu_tmplt[] = "A simple form\ \n

Statu du robot

\n\ -

Chassis: %s

Gradin: %s

Triangulation: %s

Detection_adversaire: %s

"; +

Chassis: %s

Gradin: %s

Triangulation: %s

Detection_adversaire: %s

Actionneur (0x20): %s

"; struct chassis_reception_t chassis_reception; struct detect_gradin_t detect_gradin; struct triangulation_reception_t triangulation_reception; struct detect_adv_reception_t detect_adv_reception; + struct com_actionneur_t com_actionneur; statu_to_string(chassis, !Scan_chassis(&chassis_reception, false) ); statu_to_string(gradin, !Detect_gradin(&detect_gradin, false) ); statu_to_string(triangulation, !Scan_Triangulation(&triangulation_reception, false) ); statu_to_string(detection_adversaire, !Detect_adv_lire(&detect_adv_reception, false) ); + statu_to_string(actionneur, !Actionneur_scan(&com_actionneur, false) ); - sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire); + + sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire, actionneur); server.send(200, "text/html", message_statu); }