From e05f0aa6898a1981c4bc6fbdd93f5490e8509569 Mon Sep 17 00:00:00 2001 From: Maxime Date: Mon, 26 May 2025 17:55:48 +0200 Subject: [PATCH] correction : pidvitesse ok --- Core/Src/CommCallbacks.cpp | 3 ++- Core/Src/modelec.cpp | 41 ++++++++++++++++++-------------------- Core/Src/pidPosition.cpp | 2 +- Core/Src/pidVitesse.cpp | 24 ++++++---------------- 4 files changed, 28 insertions(+), 42 deletions(-) diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index adaa637..512a401 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -6,7 +6,7 @@ */ - +/* #include "CommCallbacks.hpp" #include "main.h" #include "modelec.cpp" @@ -50,3 +50,4 @@ void Comm_AddWaypoint(int id, int type, float x, float y, float theta) { WaypointManager::getInstance().addWaypoint(wp); } +*/ diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 26bf9c1..6c74b83 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -57,16 +57,13 @@ bool isDelayPassed(uint32_t delay) { } //PID -void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition& pid, PidVitesse& pidG, PidVitesse& pidD, float vitGauche, float vitDroit, int cnt){ +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, vitGauche, vitDroit); - std::array vitesse = {0, 0}; - if(cnt<18){ - vitesse = {0.235, 0.235}; - } + std::array vitesse = {0.05,0.05}; char debug_msg[128]; sprintf(debug_msg, "[CONS] G: %.3f m/s | D: %.3f m/s\r\n", vitesse[0], vitesse[1]); @@ -84,29 +81,31 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPositi float nouvelOrdreG = pidG.getNouvelleConsigneVitesse(); float nouvelOrdreD = pidD.getNouvelleConsigneVitesse(); - sprintf(debug_msg, "[CORR] G: %.3f m/s | D: %.3f m/s\r\n", nouvelOrdreG, nouvelOrdreD); - CDC_Transmit_FS((uint8_t*)debug_msg, strlen(debug_msg)); + int erreurG = pidG.getPWMCommand(nouvelOrdreG); int erreurD = pidD.getPWMCommand(nouvelOrdreD); - // 1. On récupère l'erreur de vitesse actuelle pour chaque PID - float erreurVitG = pidG.getErreurVitesse(); - float erreurVitD = pidD.getErreurVitesse(); + // Limite fixe du delta PWM + const int MAX_ERREUR_PWM = 50; - // 2. On détermine dynamiquement la limite du delta PWM à appliquer - int maxErreurG = std::min(std::max((int)(fabs(erreurVitG) * 300.0f), 20), 150); - int maxErreurD = std::min(std::max((int)(fabs(erreurVitD) * 300.0f), 20), 150); + if (erreurG > MAX_ERREUR_PWM) + erreurG = MAX_ERREUR_PWM; + else if (erreurG < -MAX_ERREUR_PWM) + erreurG = -MAX_ERREUR_PWM; - // 3. On applique la limite dynamique sur les erreurs de PWM - erreurG = std::min(std::max(erreurG, -maxErreurG), maxErreurG); - erreurD = std::min(std::max(erreurD, -maxErreurD), maxErreurD); + if (erreurD > MAX_ERREUR_PWM) + erreurD = MAX_ERREUR_PWM; + else if (erreurD < -MAX_ERREUR_PWM) + erreurD = -MAX_ERREUR_PWM; int ordrePWMG = motor.getLeftCurrentPWM() + erreurG; int ordrePWMD = motor.getRightCurrentPWM() + erreurD; motor.setLeftTargetPWM(ordrePWMG); motor.setRightTargetPWM(ordrePWMD); + sprintf(debug_msg, "[CORR] G: %.3f m/s | D: %.3f m/s\r\n", ordrePWMG, ordrePWMD); + CDC_Transmit_FS((uint8_t*)debug_msg, strlen(debug_msg)); } @@ -133,8 +132,8 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) { ); //*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); + *out_pidG = new PidVitesse(0.2, 0.0, 0.01, 0); + *out_pidD = new PidVitesse(0.2, 0.0, 0.01, 0); return; @@ -211,7 +210,6 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) { //receiveControlParams(); //GPIOC->ODR ^= (1 << 10); - int cnt=0; //On met à jour toutes les 10ms if (isDelayPassed(10)) { ModelecOdometryUpdate(); @@ -221,7 +219,7 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) { currentPoint.setX(x); currentPoint.setY(y); currentPoint.setTheta(theta); - //Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE); + Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE); Point targetPoint(0.50, 0.0,0, StatePoint::FINAL); char debugMsg[128]; sprintf(debugMsg, "Speed avant determination : L=%.3f | R=%.3f\r\n", @@ -229,10 +227,9 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) { CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg)); - determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed(), cnt); + determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed()); //HAL_Delay(1000); motor.update(); - cnt++; diff --git a/Core/Src/pidPosition.cpp b/Core/Src/pidPosition.cpp index 159cd12..4776a8a 100644 --- a/Core/Src/pidPosition.cpp +++ b/Core/Src/pidPosition.cpp @@ -196,7 +196,7 @@ std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f 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)); - if (fabs(erreurAvant) < 0.1 && fabs(erreurLat) < 0.1 && fabs(erreurPosition.getTheta()) < 2) { + if (fabs(erreurAvant) < 0.05 && fabs(erreurLat) < 0.05 && fabs(erreurPosition.getTheta()) < 0.5) { sprintf(log, "[PID] OBJECTIF ATTEINT — Robot à l'arrêt\r\n"); CDC_Transmit_FS((uint8_t*)log, strlen(log)); return {0.0, 0.0}; diff --git a/Core/Src/pidVitesse.cpp b/Core/Src/pidVitesse.cpp index 366fa58..59bfe23 100644 --- a/Core/Src/pidVitesse.cpp +++ b/Core/Src/pidVitesse.cpp @@ -80,19 +80,14 @@ void PidVitesse::updateErreurVitesse(float vitesseActuelle) { } void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { - // === Paramètres de sécurité / stabilisation === - const float zoneMorteErreur = 0.02f; // seuil min pour agir sur l'erreur - const float zoneMorteSortie = 0.005f; // seuil min pour agir sur la sortie - const float integralMax = 10.0f; // anti-windup - const float deriveeMax = 1.0f; // limitation de la dérivée + // === Paramètres de stabilisation === + const float integralMax = 10.0f; // anti-windup + const float deriveeMax = 1.0f; // limitation de la dérivée // Mise à jour de l'erreur de vitesse this->updateErreurVitesse(vitesseActuelle); - // Zone morte sur l’erreur - if (fabs(this->erreurVitesse) < zoneMorteErreur) { - this->erreurVitesse = 0.0f; - } + // Si la consigne est ~0, on neutralise tout if (fabs(this->consigneVitesseFinale) < 0.001f) { this->integral = 0.0f; this->derivee = 0.0f; @@ -100,8 +95,7 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { return; } - - // Calcul du terme dérivé (bruit possible) + // Calcul du terme dérivé this->derivee = this->erreurVitesse - this->erreurVitesse_old; // Limitation de la dérivée (anti-pics) @@ -120,13 +114,6 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { this->getKi() * this->integral + this->getKd() * this->derivee; - // Zone morte sur la sortie PID - if (fabs(pid) < zoneMorteSortie) { - pid = 0.0f; - // Optionnel : reset intégrale pour couper net le mouvement - this->integral = 0.0f; - } - // Application de la commande this->nouvelleConsigneVitesse = pid; @@ -143,3 +130,4 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { // CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer)); } +