Com I2C ok, applicatif à finir
This commit is contained in:
parent
75ea97fdd5
commit
327deb6283
@ -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,
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#include <Arduino.h>
|
||||
#include <WebServer.h>
|
||||
|
||||
|
||||
//#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[] = "<!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>Chassis: %s</p><p>Gradin: %s</p><p>Triangulation: %s</p><p>Detection_adversaire: %s</p><p>Actionneur (0x20): %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);
|
||||
|
||||
sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire, actionneur);
|
||||
server.send(200, "text/html", message_statu);
|
||||
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user