Keuronde/Robot2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: Keuronde:master
Branches Tags
Keuronde:master
Keuronde:non_testé
..
compare: Keuronde:non_testé
Branches Tags
Keuronde:master
Keuronde:non_testé

No commits in common. "master" and "non_testé" have entirely different histories.
master ... non_testé

13 changed files with 186 additions and 455 deletions
Show Stats Expand all files Collapse all files

223
Actionneurs/Actionneurs.ino
View File

@ -5,26 +5,28 @@
#define SERVO_PINCE_GAUCHE 8
#define SERVO_PINCE_DROITE 9
#define FOURCHE_TRANSPORT 5, 277
#define FOURCHE_LEVEE 5, 206
#define FOURCHE_PRISE 5, 308
#define FOURCHE_TRANSPORT 5, 287
#define FOURCHE_LEVEE 5, 217
#define FOURCHE_PRISE 5, 327
#define DOIGT_PINCE_GAUCHE_FERME 3, 1530
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
#define DOIGT_PINCE_GAUCHE_SEUIL 1545
#define DOIGT_PINCE_GAUCHE_FERME 3, 1500
#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1842
#define DOIGT_PINCE_GAUCHE_SEUIL 1515
#define DOIGT_PINCE_DROIT_FERME 4, 2500
#define DOIGT_PINCE_DROIT_OUVRE 4, 2365
#define DOIGT_PINCE_DROIT_SEUIL 1575
/// 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 AIMANT_PINCE_DROIT_TIENT 8, 176
#define AIMANT_PINCE_DROIT_LACHE 8, 366
#define AIMANT_PINCE_DROIT_TIENT 8, 147
#define AIMANT_PINCE_DROIT_LACHE 8, 337
#define AIMANT_PINCE_GAUCHE_TIENT 9, 366
#define AIMANT_PINCE_GAUCHE_LACHE 9, 236
#define AIMANT_PINCE_GAUCHE_TIENT 9, 500
#define AIMANT_PINCE_GAUCHE_LACHE 9, 400
#define I2C_CDE_DEMANDE 0x00
#define I2C_CDE_REALISE 0x01
#define I2C_DCE_REALISE 0x01
volatile byte * I2C_memory;
@ -32,10 +34,14 @@ 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()
{
@ -43,8 +49,15 @@ 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);
@ -81,6 +94,7 @@ void setup()
}
}while(error);
// Servo pinces à aimant
ledcAttach(8, 50, 12);
ledcWrite(8, 307);
@ -90,7 +104,6 @@ void setup()
Translateur_init();
/*while(1){
@ -114,8 +127,28 @@ void setup()
delay(5000);
}*/
Ascenseur_init();
/*
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();
}
void reglage_servo(int pin_servo){
@ -163,12 +196,15 @@ void loop()
{
static int m_pos=0;
static unsigned long myTime=0;
static position_t cible;
if(millis() > myTime + 30){
if(millis() > myTime + 100){
myTime = millis();
Serial.print(">millis:");
Serial.println(millis());
Serial.printf(">I2C_c:%d\n>I2C_t:%d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]);
}
@ -180,12 +216,11 @@ void loop()
int Move;
int Current;
if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_CDE_REALISE]){
if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_DCE_REALISE]){
switch (I2C_memory[I2C_CDE_DEMANDE]){
case 0:
// Aucune commande
I2C_memory[I2C_CDE_REALISE] = 0;
I2C_memory[I2C_DCE_REALISE] = 0;
break;
case 1:
@ -196,46 +231,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_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
}
break;
case 3:
// Position de prise planche
if(Actionneur_prepare_prise_planche() == ACTION_TERMINEE){
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
// Prise initiale
if(Actionneur_prise_initiale() == ACTION_TERMINEE){
I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
}
break;
case 4:
// Prise initiale
if(Actionneur_prise_initiale() == ACTION_TERMINEE){
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
}
// Position de transport
// Utile ? - n'est-ce pas la position après la prise initiale ?
break;
case 5:
// Empile
if(Actionneur_empile() == ACTION_TERMINEE){
Serial.println("I2C : ACTION_TERMINEE !!!");
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
}
break;
case 6:
while(Actionneur_depile_planche() != ACTION_TERMINEE);
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
break;
case 7:
while(Actionneur_prepare_prise_extern() != ACTION_TERMINEE);
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
break;
case 8:
while(Actionneur_fourche_transport() != ACTION_TERMINEE);
I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
break;
}
@ -262,12 +280,10 @@ 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();
@ -289,17 +305,9 @@ 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 == 'f'){
Servo_set(FOURCHE_LEVEE);
}
if(inByte == 'y'){
while(Actionneur_depile_planche() == ACTION_EN_COURS);
}
if(inByte == '1'){
reglage_servo(8);
@ -319,9 +327,6 @@ void loop()
if(inByte == 'k'){
while(Actionneur_empile() == ACTION_EN_COURS);
}
if(inByte == 'n'){
while(Actionneur_prepare_prise_planche() == ACTION_EN_COURS);
}
/*
if(inByte == 'u'){
while(Actionneur_deplacement() == ACTION_EN_COURS);
@ -360,6 +365,34 @@ void loop()
Serial.printf(">OUT4:%d\n",digitalRead(4));
Ascenseur_gestion();
/*
if(sms_sts.FeedBack(ID)!=-1){
Pos = sms_sts.ReadPos(-1);
Speed = sms_sts.ReadSpeed(-1);
Load = sms_sts.ReadLoad(-1);
Voltage = sms_sts.ReadVoltage(-1);
Temper = sms_sts.ReadTemper(-1);
Move = sms_sts.ReadMove(-1);
Current = sms_sts.ReadCurrent(-1);
Serial.print(">Position:");
Serial.println(Pos);
Serial.print(">Speed:");
Serial.println(Speed);
Serial.print(">Load:");
Serial.println(Load);
Serial.print(">Voltage:");
Serial.println(Voltage);
Serial.print(">Temper:");
Serial.println(Temper);
Serial.print(">Move:");
Serial.println(Move);
Serial.print(">Current:");
Serial.println(Current);
delay(50);
}else{
Serial.println("FeedBack err");
delay(500);
}*/
delay(25);
}
@ -397,66 +430,20 @@ enum etat_action_t Actionneur_deplacement(void){
}
enum etat_action_t Actionneur_prepare_prise_extern(void){
Servo_set(FOURCHE_TRANSPORT);
// Ascenseur en bas
Ascenseur_descend();
// Pinces internes fermées
Servo_set(DOIGT_PINCE_DROIT_FERME);
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
// Aimant sortis
Servo_set(AIMANT_PINCE_DROIT_LACHE);
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
// Translateur à l'arrière
Translateur_recule();
return ACTION_TERMINEE;
}
enum etat_action_t Actionneur_prepare_prise_planche(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_depile_planche(void){
Servo_set(AIMANT_PINCE_DROIT_LACHE);
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
delay(1000);
ledcWrite(FOURCHE_LEVEE);
delay(1000);
for(int i=0;i<20;i++){
ledcWrite(FOURCHE_PRISE);
delay(50);
ledcWrite(FOURCHE_LEVEE);
delay(200);
}
return ACTION_TERMINEE;
}
enum etat_action_t Actionneur_prise_initiale(void){
static enum etat_actionneur_t{
ACTIONNEUR_DEPILE,
ACTIONNEUR_PRISE_INTERNE_1,
ACTIONNEUR_PRISE_INTERNE_2,
ACTIONNEUR_RECULE_PRISE_INTERNE_1,
ACTIONNEUR_RECULE_PRISE_INTERNE_2
} etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
} etat_actionneur=ACTIONNEUR_DEPILE;
switch(etat_actionneur){
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");
@ -487,8 +474,8 @@ enum etat_action_t Actionneur_prise_initiale(void){
case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
if(Translateur_etat() == ACTION_TERMINEE){
//Servo_set(FOURCHE_TRANSPORT);
etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
Servo_set(FOURCHE_TRANSPORT);
etat_actionneur=ACTIONNEUR_DEPILE;
return ACTION_TERMINEE;
}
break;
@ -498,11 +485,6 @@ enum etat_action_t Actionneur_prise_initiale(void){
}
enum etat_action_t Actionneur_fourche_transport(){
Servo_set(FOURCHE_TRANSPORT);
return ACTION_TERMINEE;
}
enum etat_action_t Actionneur_empile(){
static enum etat_actionneur_t{
ACTIONNEUR_LEVE,
@ -517,7 +499,6 @@ enum etat_action_t Actionneur_empile(){
case ACTIONNEUR_LEVE:
Serial.println("ACTIONNEUR_LEVE");
etat_actionneur = ACTIONNEUR_LEVE_1;
Servo_set(FOURCHE_PRISE);
Ascenseur_monte();
break;
@ -555,8 +536,6 @@ enum etat_action_t Actionneur_empile(){
etat_actionneur = ACTIONNEUR_DESCEND;
Servo_set(DOIGT_PINCE_DROIT_FERME);
Servo_set(DOIGT_PINCE_GAUCHE_FERME);
Servo_set(AIMANT_PINCE_DROIT_LACHE);
Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
Ascenseur_descend();
}
break;

16
Actionneurs/Ascenseur.ino
View File

@ -86,7 +86,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
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) > 200 ){
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);
@ -99,7 +99,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
printf(">delta_pos_g:%d\n",pos_g - old_pos_g);*/
my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
printf(">load_g:%d\n", my_load);
if(abs(my_load) > 200 ){
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);
@ -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 - (9400 * ASC_D_SIGNE);
position_haute_gauche = position_basse_gauche - (9400 * ASC_G_SIGNE);
position_haute_droit = position_basse_droit - (8100 * ASC_D_SIGNE);
position_haute_gauche = position_basse_gauche - (8100 * 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.8 + position_basse_droit * 0.2;
tab_position[1] = position_haute_gauche * 0.8 + position_basse_gauche * 0.2;
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);
@ -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);

256
Cerveau/Cerveau.ino
View File

@ -6,7 +6,6 @@
#include "Com_chassis.h"
#include "Com_detection_adversaire.h"
#include "Com_gradins.h"
#include "Com_actionneur.h"
#include "ServerWeb.h"
@ -281,7 +280,6 @@ void gestion_match(){
struct detect_gradin_t detect_gradin;
enum etat_action_t etat_action;
static int translation_x_mm, translation_y_mm;
static char tirette_enlevee=1;
static float rotation_rad;
static int couleur;
@ -321,7 +319,6 @@ void gestion_match(){
translation_x_mm = 500;
translation_y_mm = 0;
rotation_rad = 0;
//index_Maitre = DEPLACEMENT_RELATIF;
index_Maitre = TEST_APPROCHE_GRADIN;
}
@ -338,17 +335,12 @@ void gestion_match(){
translation_x_mm = 0;
translation_y_mm = 0;
rotation_rad = 100;
Serial.println("C1");
Triangulation_send_immobile(1);
Serial.println("C2");
while(M5.BtnC.read()){
M5.update();
}
Serial.println("C3");
delay(200);
Serial.println("C4");
IHM_attente_match(&couleur);
tirette_enlevee = 0;
index_Maitre = MATCH_EN_COURS;
@ -369,20 +361,14 @@ void gestion_match(){
case DEPLACEMENT_RELATIF : // Deplacement relatif en cours
if(deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1) != ACTION_EN_COURS){
if(deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1) == ACTION_TERMINEE){
index_Maitre = ATTENTE_ORDRE;
}
break;
case MATCH_EN_COURS:
Serial.printf(">tirette_m:%d\n", tirette_enlevee);
if(tirette_enlevee == 0){
lire_tirette(&tirette_enlevee);
}
if(tirette_enlevee == 1){
if(Strategie(couleur) == ACTION_TERMINEE){
index_Maitre = ATTENTE_ORDRE;
}
if(Strategie(couleur) == ACTION_TERMINEE){
index_Maitre = ATTENTE_ORDRE;
}
break;
@ -442,114 +428,73 @@ void IHM_attente_match(int * couleur){
enum etat_action_t Strategie(int couleur){
static enum {
STRAT_INIT, // On attend de ne pas voir la tirette dans la detection de l'adversaire
STRAT_RECULE_BANDEROLE, // Deplacement relatif
STRAT_ALLER_GRADINS_1_A, // Déplacement relatif
STRAT_ALLER_GRADINS_1_B, // Cherche gradin
STRAT_ALLER_GRADINS_1_A, // Déplacement absolu
STRAT_ALLER_GRADINS_1_B, // Déplacement relatif
STRAT_ALLER_GRADINS_1_C, // Déplacement relatif
STRAT_DEPOSE_GRADIN_1A, // Empile gradin
STRAT_DEPOSE_GRADIN_1B, // Recule du gradin
STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement relaif
STRAT_ATTENTE_BACKSTAGE, // Attente
STRAT_ALLER_BACKSTAGE, // Déplacement relatif
STRAT_FIN
STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
STRAT_ALLER_BACKSTAGE // Déplacement relatif
}etat_strategie = STRAT_INIT;
}etat_strategie = STRAT_RECULE_BANDEROLE;
enum etat_action_t etat_action;
int translation_x_mm, translation_y_mm;
static unsigned long temps_timer;
float rotation_rad;
switch(etat_strategie){
case STRAT_INIT:
temps_timer = millis();
etat_strategie = STRAT_RECULE_BANDEROLE;
break;
case STRAT_RECULE_BANDEROLE:
if(millis() - temps_timer >300){
// Déplacement en X
translation_x_mm = -300;
translation_y_mm = 0;
rotation_rad = 0;
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_GRADINS_1_A;
}
// Déplacement en X
translation_x_mm = -450;
translation_y_mm = 0;
rotation_rad = 0;
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_GRADINS_1_A;
}
break;
case STRAT_ALLER_GRADINS_1_A:
translation_x_mm = -450;
rotation_rad = 0;
if(couleur == COULEUR_JAUNE){
translation_y_mm = -450;
etat_action = deplacement_absolu(800, 800, -M_PI/2., 1);
}else{
translation_y_mm = 450;
etat_action = deplacement_absolu(3000 - 800, 800, -M_PI/2., 1);
}
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_GRADINS_1_B;
}
break;
case STRAT_ALLER_GRADINS_1_B:
etat_action = gradin_approche();
if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
etat_action = deplacement_relatif(300, 0, 0, 0);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_GRADINS_1_C;
}
break;
case STRAT_ALLER_GRADINS_1_C:
etat_action = deplacement_relatif(100, 0, 0, 0);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_DEPOSE_GRADIN_1A;
}
break;
case STRAT_DEPOSE_GRADIN_1A:
etat_action = actionneur_action(ACTIONNEUR_EMPILE);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_DEPOSE_GRADIN_1B;
}
break;
case STRAT_DEPOSE_GRADIN_1B:
translation_x_mm = -250;
rotation_rad = 0;
translation_y_mm = 0;
etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
etat_action = deplacement_relatif(-250, 0, 0, 0);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
}
break;
case STRAT_ALLER_PREPA_BACKSTAGE:
if(couleur == COULEUR_JAUNE){
etat_action = deplacement_relatif(0, 0, -15. / 180 * M_PI, 0);
etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
}else{
etat_action = deplacement_relatif(0, 0, 15. / 180 * M_PI, 0);
etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0);
}
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_ATTENTE_BACKSTAGE;
}
break;
case STRAT_ATTENTE_BACKSTAGE:
if (millis() - temps_timer > 95000){
etat_strategie = STRAT_ALLER_BACKSTAGE;
}
break;
case STRAT_ALLER_BACKSTAGE:
etat_action = deplacement_relatif(-1140, 0, 0, 1);
etat_action = deplacement_relatif(500, 0, 0, 0);
if(etat_action == ACTION_TERMINEE){
etat_strategie = STRAT_FIN;
etat_strategie = STRAT_RECULE_BANDEROLE;
return ACTION_TERMINEE;
}
break;
case STRAT_FIN:
return ACTION_TERMINEE;
break;
}
return ACTION_EN_COURS;
@ -565,8 +510,7 @@ enum etat_action_t gradin_approche(void){
GA_GOTO_EN_FACE,
GA_TOURNE_LARGE,
GA_GOTO_PROCHE,
GA_GOTO_PREND_1,
GA_GOTO_PREND_2,
GA_GOTO_PREND,
GA_RECULE
} statu_approche_gradin = GA_INIT;
static float angle_parcouru, angle_mem;
@ -584,7 +528,6 @@ enum etat_action_t gradin_approche(void){
switch(statu_approche_gradin){
case GA_INIT:
while(actionneur_action(ACTIONNEUR_DEPLACEMENT)!=ACTION_TERMINEE);
angle_parcouru = 0;
statu_approche_gradin = GA_CHERCHE_GAUCHE;
break;
@ -633,6 +576,7 @@ enum etat_action_t gradin_approche(void){
case GA_GOTO_EN_FACE:
case GA_GOTO_LARGE:
Detect_gradin(&detect_gradin);
if(detect_gradin.status != 2){
nb_erreur++;
if(nb_erreur > 100){
@ -646,12 +590,10 @@ enum etat_action_t gradin_approche(void){
// On se centre par rapport au gradin
//translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
//translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
/// Attention, réflection dans le repère des capteurs
translation_x = (detect_gradin.centre_y_mm + 75) * sin(detect_gradin.angle_rad) * cos(detect_gradin.angle_rad) + detect_gradin.centre_x_mm;
translation_y = (detect_gradin.centre_y_mm + 75) * sin(detect_gradin.angle_rad) * sin(detect_gradin.angle_rad) ;
translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
// Changement de repère : capteurs => Chassis
while(deplacement_relatif(translation_y, -translation_x, 0, 0) != ACTION_TERMINEE);
while(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE);
statu_approche_gradin = GA_TOURNE_LARGE;
angle_mem = detect_gradin.angle_rad;
@ -660,18 +602,13 @@ enum etat_action_t gradin_approche(void){
case GA_TOURNE_LARGE:
while(deplacement_relatif(0, 0, -angle_mem, 0) != ACTION_TERMINEE);
/*//// !!! DEBUG
statu_approche_gradin = GA_INIT;
return ACTION_ECHEC;
/// FIN DEBUG*/
delay(150);
Detect_gradin(&detect_gradin);
if(detect_gradin.status != 2){
statu_approche_gradin = GA_CHERCHE_GAUCHE;
}else if(detect_gradin.centre_y_mm < 180){
statu_approche_gradin = GA_RECULE;
}else if((fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD * 6) && (detect_gradin.centre_y_mm >= 240) ||
(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD) && (detect_gradin.centre_y_mm < 240)){
}else if(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD){
angle_mem = detect_gradin.angle_rad;
}else{
if(detect_gradin.centre_y_mm > 240){
@ -680,7 +617,7 @@ enum etat_action_t gradin_approche(void){
if(fabs(detect_gradin.centre_x_mm) > 10){
statu_approche_gradin = GA_GOTO_LARGE;
}else{
statu_approche_gradin = GA_GOTO_PREND_1;
statu_approche_gradin = GA_GOTO_PREND;
}
}
}
@ -693,36 +630,13 @@ enum etat_action_t gradin_approche(void){
}
break;
case GA_GOTO_PREND_1:
while(deplacement_relatif((detect_gradin.centre_y_mm - 25 - 110), 0, 0, 0) != ACTION_TERMINEE);
statu_approche_gradin = GA_GOTO_PREND_2;
break;
case GA_GOTO_PREND_2:
// On positionne la fourche
while(actionneur_action(ACTIONNEUR_PREPARE_PRISE_PLANCHE)!=ACTION_TERMINEE);
// On Avance de 15 cm
while(deplacement_relatif(150, 0, 0, 0) != ACTION_TERMINEE);
// Depile les planches
while(actionneur_action(ACTIONNEUR_DEPILE)!=ACTION_TERMINEE);
// Recule de 4 cm
while(deplacement_relatif(-40, 0, 0, 0) != ACTION_TERMINEE);
// Prepa prise externe
while(actionneur_action(ACTIONNEUR_PRISE_EXTERNE)!=ACTION_TERMINEE);
// Avance de 4,4 cm
while(deplacement_relatif(65, 0, 0, 0) != ACTION_TERMINEE);
// On attrape les canettes internes
while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE);
// On recule pour ne pas faire tomber la fourche sur les cannettes
//while(deplacement_relatif(-20, 0, 0, 0) != ACTION_TERMINEE);
// On baisse la fourche
//while(actionneur_action(ACTIONNEUR_FOURCHE_TRANSPORT)!=ACTION_TERMINEE);
statu_approche_gradin = GA_CHERCHE_GAUCHE;
case GA_GOTO_PREND:
while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE);
statu_approche_gradin = GA_INIT;
delay(5000);
return ACTION_TERMINEE;
break;
case GA_RECULE:
while(deplacement_relatif((detect_gradin.centre_y_mm - 220), 0, 0, 0) != ACTION_TERMINEE);
@ -877,47 +791,8 @@ int detection_adversaire(float angle_deplacement){
return 0;
}
#define SEUIL_OBSTACLE_CM 50
/// @brief A partir des déplacements X et Y, selection des capteurs à utiliser pour détecter l'adversaire
/// @return 1 obstable, 0 pas d'obstacle
int presence_obstacle(int distance_x_mm, int distance_y_mm){
struct detect_adv_reception_t detect_adv_reception;
Detect_adv_lire(&detect_adv_reception);
float angle = atan2f(distance_y_mm, distance_x_mm);
if(angle < 0){
angle += 2 * M_PI;
}
// Angle entre 0 et 2 PI
int segment_central = 23 - angle / 2. / M_PI * 24;
// segment_central entre 0 et 23;
if(segment_central == 23){
segment_central = 0;
}
// segment_central entre 0 et 22;
int capteur_central = segment_central / 2;
int capteur_suivant = capteur_central + 1;
int capteur_precedent = capteur_central - 1;
if(capteur_suivant > 11){
capteur_suivant = 0;
}
if(capteur_precedent < 0){
capteur_precedent = 11;
}
if(detect_adv_reception.distance_cm[capteur_precedent] < SEUIL_OBSTACLE_CM ||
detect_adv_reception.distance_cm[capteur_central] < SEUIL_OBSTACLE_CM ||
detect_adv_reception.distance_cm[capteur_suivant] < SEUIL_OBSTACLE_CM ){
// Arrêt du mouvement
return 1;
}
return 0;
}
/// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
/// @param evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
/// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
static enum{
DR_INIT,
@ -926,6 +801,7 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
struct chassis_emission_t chassis_emission;
struct chassis_reception_t chassis_reception;
struct detect_adv_reception_t detect_adv_reception;
switch(etat_deplacement){
case DR_INIT:
@ -943,11 +819,21 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
case DR_MVT_EN_COUR:
if(evitement){
if(presence_obstacle(distance_x_mm, distance_y_mm)){
chassis_emission.status = MOUVEMENT_INTERRUPTION;
send_Chassis(&chassis_emission);
etat_deplacement = DR_INIT;
return ACTION_ECHEC;
// On lit les capteurs
Detect_adv_lire(&detect_adv_reception);
// On analyse les valeurs - TODO : créer une fonction plus évoluée
if(distance_x_mm > 0){
if(detect_adv_reception.distance_cm[0] < 50){
// Arrêt du mouvement
chassis_emission.status = MOUVEMENT_INTERRUPTION;
send_Chassis(&chassis_emission);
}
}else if(distance_x_mm < 0){
if(detect_adv_reception.distance_cm[6] < 50){
// Arrêt du mouvement
chassis_emission.status = MOUVEMENT_INTERRUPTION;
send_Chassis(&chassis_emission);
}
}
}
@ -964,38 +850,6 @@ 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(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;
Actionneur_send(&com_actionneur_env);
do{
Actionneur_scan(&com_actionneur_rec);
}while(com_actionneur_rec.action_terminee != com_actionneur_env.demande_action);
// On réinitialise la machine à état
com_actionneur_env.demande_action = 0;
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;

20
Cerveau/Com_actionneur.h
View File

@ -1,20 +0,0 @@
#ifndef COM_ACTIONNEUR_H
#define COM_ACTIONNEUR_H
#define I2C_SLAVE_actionneur 0x20
#define ACTIONNEUR_NO_ACTION 0
#define ACTIONNEUR_POS_INITIAL 1
#define ACTIONNEUR_DEPLACEMENT 2
#define ACTIONNEUR_PREPARE_PRISE_PLANCHE 3
#define ACTIONNEUR_PRISE 4
#define ACTIONNEUR_EMPILE 5
#define ACTIONNEUR_DEPILE 6
#define ACTIONNEUR_PRISE_EXTERNE 7
#define ACTIONNEUR_FOURCHE_TRANSPORT 8
struct com_actionneur_t{
char demande_action, action_terminee;
};
#endif //COM_ACTIONNEUR_H

40
Cerveau/Com_actionneur.ino
View File

@ -1,40 +0,0 @@
#include "Com_actionneur.h"
#include <Arduino.h>
#include <HardwareSerial.h>
int Actionneur_scan(struct com_actionneur_t * com_actionneur){
return Actionneur_scan(com_actionneur, true);
}
/// @brief Commande l'actionneur
int Actionneur_scan(struct com_actionneur_t * com_actionneur, bool continuous_try){
unsigned char tampon[4];
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
while(error !=0 && continuous_try){
affiche_erreur("Actionneur_scan", "Erreur I2C");
error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
}
Serial.printf(">r0:%d\n>r1:%d\n", tampon[0], tampon[1]);
com_actionneur->action_terminee = tampon[1];
com_actionneur->demande_action = tampon[0];
return error;
}
int Actionneur_send(struct com_actionneur_t * com_actionneur){
unsigned char tampon[1];
tampon[0] = com_actionneur->demande_action;
error = I2C_ecrire_registre(I2C_SLAVE_actionneur, 0, tampon, 1);
if (error !=0){
affiche_erreur("Actionneur_send", "Erreur I2C");
while(1);
}
return error;
}

23
Cerveau/Com_chassis.ino
View File

@ -8,23 +8,23 @@ int Scan_chassis(struct chassis_reception_t * chassis_reception){
}
/// @brief Lit l'état du chassis en I2C
int Scan_chassis(struct chassis_reception_t * chassis_reception, bool continuous_try){
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);
while (error !=0 && continuous_try){
if (error !=0){
Err_Chassi_com =1;IndexErr = 2;
affiche_erreur("Scan_Chassi", "Erreur I2C");
error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
}
if(error == 0){
if(blocking){
while(1);
}
}else{
Err_Chassi_com =0;
IndexErr = 0;
Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
chassis_reception->status = tampon2[0];
}
return error;
}
@ -35,9 +35,9 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
// Conversion des mm ou radian en pas
nb_pas_x = chassis_emission->translation_x_mm * -4.049;
nb_pas_x = chassis_emission->translation_x_mm * 4.049;
nb_pas_y = chassis_emission->translation_y_mm * 4.953;
nb_pas_rot = chassis_emission->rotation_z_rad * -791.;
nb_pas_rot = chassis_emission->rotation_z_rad * 791.;
Mot[0] = chassis_emission->status;
//y*=-1;
@ -55,11 +55,8 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
Mot[10] = chassis_emission->acceleration;
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
while (error !=0){
error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
Err_Cha_send =1; IndexErr = 5;
}
Err_Cha_send =0;IndexErr = 0;
if (error !=0){Err_Cha_send =1; IndexErr = 5;}
else{Err_Cha_send =0;IndexErr = 0;}
if(error==0){ //si pas d'erreur de transmission alors RAZ des valeurs
nbr_essai ++;
cmd_chassi_x = 0;

11
Cerveau/Com_detection_adversaire.ino
View File

@ -6,14 +6,17 @@ 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
int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool continuous_try){
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);
while (error !=0 && continuous_try){
if (error !=0){
affiche_erreur("Scan_Detect_adversaire", "Erreur I2C");
error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
if(blocking){
while(1);
}
}else{
Serial.println("I2C OK");
}
return error;
}

2
Cerveau/Com_gradins.h
View File

@ -9,6 +9,4 @@ struct detect_gradin_t{
float angle_rad;
};
int lire_tirette(void);
#endif

19
Cerveau/Com_gradins.ino
View File

@ -30,22 +30,3 @@ int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){
}
return error;
}
int lire_tirette(char * tirette){
return lire_tirette(tirette, true);
}
int lire_tirette(char * tirette, bool continuous_try){
unsigned char tampon;
//(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
while(error !=0 && continuous_try){
affiche_erreur("lire_tirette", "Erreur I2C");
error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
}
*tirette = tampon;
Serial.printf(">tirette:%d", *tirette);
return error;
}

2
Cerveau/I2C_Master_lib.ino
View File

@ -26,7 +26,7 @@ uint8_t I2C_lire_registre(int adresse_i2c, uint adresse_registre, unsigned char
WIRE.requestFrom(adresse_i2c, taille_donnees + 1);
// ceci contient l'adresse
if(adresse_i2c == I2C_SLAVE_chassi || adresse_i2c == I2C_SLAVE_actionneur){
if(adresse_i2c == I2C_SLAVE_chassi){
Wire.readBytes(&temp, 1);
}
// Et ceci les données

10
Cerveau/ServerWeb.ino
View File

@ -1,7 +1,6 @@
#include <Arduino.h>
#include <WebServer.h>
//#include "Chassis.h"
//#include "ServerWeb.h"
@ -102,26 +101,23 @@ void handleForm() {
}
void showStatus() {
char chassis[50], gradin[50], triangulation[50], detection_adversaire[50], actionneur[50];
char chassis[50], gradin[50], triangulation[50], detection_adversaire[50];
char message_statu[500];
const char message_statu_tmplt[] = "<!DOCTYPE html><html><head><meta charset=\"UTF-8\"><title>A simple form</title></head>\
<body>\n<h1>Statu du robot</h1>\n\
<p>Chassis: %s</p><p>Gradin: %s</p><p>Triangulation: %s</p><p>Detection_adversaire: %s</p><p>Actionneur (0x20): %s</p>";
<p>Chassis: %s</p><p>Gradin: %s</p><p>Triangulation: %s</p><p>Detection_adversaire: %s</p>";
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, actionneur);
sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire);
server.send(200, "text/html", message_statu);
}

19
Chassis/Chassis.ino
View File

@ -232,7 +232,7 @@ void Mvmt(){
// Serial.print("X Y Z A !");Serial.print("\t\t");Serial.print("\tinter");Serial.println("\tModif");
// Serial.print(stepperX.distanceToGo());Serial.print("/");Serial.print(stepperY.distanceToGo());Serial.print("/");Serial.print(stepperZ.distanceToGo());Serial.print("/");Serial.print(stepperA.distanceToGo());Serial.print("\t");Serial.print(Interrupt);Serial.print("\t");Serial.println(Modif_Mvt);
#endif
while((stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0) && (I2C_memory[0] != 1)){
while((stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0) /*&& (Interrupt != 1 || Modif_Mvt != 1)*/){
stepperX.run(); // X
stepperY.run(); // Y
stepperZ.run(); // Z
@ -244,23 +244,6 @@ void Mvmt(){
// delay(100);
#endif
}
// Bien bloquer le robot en cas d'interruption
if(I2C_memory[0] == 1){
stepperX.setAcceleration(40000);
stepperY.setAcceleration(40000);
stepperZ.setAcceleration(40000);
stepperA.setAcceleration(40000);
stepperX.stop(); // X
stepperY.stop(); // Y
stepperZ.stop(); // Z
stepperA.stop(); // A
while(stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0){
stepperX.run(); // X
stepperY.run(); // Y
stepperZ.run(); // Z
stepperA.run(); // A
}
}
}
// if(Modif_Mvt == 1){
// Position_Calculation();

BIN
Doc/Communication I2C.odt
View File

Binary file not shown.