265 lines
8.5 KiB
C++
265 lines
8.5 KiB
C++
// Programme Chassi Robot Riom Robotique
|
|
// Version : 5=6
|
|
// Etat : (En cours de dev / à tester / Fonctionnel)
|
|
//
|
|
// à tester
|
|
// en cours de test i2c, 02/03/2024 je recoit bien la trame i2c avec les donées dans le bon sens
|
|
//
|
|
//*********************************************************************
|
|
//******************** Integration et config WIFI *********************
|
|
//*********************************************************************
|
|
// serveur - depuis machine reseau http://192.XXX.XXX.XX?variable extrait la variable
|
|
//#include <M5Core2.h>
|
|
#include "Wire.h"
|
|
#define I2C_DEV_ADDR 0x55
|
|
|
|
//Definition des Variantes :
|
|
#define serialP;
|
|
#define gst_I2C;
|
|
|
|
volatile byte * I2C_memory;
|
|
|
|
//*********************************************************************
|
|
//******************** Integration et config Moteur *******************
|
|
//*********************************************************************
|
|
#include <AccelStepper.h>
|
|
#define pul1 D10
|
|
#define dir1 D9
|
|
#define pul2 D2
|
|
#define dir2 D1
|
|
#define pul3 D8
|
|
#define dir3 D7
|
|
#define pul4 D6
|
|
#define dir4 D3
|
|
|
|
// AccelStepper stepperX(1.8, dir1, pul1); // X
|
|
// AccelStepper stepperY(1.8, dir2, pul2); // Y
|
|
// AccelStepper stepperZ(1.8, dir3, pul3); // Z
|
|
// AccelStepper stepperA(1.8, dir4, pul4); // A
|
|
AccelStepper stepperX(1.8, pul1, dir1); // X
|
|
AccelStepper stepperY(1.8, pul2, dir2); // Y
|
|
AccelStepper stepperZ(1.8, pul3, dir3); // Z
|
|
AccelStepper stepperA(1.8, pul4, dir4); // A
|
|
|
|
int Men= D0; //Motor All enable pin
|
|
|
|
// Data pour la lecture de donnée
|
|
String nom = "Arduino";
|
|
String msg;
|
|
int inChar;
|
|
int data[] = {0, 0};
|
|
int com[] = {0, 0, 0};
|
|
|
|
// -------------- Direction des moteurs ( pour un sens de marche commun )
|
|
int dirX= +1;
|
|
int dirY= +1;
|
|
int dirZ= +1;
|
|
int dirA= +1;
|
|
|
|
int cli_led=0; // initial value of input
|
|
int Move = 0;
|
|
String rc;
|
|
|
|
int16_t Target[] = {0,0,0,0,0}; //Tableau de la commande de position (X,Y,Rotation,speed,accel)
|
|
int16_t AxeX[] = {0,0,0,0}; // distance en X pour chaque moteur
|
|
int16_t AxeY[] = {0,0,0,0}; // distance en Y pour chaque moteur
|
|
int16_t AxeRot[] = {0,0,0,0}; // distance en R pour chaque moteur
|
|
int16_t Vecteur[] = {0,0,0,0};// distance en X pour chaque moteur
|
|
|
|
int PosX = 0; //Position X du robot
|
|
int PosY = 0; //Position Y du robot
|
|
int Rot = 0; //Rotation du robot
|
|
int vitesse = 2000;
|
|
int accel = 2000;
|
|
|
|
|
|
uint8_t Mot[] = {0,0,0,0,0,0,0,0,0,0,0,0} ; //11*8bits Mot de commande I2C du Chassi ****Cmd, X, Y, Rot, Vit, Acc****
|
|
uint8_t c[] = {0,0,0,0,0,0,0,0,0,0,0} ;
|
|
bool Interrupt = 0; // commande d'intéruption de mouvement (Arret d'Urgence) premier bit du mot de Cmd
|
|
bool Modif_Mvt = 0; // commande de modification de mouvement deuxieme bit du mot de Cmd
|
|
bool fin_mvt = 0;
|
|
int h=0;
|
|
|
|
|
|
void setup()
|
|
{
|
|
pinMode(Men,OUTPUT);
|
|
stop();
|
|
#ifdef serialP
|
|
Serial.begin(115200);
|
|
#endif
|
|
|
|
// X
|
|
stepperX.setMaxSpeed(vitesse); stepperX.setAcceleration(accel);
|
|
// Y
|
|
stepperY.setMaxSpeed(vitesse); stepperY.setAcceleration(accel);
|
|
// Z
|
|
stepperZ.setMaxSpeed(vitesse); stepperZ.setAcceleration(accel);
|
|
// A
|
|
stepperA.setMaxSpeed(vitesse); stepperA.setAcceleration(accel);
|
|
|
|
|
|
//Serial.println(Men);
|
|
I2C_Slave_init(0x55);
|
|
I2C_memory = get_i2c_data();
|
|
|
|
|
|
//digitalWrite(Men, LOW);// Low Level Enable
|
|
pinMode(pul1, OUTPUT);
|
|
pinMode(pul2, OUTPUT);
|
|
pinMode(pul3, OUTPUT);
|
|
pinMode(pul4, OUTPUT);
|
|
pinMode(dir1, OUTPUT);
|
|
pinMode(dir2, OUTPUT);
|
|
pinMode(dir3, OUTPUT);
|
|
pinMode(dir4, OUTPUT);
|
|
|
|
}
|
|
|
|
|
|
void loop()
|
|
{
|
|
static char wait = 0;
|
|
static int64_t time;
|
|
|
|
for(int i=0; i<8; i++){
|
|
Serial.printf("0x%x ", I2C_memory[i]);
|
|
}
|
|
Serial.printf("\n");
|
|
|
|
|
|
if(((I2C_memory[0] & 0x02)==2) && Modif_Mvt==0){
|
|
Modif_Mvt = 1;
|
|
}
|
|
if(Modif_Mvt == 1){
|
|
//Mot = I2C_memory;
|
|
Target[0]= (I2C_memory[1] << 8) | I2C_memory[2];
|
|
Target[1]= (I2C_memory[3] << 8) | I2C_memory[4];
|
|
Target[2]= (I2C_memory[5] << 8) | I2C_memory[6];
|
|
Target[3]= (I2C_memory[7] << 8) | I2C_memory[8];
|
|
Target[4]= (I2C_memory[9] << 8) | I2C_memory[10];
|
|
for(int i=1; i<9; i++){
|
|
I2C_envoi_8bits(0,i);
|
|
}
|
|
Position_Calculation(); //************* Calcul du Vecteur XY (incrémental)
|
|
Mvmt(); //************* Envoie de la position à ateindre pour chaque moteur + Execution du mouvement
|
|
stop(); // Servo Off
|
|
|
|
I2C_memory[0] = 0x04;
|
|
I2C_envoi_8bits(I2C_memory[0] ,0);
|
|
}
|
|
|
|
|
|
// else{
|
|
// I2C_memory = get_i2c_data();
|
|
// I2C_envoi_8bits(I2C_memory[0] | 1<<2,0);
|
|
// I2C_envoi_8bits(I2C_memory[0] | 1<<1,0);
|
|
// for(int i=1; i<9; i++){
|
|
// I2C_envoi_8bits(0,i);
|
|
|
|
if((I2C_memory[0] & 0x02)==0 && Modif_Mvt==1){
|
|
Modif_Mvt = 0;
|
|
}
|
|
|
|
//Dé-asservissement des moteurs
|
|
|
|
|
|
|
|
}
|
|
|
|
void Position_Calculation(){
|
|
PosX = Target[0];
|
|
AxeX[0] = {PosX}; //x
|
|
AxeX[1] = {PosX}; //y
|
|
AxeX[2] = {-PosX}; //z
|
|
AxeX[3] = {-PosX}; //a
|
|
PosY = Target[1];
|
|
AxeY[0] = {PosY}; //x
|
|
AxeY[1] = {PosY}; //y
|
|
AxeY[2] = {PosY}; //z
|
|
AxeY[3] = {PosY}; //a
|
|
Rot = Target[2];
|
|
AxeRot[0] = {-Rot}; //x
|
|
AxeRot[1] = {Rot}; //y
|
|
AxeRot[2] = {Rot}; //z
|
|
AxeRot[3] = {-Rot}; //a
|
|
|
|
vitesse = Target[3];
|
|
accel = Target[4];
|
|
|
|
for (int i=0; i<4; i++){
|
|
Vecteur[i] = (AxeX[i] + AxeY[i] + AxeRot[i])/2;
|
|
Serial.printf("Vecteur[%d]: %d\n", i, Vecteur[i]);
|
|
}
|
|
|
|
stepperX.moveTo(stepperX.currentPosition() + Vecteur[0]*dirX);
|
|
stepperY.moveTo(stepperY.currentPosition() + Vecteur[1]*dirY);
|
|
stepperZ.moveTo(stepperZ.currentPosition() + Vecteur[2]*dirZ);
|
|
stepperA.moveTo(stepperA.currentPosition() + Vecteur[3]*dirA);
|
|
|
|
stepperX.setMaxSpeed(vitesse); stepperX.setAcceleration(accel); // X
|
|
stepperY.setMaxSpeed(vitesse); stepperY.setAcceleration(accel); // Y
|
|
stepperZ.setMaxSpeed(vitesse); stepperZ.setAcceleration(accel); // Z
|
|
stepperA.setMaxSpeed(vitesse); stepperA.setAcceleration(accel); // A
|
|
|
|
|
|
#ifdef serialP
|
|
// Serial.print("Target 0 ");Serial.print("\tTarget 1");;Serial.print("\tTarget 2");;Serial.print("\tTarget 3");Serial.println("\tTarget 4");
|
|
// Serial.print(Target[0]);Serial.print("\t\t");Serial.print(Target[1]);Serial.print("\t\t");Serial.print(Target[2]);Serial.print("\t\t");Serial.print(Target[3]);Serial.print("\t\t");Serial.println(Target[4]);
|
|
// Serial.println();
|
|
// Serial.print("X ");Serial.print("\t\tY");;Serial.print("\t\tZ");;Serial.print("\t\tA");Serial.println("\t\tRot");
|
|
// Serial.print(Vecteur[0]);Serial.print("\t\t");Serial.print(Vecteur[1]);Serial.print("\t\t");Serial.print(Vecteur[2]);Serial.print("\t\t");Serial.print(Vecteur[3]);Serial.print("\t\t");Serial.println(Vecteur[4]);
|
|
// delay(100);
|
|
#endif
|
|
for (int i=0; i<4; i++){
|
|
Target[i] = 0;
|
|
}
|
|
// RAZ du flag de mouvement modifié
|
|
Modif_Mvt = 0;
|
|
|
|
for (int i=0; i < sizeof(Mot); i++){
|
|
Mot[i] = 0;
|
|
}
|
|
}
|
|
void Mvmt(){
|
|
int64_t time = esp_timer_get_time();
|
|
|
|
// Mise en Servo ON des moteurs
|
|
digitalWrite(Men,LOW);// Low Level Enable
|
|
if(stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0){
|
|
//Execution du mouvement global (resort de la boucle une fois arrivé)
|
|
#ifdef serialP
|
|
// 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) /*&& (Interrupt != 1 || Modif_Mvt != 1)*/){
|
|
stepperX.run(); // X
|
|
stepperY.run(); // Y
|
|
stepperZ.run(); // Z
|
|
stepperA.run(); // A
|
|
//-------------------
|
|
#ifdef serialP
|
|
// 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);
|
|
// delay(100);
|
|
#endif
|
|
}
|
|
}
|
|
// if(Modif_Mvt == 1){
|
|
// Position_Calculation();
|
|
// Serial.println("\t\tRe Calculation !!!");
|
|
// }
|
|
}
|
|
void stop(){
|
|
if(Interrupt== 1){
|
|
//delay(1000);
|
|
Interrupt = 0;
|
|
}
|
|
#ifdef serialP
|
|
// h++;
|
|
// Serial.print("**********STOP****** ");Serial.println(h);
|
|
#endif
|
|
digitalWrite(Men,HIGH);// Low Level Enable
|
|
delay(500);
|
|
}
|