diff --git a/Actionneurs/Actionneurs.ino b/Actionneurs/Actionneurs.ino index 4f5fe35..f61b7cc 100644 --- a/Actionneurs/Actionneurs.ino +++ b/Actionneurs/Actionneurs.ino @@ -25,6 +25,11 @@ #define AIMANT_PINCE_GAUCHE_TIENT 9, 500 #define AIMANT_PINCE_GAUCHE_LACHE 9, 400 +#define I2C_CDE_DEMANDE 0x00 +#define I2C_DCE_REALISE 0x01 + +volatile byte * I2C_memory; + SMS_STS sms_sts; int ID_Feetech = 4; @@ -34,12 +39,7 @@ struct position_t{ uint position; } cible_haute, cible_basse, actuelle; - - - -enum etat_action_t Actionneur_empile(); - - +enum etat_action_t Actionneur_empile(void); void setup() { @@ -64,6 +64,9 @@ void setup() //ledcWrite(5, 227); // Position levée (butée haute) //ledcWrite(5, 345); // Position de prise + I2C_Slave_init(0x20); + I2C_memory = get_i2c_data(); + /* while(1){ ledcWrite(FOURCHE_PRISE); @@ -211,6 +214,56 @@ void loop() int Move; int Current; + if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_DCE_REALISE]){ + switch (I2C_memory[I2C_CDE_DEMANDE]){ + case 0: + // Aucune commande + I2C_memory[I2C_DCE_REALISE] = 0; + break; + + case 1: + // Position de départ + break; + + case 2: + // Position de déplacement + // Fourche levée pour ne pas géner les capteurs + Servo_set(FOURCHE_LEVEE); + // 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_LACHE); + Servo_set(AIMANT_PINCE_GAUCHE_LACHE); + // Translateur à l'arrière + Translateur_recule(); + I2C_memory[I2C_DCE_REALISE] = 2; + break; + + case 3: + // Prise initiale + // Fourche en position de prise + 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(); + I2C_memory[I2C_DCE_REALISE] = 3; + + case 4: + // Position de transport + } + + } + if (Serial.available() > 0) { // get incoming byte: @@ -252,6 +305,9 @@ void loop() if(inByte == 'u'){ while(Actionneur_empile() == ACTION_EN_COURS); } + if(inByte == 'v'){ + Ascenseur_cycle(); + } if(inByte == 'c'){ Serial.println("Cycle translateur"); while(Serial.available() > 0){ @@ -264,8 +320,6 @@ void loop() } Serial.printf(">GPIO2:%d\n",digitalRead(2)); Serial.printf(">GPIO10:%d\n",digitalRead(10)); - Serial.printf(">OUT3:%d\n",digitalRead(3)); - Serial.printf(">OUT4:%d\n",digitalRead(4)); delay(25); } } @@ -365,7 +419,7 @@ enum etat_action_t Actionneur_empile(){ Servo_set(AIMANT_PINCE_DROIT_LACHE); Servo_set(AIMANT_PINCE_GAUCHE_LACHE); // Attente avance - delay(5000); + delay(2000); etat_actionneur = ACTIONNEUR_DEPILE; break; @@ -386,6 +440,7 @@ enum etat_action_t Actionneur_empile(){ case ACTIONNEUR_PRISE_INTERNE_2: Serial.println("ACTIONNEUR_PRISE_INTERNE_2"); if(Translateur_etat() == ACTION_TERMINEE){ + delay(500); Servo_set(DOIGT_PINCE_GAUCHE_FERME); Servo_set(DOIGT_PINCE_DROIT_FERME); delay(500); @@ -396,7 +451,6 @@ enum etat_action_t Actionneur_empile(){ 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; @@ -419,28 +473,35 @@ enum etat_action_t Actionneur_empile(){ case ACTIONNEUR_AVANCE_PRISE_INTERNE: Serial.println("ACTIONNEUR_AVANCE_PRISE_INTERNE"); if(Translateur_etat() == ACTION_TERMINEE){ + delay(500); + Ascenseur_depose(); + Servo_set(DOIGT_PINCE_DROIT_OUVRE); + Servo_set(DOIGT_PINCE_GAUCHE_OUVRE); 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; + Serial.println("ACTIONNEUR_DEPOSE_PRISE_INTERNE"); + if(Ascenseur_get_etat() == ACTION_TERMINEE){ + delay(500); + Translateur_recule(); + etat_actionneur = ACTIONNEUR_RANGE_TRANSLATEUR; + } break; case ACTIONNEUR_RANGE_TRANSLATEUR: + Serial.println("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(); + Ascenseur_descend(); } break; case ACTIONNEUR_DESCEND: + Serial.println("ACTIONNEUR_DESCEND"); if(Ascenseur_get_etat() == ACTION_TERMINEE){ etat_actionneur = ACTIONNEUR_DEPLACEMENT; Servo_set(FOURCHE_TRANSPORT); diff --git a/Actionneurs/Ascenseur.h b/Actionneurs/Ascenseur.h index 430f6af..bca8168 100644 --- a/Actionneurs/Ascenseur.h +++ b/Actionneurs/Ascenseur.h @@ -10,4 +10,6 @@ int Ascenseur_descend(void); void Ascenseur_step_up(void); void Ascenseur_step_down(void); void Ascenseur_gestion(void); +void Ascenseur_cycle(void); +int Ascenseur_depose(void); enum etat_action_t Ascenseur_get_etat(void); \ No newline at end of file diff --git a/Actionneurs/Ascenseur.ino b/Actionneurs/Ascenseur.ino index 4b9fced..a499272 100644 --- a/Actionneurs/Ascenseur.ino +++ b/Actionneurs/Ascenseur.ino @@ -9,6 +9,7 @@ enum etat_ascenseur_t{ ASCENSEUR_ACTIF, ASCENSEUR_MONTE, ASCENSEUR_DESCENT, + ASCENSEUR_DEPOSE, ASCENSEUR_BUSY, }etat_ascenseur=ASCENSEUR_INIT; @@ -139,6 +140,11 @@ enum etat_action_t Ascenseur_cherche_butees(void){ /// @return 1 si l'ascenseur n'est pas prêt int Ascenseur_monte(void){ if(etat_ascenseur == ASCENSEUR_ACTIF){ + tab_position[0] = position_haute_droit; + tab_position[1] = position_haute_gauche; + tab_vitesses_u[0] = 2000; + tab_vitesses_u[1] = 2000; + sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc); etat_ascenseur = ASCENSEUR_MONTE; return 0; } @@ -149,11 +155,30 @@ int Ascenseur_monte(void){ /// @return 1 si l'ascenseur n'est pas prêt int Ascenseur_descend(void){ if(etat_ascenseur == ASCENSEUR_ACTIF){ + tab_position[0] = position_basse_droit; + 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); etat_ascenseur = ASCENSEUR_DESCENT; return 0; } return 1; } +/// @brief Commande l'ascenseur en position de dépose +/// @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_vitesses_u[0] = 2000; + tab_vitesses_u[1] = 2000; + sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc); + etat_ascenseur = ASCENSEUR_DEPOSE; + return 0; + } + return 1; +} enum etat_action_t Ascenseur_get_etat(void){ if(etat_ascenseur == ASCENSEUR_ACTIF){ @@ -195,6 +220,24 @@ void Ascenseur_update_step(void){ } +void Ascenseur_cycle(){ + while(!Serial.available()){ + Serial.println("Ascenseur_monte"); + Ascenseur_monte(); + while(Ascenseur_get_etat() != ACTION_TERMINEE){ + Ascenseur_gestion(); + } + Serial.println("Ascenseur_descend"); + Ascenseur_descend(); + while(Ascenseur_get_etat() != ACTION_TERMINEE){ + Ascenseur_gestion(); + } + } + while(Serial.available() > 0){ + Serial.read(); + } +} + void Ascenseur_gestion(void){ bool erreur; @@ -218,27 +261,24 @@ void Ascenseur_gestion(void){ break; case ASCENSEUR_MONTE: - tab_position[0] = position_haute_droit; - tab_position[1] = position_haute_gauche; - tab_vitesses_u[0] = 2000; - tab_vitesses_u[1] = 2000; - sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc); + delay(200); etat_ascenseur = ASCENSEUR_BUSY; break; case ASCENSEUR_DESCENT: - tab_position[0] = position_basse_droit; - 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); + delay(200); + etat_ascenseur = ASCENSEUR_BUSY; + break; + + case ASCENSEUR_DEPOSE: + delay(200); etat_ascenseur = ASCENSEUR_BUSY; break; case ASCENSEUR_BUSY: mov_g = sms_sts.ReadMove(ID_FEETECH_ASC_G); mov_d = sms_sts.ReadMove(ID_FEETECH_ASC_D); - if(!mov_g && !mov_d){ + if(!mov_g && !mov_d) { // Fin du mouvement etat_ascenseur = ASCENSEUR_ACTIF; diff --git a/Actionneurs/I2C_Slave_lib.ino b/Actionneurs/I2C_Slave_lib.ino new file mode 100644 index 0000000..1cf38b4 --- /dev/null +++ b/Actionneurs/I2C_Slave_lib.ino @@ -0,0 +1,61 @@ +#define TAILLE_MEMOIRE_I2C 256 +#define TAILLE_MESSAGE_ENVOI_MAX 32 +byte memoire_I2C[TAILLE_MEMOIRE_I2C]; +byte memoire_I2C_index=0; + +bool nouveau_message=false; + +uint8_t * get_i2c_data(){ + return memoire_I2C; +} +void onRequest(){ + uint32_t taille_envoi; + taille_envoi = min (TAILLE_MEMOIRE_I2C-memoire_I2C_index, TAILLE_MESSAGE_ENVOI_MAX); + + Wire.write(&memoire_I2C[memoire_I2C_index], taille_envoi); + memoire_I2C_index += taille_envoi; + if(memoire_I2C_index>=TAILLE_MEMOIRE_I2C){ + Serial.printf("memoire_I2C_index>=TAILLE_MEMOIRE_I2C\n"); + } +} + +void onReceive(int len){ + memoire_I2C_index = Wire.read(); + while(Wire.available()){ + nouveau_message=true; + memoire_I2C[memoire_I2C_index] = Wire.read(); + memoire_I2C_index++; + } + //Serial.printf("I2C reg: %d, val %d\n", memoire_I2C_index, memoire_I2C[memoire_I2C_index]); +} + +void I2C_Slave_init(int addr){ + Wire.onReceive(onReceive); + Wire.onRequest(onRequest); + Wire.begin(addr); +} + +bool I2C_Slave_nouveau_message(){ + if(nouveau_message){ + nouveau_message=false; + return true; + } + return false; +} + +void I2C_envoi_8bits(byte value, char adresse){ + //printf("I2C_envoi_8bits a:%d v:%d %x %b\n",adresse, value, value, value); + memoire_I2C[adresse] = value; +} + +void I2C_envoi_16bits(int16_t value, char adresse){ + memoire_I2C[adresse] = (value >> 8) & 0xFF; + memoire_I2C[adresse+1] = value & 0xFF; +} + +void I2C_envoi_32bits(int32_t value, char adresse){ + memoire_I2C[adresse] = value >> 24; + memoire_I2C[adresse+1] = (value >> 16) & 0xFF; + memoire_I2C[adresse+2] = (value >> 8) & 0xFF; + memoire_I2C[adresse+3] = value & 0xFF; +} diff --git a/Actionneurs/Translateur.ino b/Actionneurs/Translateur.ino new file mode 100644 index 0000000..448a4eb --- /dev/null +++ b/Actionneurs/Translateur.ino @@ -0,0 +1,92 @@ +#define DUREE_MVT_TRANSLATEUR_MS 1000 +enum translateur_action_t{ + TRANSLATEUR_INIT, + TRANSLATEUR_AVANCE, + TRANSLATEUR_RECULE, +}translateur_action = TRANSLATEUR_INIT; + +unsigned long temps_debut_action = 0; + +void Translateur_init(void){ +// Moteur + /*ledcAttach(3, 500, 8); + ledcWrite(3, 0); + ledcAttach(4, 500, 8); + ledcWrite(4, 0);*/ + pinMode(3, OUTPUT); + pinMode(4, OUTPUT); + // 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(temps_debut_action + DUREE_MVT_TRANSLATEUR_MS < millis()){ + if(translateur_action == TRANSLATEUR_AVANCE){ + if(digitalRead(2) == 0){ + return ACTION_TERMINEE; + } + }else if(translateur_action == TRANSLATEUR_RECULE){ + if(digitalRead(10) == 0){ + return ACTION_TERMINEE; + } + }else{ + 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 Translateur_avance(){ + Moteur_set(-255); + temps_debut_action = millis(); + translateur_action = TRANSLATEUR_AVANCE; +} + +void Translateur_recule(){ + Moteur_set(255); + temps_debut_action = millis(); + translateur_action = TRANSLATEUR_RECULE; +} + +/// @brief pilote la vitesse des moteurs +/// @param vitesse vitesse signée, sera saturée à 255 +void Moteur_set(int vitesse){ + if(vitesse < 0){ + digitalWrite(3, 1); + digitalWrite(4, 0); + }else{ + digitalWrite(3, 0); + digitalWrite(4, 1); + } + /*ledcWrite(3, 0); + ledcWrite(4, 0); + if(vitesse < 0){ + vitesse = -vitesse; + if(vitesse > 255){ + vitesse = 255; + } + ledcWrite(3, vitesse); + }else{ + if(vitesse > 255){ + vitesse = 255; + } + ledcWrite(4, vitesse); + }*/ +} + diff --git a/Cerveau/Com_chassis.ino b/Cerveau/Com_chassis.ino index e5eb8cb..dab0b59 100644 --- a/Cerveau/Com_chassis.ino +++ b/Cerveau/Com_chassis.ino @@ -3,15 +3,21 @@ #include -/// @brief Lit l'état du chassis en I2C int Scan_chassis(struct chassis_reception_t * chassis_reception){ + return Scan_chassis(chassis_reception, true); +} + +/// @brief Lit l'état du chassis en I2C +int Scan_chassis(struct chassis_reception_t * chassis_reception, bool blocking){ unsigned char tampon2[14]; //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée) error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12); if (error !=0){ Err_Chassi_com =1;IndexErr = 2; affiche_erreur("Scan_Chassi", "Erreur I2C"); - while(1); + if(blocking){ + while(1); + } }else{ Err_Chassi_com =0; IndexErr = 0; diff --git a/Cerveau/Com_detection_adversaire.h b/Cerveau/Com_detection_adversaire.h index 2be81b1..c687c21 100644 --- a/Cerveau/Com_detection_adversaire.h +++ b/Cerveau/Com_detection_adversaire.h @@ -7,6 +7,7 @@ struct detect_adv_reception_t { unsigned char distance_cm[12]; }; -void Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception); +int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool); +int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception); #endif diff --git a/Cerveau/Com_detection_adversaire.ino b/Cerveau/Com_detection_adversaire.ino index baa86b4..f2eef8b 100644 --- a/Cerveau/Com_detection_adversaire.ino +++ b/Cerveau/Com_detection_adversaire.ino @@ -2,17 +2,21 @@ #include #include - +int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){ + return Detect_adv_lire(detect_adv_reception, true); +} /// @brief Lit les capteurs VL53L1X -void Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){ +int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool blocking){ unsigned char tampon2[14]; //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée) error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12); if (error !=0){ affiche_erreur("Scan_Detect_adversaire", "Erreur I2C"); - while(1); + if(blocking){ + while(1); + } }else{ Serial.println("I2C OK"); - } + return error; } diff --git a/Cerveau/Com_gradins.ino b/Cerveau/Com_gradins.ino index fc2dbe4..24b09e1 100644 --- a/Cerveau/Com_gradins.ino +++ b/Cerveau/Com_gradins.ino @@ -4,8 +4,12 @@ #include "Com_gradins.h" +int Detect_gradin(struct detect_gradin_t * detect_gradin){ + return Detect_gradin(detect_gradin, true); +} + /// @brief Lit les capteurs VL53L1X -void Detect_gradin(struct detect_gradin_t * detect_gradin){ +int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){ unsigned char tampon[14]; char chaine[200]; int angle_mrad; @@ -13,7 +17,9 @@ void Detect_gradin(struct detect_gradin_t * detect_gradin){ error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13); if (error !=0){ affiche_erreur("Detect_gradin", "Erreur I2C"); - while(1); + if (blocking){ + while(1); + } }else{ detect_gradin->status = tampon[0]; @@ -21,8 +27,6 @@ void Detect_gradin(struct detect_gradin_t * detect_gradin){ detect_gradin->centre_y_mm = tampon[5] << 24 | tampon[6] << 16 | tampon[7] << 8 | tampon[8]; angle_mrad = tampon[9] << 24 | tampon[10] << 16 | tampon[11] << 8 | tampon[12]; detect_gradin->angle_rad = angle_mrad / 1000.; - - - } + return error; } diff --git a/Cerveau/Com_triangulation.ino b/Cerveau/Com_triangulation.ino index 3a3f548..7dbe07c 100644 --- a/Cerveau/Com_triangulation.ino +++ b/Cerveau/Com_triangulation.ino @@ -1,5 +1,10 @@ +int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){ + return Scan_Triangulation(triangulation_reception, true); +} + + /// @brief Récupère position (X, Y) et l'orientation du robot -void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){ +int Scan_Triangulation(struct triangulation_reception_t * triangulation_reception, bool blocking){ unsigned char tampon2[14]; lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0; //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée) @@ -8,7 +13,10 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti if (error !=0){ Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0; affiche_erreur("Scan_Triangulation", "Erreur I2C"); - while(1); + if(blocking){ + while(1); + } + return error; } else{Err_Tri_com =0;IndexErr = 0;} if (error ==0){ @@ -35,6 +43,7 @@ void Scan_Triangulation(struct triangulation_reception_t * triangulation_recepti if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){ triangulation_reception->validite = true; } + return error; } void Triangulation_send_immobile(int immobile){ diff --git a/Cerveau/ServerWeb.ino b/Cerveau/ServerWeb.ino index 7d64c03..9ccc9bd 100644 --- a/Cerveau/ServerWeb.ino +++ b/Cerveau/ServerWeb.ino @@ -105,12 +105,19 @@ void showStatus() { char message_statu[500]; const char message_statu_tmplt[] = "A simple form\ \n

Statu du robot

\n\ -

Chassis: %s

Gradin: %d

Triangulation: %d

Detection_adversaire: %d

"; +

Chassis: %s

Gradin: %s

Triangulation: %s

Detection_adversaire: %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; - statu_to_string(chassis, !Scan_chassis(&chassis_reception) ); + 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) ); - sprintf(message_statu, message_statu_tmplt, chassis, 0, 0, 0); + + sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire); server.send(200, "text/html", message_statu); } diff --git a/Doc/Communication I2C.odt b/Doc/Communication I2C.odt index 1e3aea1..f1193c0 100644 Binary files a/Doc/Communication I2C.odt and b/Doc/Communication I2C.odt differ diff --git a/Doc/Communication I2C.pdf b/Doc/Communication I2C.pdf index 81a483a..ac3d41c 100644 Binary files a/Doc/Communication I2C.pdf and b/Doc/Communication I2C.pdf differ