From 0695b07c25dcca37e462aaadc125d181b2247d24 Mon Sep 17 00:00:00 2001 From: Maxime Date: Thu, 22 May 2025 12:49:51 +0200 Subject: [PATCH] corrections en cours --- Core/Inc/pidPosition.h | 6 +-- Core/Src/modelec.cpp | 17 ++++-- Core/Src/motors.cpp | 2 +- Core/Src/pid.cpp | 6 +-- Core/Src/pidPosition.cpp | 108 ++++++++++++++++++++++++++------------- Core/Src/pidVitesse.cpp | 2 +- 6 files changed, 93 insertions(+), 48 deletions(-) diff --git a/Core/Inc/pidPosition.h b/Core/Inc/pidPosition.h index 673363c..b04d88c 100644 --- a/Core/Inc/pidPosition.h +++ b/Core/Inc/pidPosition.h @@ -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 updateNouvelOrdreVitesse(Point pointActuel); + std::array updateNouvelOrdreVitesse(Point pointActuel, float vitGauche, float vitDroit); // Surcharge des méthodes virtuelles void updateErreur(); diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index aa7c05c..a87dc4d 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -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 vitesse = pid.updateNouvelOrdreVitesse(pointActuel); + std::array 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(); diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp index c23c65f..5535ed6 100644 --- a/Core/Src/motors.cpp +++ b/Core/Src/motors.cpp @@ -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)); } diff --git a/Core/Src/pid.cpp b/Core/Src/pid.cpp index db1987a..45fb0b0 100644 --- a/Core/Src/pid.cpp +++ b/Core/Src/pid.cpp @@ -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 diff --git a/Core/Src/pidPosition.cpp b/Core/Src/pidPosition.cpp index 4f9456c..c12dcb7 100644 --- a/Core/Src/pidPosition.cpp +++ b/Core/Src/pidPosition.cpp @@ -1,7 +1,8 @@ #include "pidPosition.h" - - +#include +#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(), - this->consignePositionFinale.getY() - p.getY(), - this->consignePositionFinale.getTheta() - p.getTheta(), - StatePoint::NONDETERMINE); + return Point( + this->consignePositionFinale.getX() - p.getX(), + this->consignePositionFinale.getY() - p.getY(), + 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 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 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}; } + diff --git a/Core/Src/pidVitesse.cpp b/Core/Src/pidVitesse.cpp index 2bf9f52..7cf4842 100644 --- a/Core/Src/pidVitesse.cpp +++ b/Core/Src/pidVitesse.cpp @@ -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)); }