modelec/odo_STM32
1
0
Fork 0
mirror of https://github.com/modelec/odo_STM32.git synced 2026-01-18 16:27:25 +01:00
Code Issues Packages Projects Releases Wiki Activity

corrections en cours

Browse Source
This commit is contained in:
Maxime
2025-05-22 12:49:51 +02:00
parent 4c5730c4fd
commit 0695b07c25
6 changed files with 93 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Core/Inc/pidPosition.h
View File

@@ -8,8 +8,8 @@ private:
Point erreurPosition, erreurPosition_old;
Point consignePositionFinale;
float d[2]; //valeurs calculé dans les méthodes update (position)
float i[2];
float d[3] = {0.0f, 0.0f, 0.0f}; //valeurs calculé dans les méthodes update (position)
float i[3] = {0.0f, 0.0f, 0.0f};
float Kp_theta, Ki_theta, Kd_theta;
@@ -46,7 +46,7 @@ public:
// Méthodes spécifiques à la position
Point calculErreurPosition(Point p);
void updateErreurPosition(Point pointActuel);
std::array<double, 2> updateNouvelOrdreVitesse(Point pointActuel);
std::array<double, 2> updateNouvelOrdreVitesse(Point pointActuel, float vitGauche, float vitDroit);
// Surcharge des méthodes virtuelles
void updateErreur();

17
Core/Src/modelec.cpp
View File

@@ -50,12 +50,12 @@ bool isDelayPassed(uint32_t delay) {
}
//PID
void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition pid, PidVitesse pidG, PidVitesse pidD){
void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition& pid, PidVitesse& pidG, PidVitesse& pidD, float vitGauche, float vitDroit){
//PidPosition pid(0,0,0,0,0,0,objectifPoint);
pid.setConsignePositionFinale(objectifPoint);
std::array<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel);
std::array<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit);
char debug_msg[128];
sprintf(debug_msg, "[CONS] G: %.3f m/s | D: %.3f m/s\r\n", vitesse[0], vitesse[1]);
@@ -112,7 +112,16 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) {
theta = 0.0f;
//motor.accelerer(300);
*out_pid = new PidPosition(1.5,0.02,0.4,3.0, 0.01, 0.6, Point());
*out_pid = new PidPosition(
0.3, // kp
0.05, // ki
0.5, // kd
0.5, // Kp_theta
0.02, // Ki_theta
0.7, // Kd_theta
Point()
);
//*out_pid = new PidPosition(1.2,0.02,0.8,0, 0, 0, Point());
*out_pidG = new PidVitesse(0.2, 0.05, 0.01, 0);
*out_pidD = new PidVitesse(0.2, 0.05, 0.01, 0);
@@ -206,7 +215,7 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) {
CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg));
determinationCoefPosition(currentPoint,targetPoint, *pidPosition, *pidVitesseG, *pidVitesseD);
determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed()); //TODO vérification inversement target et current
//HAL_Delay(1000);
motor.update();

2
Core/Src/motors.cpp
View File

@@ -164,6 +164,6 @@ void Motor::update() {
(uint32_t)TIM8->CCR1, (uint32_t)TIM8->CCR2,
(uint32_t)TIM1->CCR1, (uint32_t)TIM1->CCR2);
CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
//CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
}

6
Core/Src/pid.cpp
View File

@@ -16,9 +16,9 @@ Pid::Pid(float kp, float ki, float kd){
this->ki = ki;
this->kd = kd;
this->Vmax = 1; //à remplacer par la vraie valeur, calculable à partir de la rpm
this->Wmax = 1; //pareil, à remplacer par la vraie valeur
this->L = 0.3; //à remplacer, à mesurer direct entre les deux roues motrices du robot (attention, valeur à mettre en mètre)
this->Vmax = 0.235;
this->Wmax = 3.11;
this->L = 0.151;
}
// Setters

106
Core/Src/pidPosition.cpp
View File

@@ -1,7 +1,8 @@
#include "pidPosition.h"
#include <cstdio>
#include "usbd_cdc_if.h"
#include "commSTM.h"
PidPosition::PidPosition(float kp, float ki, float kd,float Kp_theta, float Ki_theta, float Kd_theta, Point consignePosition): Pid(kp, ki, kd){
this->Kp_theta = Kp_theta;
@@ -96,61 +97,96 @@ void PidPosition::updateErreurPosition(Point PointActuel) {
this->erreurPosition = calculErreurPosition(PointActuel);
}
// Calcul de l'erreur de position
// Calcul de l'erreur de position
Point PidPosition::calculErreurPosition(Point p) {
return Point(this->consignePositionFinale.getX() - p.getX(),
return Point(
this->consignePositionFinale.getX() - p.getX(),
this->consignePositionFinale.getY() - p.getY(),
this->consignePositionFinale.getTheta() - p.getTheta(),
StatePoint::NONDETERMINE);
atan2(sin(this->consignePositionFinale.getTheta() - p.getTheta()),
cos(this->consignePositionFinale.getTheta() - p.getTheta())),
StatePoint::NONDETERMINE
);
}
// Mise à jour et calcul du nouvel ordre de vitesse pour les moteurs
std::array<double, 2> PidPosition::updateNouvelOrdreVitesse(Point pointActuel) {
/*
La sortie est calculée en combinant les 3 termes :
- proportionnel : on multiplie kp par l'erreur
- integral : on multiplie ki par la somme des erreurs passées
- derivee : on multiplie kd par la variation de l'erreur
*/
std::array<double, 2> PidPosition::updateNouvelOrdreVitesse(Point pointActuel, float vitGauche, float vitDroit) {
char log[128];
this->updateErreurPosition(pointActuel);
sprintf(log, "[DEBUG] Position Actuelle | X: %.3f | Consigne X: %.3f\r\n", pointActuel.getX(), consignePositionFinale.getX());
CDC_Transmit_FS((uint8_t*)log, strlen(log));
// Log de l’erreur de position actuelle
sprintf(log, "[PID] Erreur Position | X: %.3f | Y: %.3f | Theta: %.3f\r\n",
erreurPosition.getX(), erreurPosition.getY(), erreurPosition.getTheta());
CDC_Transmit_FS((uint8_t*)log, strlen(log));
// Mise à jour du terme intégral
this->i[0] += this->erreurPosition.getX();
this->i[1] += this->erreurPosition.getY();
this->i[2] += this->erreurPosition.getTheta();
// Si la commande est saturée, n'accumule pas l'intégrale
float i1 = this->i[0] + this->erreurPosition.getX();
float i2 = this->i[1] + this->erreurPosition.getY();
float i3 = this->i[2] + this->erreurPosition.getTheta();
sprintf(log, "[PID] Terme Intégral | iX: %.3f | iY: %.3f | iTheta: %.3f\r\n", i[0], i[1], i[2]);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
// Calcul du terme dérivé
this->d[0] = this->erreurPosition.getX() - this->erreurPosition_old.getX();
this->d[1] = this->erreurPosition.getY() - this->erreurPosition_old.getY();
this->d[2] = this->erreurPosition.getTheta() - this->erreurPosition_old.getTheta();
// Calcul du PID pour chaque axe
double commandeX = kp * this->erreurPosition.getX() + ki * this->i[0] + kd * this->d[0];
double commandeY = kp * this->erreurPosition.getY() + ki * this->i[1] + kd * this->d[1];
double commandeTheta = Kp_theta * this->erreurPosition.getTheta() + Ki_theta * this->i[2] + Kd_theta * this->d[2];
sprintf(log, "[PID] Terme Dérivé | dX: %.3f | dY: %.3f | dTheta: %.3f\r\n", d[0], d[1], d[2]);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
// Conversion des commandes X/Y en une vitesse linéaire et une rotation
// Calcul des commandes PID
double commandeX = kp * this->erreurPosition.getX() + ki * i1 + kd * this->d[0];
double commandeY = kp * this->erreurPosition.getY() + ki * i2 + kd * this->d[1];
double commandeTheta = Kp_theta * this->erreurPosition.getTheta() + Ki_theta * i3 + Kd_theta * this->d[2];
sprintf(log, "[PID] Commandes PID | X: %.3f | Y: %.3f | Theta: %.3f\r\n", commandeX, commandeY, commandeTheta);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
// Conversion en vitesse linéaire et angulaire
double vitesseLineaire = sqrt(commandeX * commandeX + commandeY * commandeY);
double vitesseAngulaire = commandeTheta;
// Écrêtage des valeurs
if (vitesseLineaire > this->Vmax){
vitesseLineaire = this->Vmax;
}
if (vitesseLineaire < -this->Vmax){
vitesseLineaire = -this->Vmax;
}
if (vitesseAngulaire > this->Wmax){
vitesseAngulaire = this->Wmax;
}
if (vitesseAngulaire < -this->Wmax){
vitesseAngulaire = -this->Wmax;
}
sprintf(log, "[PID] Vitesses Avant Saturation | Linéaire: %.3f m/s | Angulaire: %.3f rad/s\r\n", vitesseLineaire, vitesseAngulaire);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
sprintf(log, "[PID] Vitesses Après Saturation | Linéaire: %.3f m/s | Angulaire: %.3f rad/s\r\n", vitesseLineaire, vitesseAngulaire);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
// Conversion en vitesse des roues
double vitesseGauche = vitesseLineaire - (this->L / 2) * vitesseAngulaire;
double vitesseDroite = vitesseLineaire + (this->L / 2) * vitesseAngulaire;
//return {vitesseGauche, vitesseDroite};
return {0.5, 0.5};
if (vitesseGauche < 0.235 && vitesseDroite < 0.235 && vitesseGauche > -0.235 && vitesseDroite > -0.235){
this->i[0] = i1;
this->i[1] = i2;
this->i[2] = i3;
}
// Saturation des vitesses des roues
if (vitesseGauche > 0.235){ vitesseGauche = 0.235;}
else if (vitesseGauche < -0.235) vitesseGauche = -0.235;
if (vitesseDroite > 0.235) vitesseDroite = 0.235;
else if (vitesseDroite < -0.235) vitesseDroite = -0.235;
sprintf(log, "[SET] VITESSE SORTIE DE PID POS | G: %.3f m/s | D: %.3f m/s\r\n", vitesseGauche, vitesseDroite);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
return {vitesseGauche, vitesseDroite};
//return {0.235, 0.235};
}

2
Core/Src/pidVitesse.cpp
View File

@@ -112,5 +112,5 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
this->integral,
this->derivee,
this->nouvelleConsigneVitesse);
CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer));
//CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer));
}