Ros with esp32
unknown
c_cpp
2 years ago
19 kB
4
Indexable
#include <ros.h>
#include <geometry_msgs/Twist.h>
// Pour afficher des messages à la console selon le contexte à tester
#define DEBUGMOT false
#define DEBUGMOUV false
#define DEBUGACCEL false
// CABLAGE DU MOTEUR 1 sur l'ESP
#define Step1 19 // signal de micropas du moteur 1 à chaque front montant
#define Dir1 18 // sens de rotation du moteur HAUT : trigo BAS : horaire
#define Step2 5
#define Dir2 17
#define Step3 16
#define Dir3 4
#define Step4 2
#define Dir4 15
#define ENABLE 13 // mise en service de l'ensemble des moteurs pas à pas
#define Temoin 14 // indiquera que l'on a recu une trame ros
//--- PARTIE MOTEURS
struct Pap // définition de la structure d'un moteur pas à pas
{
uint8_t StepPin; // numéro de sa broche Step
uint8_t DirPin; // numéro de sa broche Dir
uint16_t TPas; // multiple de THPas durée entre deux fronts sur Step
uint8_t NumHPas; // numéro de la période THPas dans cette durée de Step
uint16_t NumPas; // numéro du pas Step en cours
uint16_t nbPas; // nombre de micropas attendus pour ce mouvement (signé au départ)
bool sens; // sens de la rotation (vrai si nbPas < 0)
bool tourne; // indique que ce moteur est actionné dans ce mouvement pas fini
};
volatile Pap Moteur1, Moteur2, Moteur3, Moteur4;
//--- ASPECTS TEMPORELS
hw_timer_t * timer = NULL;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
#define THPasDep 200 // nombre de us entre deux interruptions timer : 2IT
#define TpasDep 20 // multiple de THPas assurant le démarrage 4ms
#define THPasMin 50 // nombre de us de THPas pour vitesse la plus rapide 500us pour 1/4 pas
/* Le moteur le plus rapide d'un mouvement démarrera donc à un step de 10 ms.
Les autres démarre plus lentement en fonction du rapport des déplacements en dx et dy
L'accel/deccelération consistera à faire évoluer THPas entre 200 et 50 = THPasMin
pour un rythme de pas le plus rapide possible en fin accélération de 1ms
Avec cette valeur, 1/4 pas = tour/800 en 1ms --> 1tr = 0.8s --> 48 tr/mn = 54cm/s */
#define DeltaRythme 30 // nombre de ms que dure un pas d'accel/decceleration
uint16_t THPas; // valeur du rythme des IT Timer en us
float TPas; // valeur flottante pour calcul accel/deccelerations
volatile long nbIt; // nombre d'interruptions ayant eu lieu
unsigned long int depart; // moment du changement de rythme et d'affichage moteurs
uint8_t nbPasAcc; // nombre de coup d'accélération -> deccélèration de DeltaRytme
uint8_t nbPas; // on compte les périodes DeltaRythme d'accélération
bool Accel, Deccel; // indique chacun des sens de l'évolution de la vitesse faux si constante
uint16_t TempsMvt; // nombre de ms que devait durer le mouvement
//--- ASPECTS MECANIQUES
// #define PI 3.1415926535 // inutile ici car PI est défini dans le C++
#define diamRoue 82 // roue de 82mm
#define nbPasTour 200 // nombre de pas complet par tour de roue
#define microPas 4 // on travaille en 1/4 de pas
#define impParMm ((microPas * nbPasTour) / (PI * diamRoue)) // (4 * 200) / (82 * 3.1416) = 3.11
#define rayonRobot 125 // en mm
#define tourRobot (2*PI*rayonRobot)
#define impParDeciDegre (impParMm*tourRobot/3600) // 0.65 imp par déci degré
#define impParmRad (impParMm*tourRobot/(2000*PI)) // 0.37 imp par unité traduite de ROS
//--- ASPECT DEPLACEMENTS
int dx, dy, dc; // quantité de déplacement de moteurs 3 et 4 (Y), et 1 et 2 (X) en impulsions
int dcap; // changement de cap attendu, plus tard, on établiera l'unité en milli radians (mrd)
bool dirx, diry; // sens du déplacement --> dir des paires moteurs tant que l'on ne tourne pas sur soi
bool rot; // indique que l'on tourne sur soi-même (détecté par dx = dy = 0 et dcap != 0)
bool roule; // indique qu'un mouvement est en cours
uint8_t numMvt; // numéro du mouvement en cours dans le tableau
uint16_t stepX, stepY; // compteurs des IT entre chaque step : va de 0 à THPasX ou THPasY
uint16_t PasDeccel; // numéro du pas du moteur le plus actif pour décider de deccélérer
uint8_t MotPlusActif; // numéro du moteur le plus actif (celui qui se déplace le plus)
volatile int count; // declencheur de compteur
volatile int nbPas1, nbPas2, nbPas3, nbPas4; // odométrie des déplacements
//--- ASPECT MOUVEMENTS
struct Mvt // définition de mouvements,
{ // on suppose ici que si dx ou dy, pas de dc et si dc, pas de dx ou dy
int dx; // nombre de mm à faire sur les moteurs 'X' du robot
int dy; // nombre de mm à faire sur les moteurs 'Y' du robot
int dcap; // angle de rotation du robot sur lui-même en déci-degrés
};
Mvt mvt;
bool nouveau;
//--- ASPECT ROS
ros::NodeHandle nh;
volatile bool recuROS;
volatile int intDX;
volatile int intDY;
volatile int intDZ;
/************* FONCTION D'INTERRUPTION TEMPORELLE ******************/
// Code avec section critique en RAM -> + rapide actuellement dure 1us !!!
void IRAM_ATTR ITPas()
{
portENTER_CRITICAL_ISR(&timerMux);
// digitalWrite (Temoin, HIGH); // pour mesurer le temps de l'interruption
// digitalWrite (Temoin, !digitalRead(Temoin)); // pour mesurer la période des IT
if (Moteur1.tourne) // on ne s'occupe que de ceux qui tournent
{
Moteur1.NumHPas--; // comptage de cette IT dans la durée du pas
if (Moteur1.NumHPas == 0) // si on est arrivé à 0 : micro-pas suivant ?
{
if (digitalRead(Moteur1.StepPin) == LOW) // il va y avoir un PAS à compter
{
Moteur1.NumPas++; // on compte ce pas
Moteur1.NumHPas = Moteur1.TPas << 1; // on relance la tempo
}
// basculement de STEP : il faut en faire 2 pour faire un micro pas
digitalWrite (Moteur1.StepPin, ! digitalRead(Moteur1.StepPin));
}
}
if (Moteur2.tourne)
{
Moteur2.NumHPas--;
if (Moteur2.NumHPas == 0)
{
if (digitalRead(Moteur2.StepPin) == LOW)
{
Moteur2.NumPas++;
Moteur2.NumHPas = Moteur2.TPas << 1;
}
digitalWrite (Moteur2.StepPin, ! digitalRead(Moteur2.StepPin));
}
}
if (Moteur3.tourne)
{
Moteur3.NumHPas--;
if (Moteur3.NumHPas == 0)
{
if (digitalRead(Moteur3.StepPin) == LOW)
{
Moteur3.NumPas++;
Moteur3.NumHPas = Moteur3.TPas;
}
digitalWrite (Moteur3.StepPin, ! digitalRead(Moteur3.StepPin));
}
}
if (Moteur4.tourne)
{
Moteur4.NumHPas--;
if (Moteur4.NumHPas == 0)
{
if (digitalRead(Moteur4.StepPin) == LOW)
{
Moteur4.NumPas++;
Moteur4.NumHPas = Moteur4.TPas;
}
digitalWrite (Moteur4.StepPin, ! digitalRead(Moteur4.StepPin));
}
}
// digitalWrite (Temoin, LOW); // pour mesurer le temps de l'interruption
portEXIT_CRITICAL_ISR(&timerMux);
}
/******************** FONCTIONS MOTEURS *******************/
void creationTableauMoteurs()
{
// vérifier ici le bon cablage des moteurs x1, x2, y1 et y2
Moteur1.StepPin = Step1;
Moteur1.DirPin = Dir1;
Moteur2.StepPin = Step2;
Moteur2.DirPin = Dir2;
Moteur3.StepPin = Step3;
Moteur3.DirPin = Dir3;
Moteur4.StepPin = Step4;
Moteur4.DirPin = Dir4;
}
void initSignaux ()
{
// on doit fixer les sens des signaux STEP et DIR et mettre au repos STEP
pinMode (Moteur1.StepPin, OUTPUT);
pinMode (Moteur1.DirPin, OUTPUT);
digitalWrite (Moteur1.StepPin, LOW);
pinMode (Moteur2.StepPin, OUTPUT);
pinMode (Moteur2.DirPin, OUTPUT);
digitalWrite (Moteur2.StepPin, LOW);
pinMode (Moteur3.StepPin, OUTPUT);
pinMode (Moteur3.DirPin, OUTPUT);
digitalWrite (Moteur3.StepPin, LOW);
pinMode (Moteur4.StepPin, OUTPUT);
pinMode (Moteur4.DirPin, OUTPUT);
digitalWrite (Moteur4.StepPin, LOW);
pinMode(ENABLE, OUTPUT);
digitalWrite(ENABLE, LOW);
pinMode(Temoin, OUTPUT);
digitalWrite(Temoin, LOW);
}
void cdeMoteurs ()
{
if (recuROS)
{
recuROS = false;
// on lit et on stocke les nouvelle consignes, on dit qu'il y a à faire
if (mvt.dx != intDX)
{
mvt.dx = intDX;
nouveau = true;
}
if (mvt.dy != intDY)
{
mvt.dy = intDY;
nouveau = true;
}
if (mvt.dcap != intDZ)
{
mvt.dcap = intDZ;
nouveau = true;
}
digitalWrite(Temoin, !digitalRead(Temoin)); // marquer qu'on a recu
}
}
/****************** Fonctions MOUVEMENT ***************************/
void initMvt()
{
THPas = THPasDep; // on commence avec un rythme de step assurant le démarrage
// Configure le Prescaler a 80 le quart de l ESP32 est cadence a 80Mhz
// 80000000 / 80 = 1000000 tics / seconde
timer = timerBegin(0, 80, true);
timerAttachInterrupt(timer, &ITPas, true);
// Regle le declenchement d'une interruption chaque 20ms
timerAlarmWrite(timer, THPas, true);
timerAlarmEnable(timer);
}
void sens()
{
// prise en compte des signes de déplacement pour le sens
if (dx < 0)
{
Moteur1.sens = true; // true pour un sens rétrograde, trigonométrique
Moteur3.sens = false; // moteur antagoniste, sens complémentaire
dx = - dx; // remise en positif du nombre d'impulsions
}
else
{
Moteur1.sens = false; // false pour le sens horaire
Moteur3.sens = true; // moteur antagoniste, sens complémentaire
}
if (dy < 0)
{
Moteur2.sens = true;
Moteur4.sens = false;
dy = - dy;
}
else
{
Moteur2.sens = false;
Moteur4.sens = true;
}
if (dc) // dc exclu et est exclu d'avec dx et/ou dy
{
Moteur1.sens = (dc > 0); // true pour un sens rétrograde, trigonométrique
Moteur2.sens = (dc > 0); // false pour un sens rétrograde : horaire
Moteur3.sens = (dc > 0);
Moteur4.sens = (dc > 0);
}
// et on pose les signaux
digitalWrite (Moteur1.DirPin, Moteur1.sens);
digitalWrite (Moteur2.DirPin, Moteur2.sens);
digitalWrite (Moteur3.DirPin, Moteur3.sens);
digitalWrite (Moteur4.DirPin, Moteur4.sens);
}
void rythmes()
{
uint16_t THPasX, THPasY; // rythme des impulsions sur les moteurs en X et en Y en THPas
// on commence par aucun pas pour les moteurs et tous arretés
Moteur1.NumHPas = Moteur1.TPas = 0;
Moteur2.NumHPas = Moteur2.TPas = 0;
Moteur3.NumHPas = Moteur3.TPas = 0;
Moteur4.NumHPas = Moteur4.TPas = 0;
Moteur1.tourne = false;
Moteur2.tourne = false;
Moteur3.tourne = false;
Moteur4.tourne = false;
if ((dx != 0) && (dy != 0)) // si les deux axes sont non nuls
{ // il faut lire le plus grand entre dx et dy
// donner THPasDep au plus grand parcours
// et calculer le TPas de l'autre en rapport des dx/dy
if (dx > dy)
{
THPasX = TpasDep; // le plus rapide forcé de démarrer
float corr = dx / dy; // tjs > 1
THPasY = int (TpasDep * corr); // autre moteur plus lent
MotPlusActif = 1;
}
if (dy > dx)
{
THPasY = TpasDep; // le plus rapide forcé de démarrer
float corr = dy / dx; // tjs > 1
THPasX = int (TpasDep * corr); // autre moteur plus lent
MotPlusActif = 2;
}
if (dx == dy)
{
THPasX = THPasY = TpasDep;
MotPlusActif = 1;
}
Moteur1.TPas = THPasX;
Moteur3.TPas = THPasX;
Moteur2.TPas = THPasY;
Moteur4.TPas = THPasY;
// on indique qu'ils ont à faire tous les 4
Moteur1.tourne = true;
Moteur2.tourne = true;
Moteur3.tourne = true;
Moteur4.tourne = true;
// et on relance les tempos de chaque moteur au double du nombre de pas
Moteur1.NumHPas = Moteur1.TPas << 1;
Moteur2.NumHPas = Moteur2.TPas << 1;
Moteur3.NumHPas = Moteur2.TPas << 1;
Moteur4.NumHPas = Moteur4.TPas << 1;
roule = true;
}
if ((dx == 0) || (dy == 0)) // si l'un des deux est nul
{
if (dx)
{
Moteur1.TPas = TpasDep;
Moteur3.TPas = TpasDep;
Moteur1.tourne = true;
Moteur3.tourne = true;
}
if (dy)
{
Moteur2.TPas = TpasDep;
Moteur4.TPas = TpasDep;
Moteur2.tourne = true;
Moteur4.tourne = true;
}
roule = true;
// et on relance les tempos de chaque moteur qui doit tourner
if (Moteur1.tourne)
Moteur1.NumHPas = Moteur1.TPas << 1; // 2 périodes par micropas
if (Moteur2.tourne)
Moteur2.NumHPas = Moteur2.TPas << 1;
if (Moteur3.tourne)
Moteur3.NumHPas = Moteur3.TPas << 1;
if (Moteur4.tourne)
Moteur4.NumHPas = Moteur4.TPas << 1;
}
// et si on tourne sur soi-même
if (dc)
{
THPasX = THPasY = TpasDep; // tous les moteurs au même rythme
MotPlusActif = 1; // on prend celui-l comme référence
Moteur1.TPas = TpasDep; // tous les moteur au rythme de départ
Moteur2.TPas = TpasDep;
Moteur3.TPas = TpasDep;
Moteur4.TPas = TpasDep;
Moteur1.tourne = true;
Moteur2.tourne = true;
Moteur3.tourne = true;
Moteur4.tourne = true;
roule = true;
// et on relance les tempos de chaque moteur qui doit tourner
if (Moteur1.tourne)
Moteur1.NumHPas = Moteur1.TPas << 1;
if (Moteur2.tourne)
Moteur2.NumHPas = Moteur2.TPas << 1;
if (Moteur3.tourne)
Moteur3.NumHPas = Moteur3.TPas << 1;
if (Moteur4.tourne)
Moteur4.NumHPas = Moteur4.TPas << 1;
}
TPas = THPasDep;
digitalWrite (ENABLE, HIGH); // on met en service les moteurs PaP
if ((dx == 0) && (dy == 0) && (dc == 0)) // si les trois sont nuls
roule = false; // on ira chercher le mouvement suivant ...
}
void arret()
{
// si on a plus de moteur qui tourne
if (Moteur1.tourne || Moteur2.tourne || Moteur3.tourne || Moteur4.tourne);
else
{
timerAlarmDisable(timer); // arrêt des interruptions
roule = false; // et on l'indique
if (DEBUGMOUV)
Serial.println (" C'est fini pour ce mouvement");
digitalWrite(ENABLE, LOW); // on libère les moteurs
}
}
void mvtSuivant()
{
dx = int (mvt.dx * impParMm); // dx est traduit en impulsions
dy = int (mvt.dy * impParMm); // dy est traduit en impulsions
dc = int (mvt.dcap * impParmRad); // dc traduit en impulsions
sens(); // définition des sens et pose des signaux dx et dy devenus > 0
// init des nombres de step des moteurs et du fait qu'il doit tourner
if (dx)
{
Moteur1.nbPas = dx;
Moteur1.tourne = true;
Moteur3.nbPas = dx;
Moteur3.tourne = true;
}
if (dy)
{
Moteur2.nbPas = dy;
Moteur2.tourne = true;
Moteur4.nbPas = dy;
Moteur4.tourne = true;
}
if (dc)
{
Moteur1.nbPas = dc;
Moteur2.nbPas = dc;
Moteur3.nbPas = dc;
Moteur4.nbPas = dc;
Moteur1.tourne = true;
Moteur2.tourne = true;
Moteur3.tourne = true;
Moteur4.tourne = true;
}
// et init des numeros de pas des moteurs
Moteur1.NumPas = 0;
Moteur2.NumPas = 0;
Moteur3.NumPas = 0;
Moteur4.NumPas = 0;
rythmes(); // calcul des pas sur chaque moteur qui doit tourner
nbPas = 0; // on a encore fait aucun pas
//Accel = true; Deccel = false; // on passe en accélération en début de mouvement
//if (dc) Accel = false; // pas d'accélération en rotation sur soi-même
// on programme les interruptions timer correspondantes
initMvt();
roule = true;
}
void gereAvance()
{
if (Moteur1.tourne) // celui-ci doit-il tourner ?
{
if (Moteur1.NumPas >= Moteur1.nbPas) // si les pas sont fini pour ce mouvement
Moteur1.tourne = false; // on signal qu'il ne tourne plus
}
if (Moteur2.tourne)
{
if (Moteur2.NumPas >= Moteur2.nbPas)
Moteur2.tourne = false;
}
if (Moteur3.tourne)
{
if (Moteur3.NumPas >= Moteur3.nbPas)
Moteur3.tourne = false;
}
if (Moteur4.tourne)
{
if (Moteur4.NumPas >= Moteur4.nbPas)
Moteur4.tourne = false;
}
}
/****************** FONCTIONS ROS *******************************/
void twistMessageReceived(const geometry_msgs::Twist& twist)
{
// Récupérer les valeurs linéaires et angulaires en mm et mrad
float DX = 1000 * twist.linear.x;
float DY = 1000 * twist.linear.y;
float DZ = 1000 * twist.angular.z;
// Transformer les valeurs en entier
intDX = (int)DX; // information entière en mm
intDY = (int)DY; // information en mm
intDZ = (int)DZ; // information en milli radian
// Utiliser les valeurs de intDX, intDY et intDZ pour contrôler les moteurs, etc.
recuROS = true;
}
ros::Subscriber<geometry_msgs::Twist> twist_sub("cmd_vel", twistMessageReceived);
// Initialiser ROS
void initROS()
{
nh.initNode();
nh.subscribe(twist_sub);
}
/**************** PROGRAMME PROPREMENT DIT ***********************/
void setup()
{
initROS(); // ouverture du port série
creationTableauMoteurs();
initSignaux();
}
void loop()
{
nh.spinOnce();
if (recuROS)
cdeMoteurs();
if (nouveau)
mvtSuivant();
if (roule)
{
gereAvance();
arret();
}
delay (3);
}Editor is loading...