diff --git a/Core/Inc/CommCallbacks.hpp b/Core/Inc/CommCallbacks.hpp new file mode 100644 index 0000000..cb8eaad --- /dev/null +++ b/Core/Inc/CommCallbacks.hpp @@ -0,0 +1,22 @@ +#ifndef COMM_CALLBACKS_HPP +#define COMM_CALLBACKS_HPP + +#ifdef __cplusplus +extern "C" { +#endif + +// Déclarations des fonctions appelées depuis le C +void Comm_GetPosition(float* x, float* y, float* t); +void Comm_GetSpeed(float* vx, float* vy, float* omega); +void Comm_GetPID(float* p, float* i, float* d); +float Comm_GetDistance(int sensorId); // 👈 AJOUT ICI +void Comm_SetPosition(float x, float y, float t); +void Comm_SetPID(float p, float i, float d); +void Comm_AddWaypoint(int id, int type, float x, float y, float t); +void Comm_StartOdometry(bool start); + +#ifdef __cplusplus +} +#endif + +#endif // COMM_CALLBACKS_HPP diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp new file mode 100644 index 0000000..adaa637 --- /dev/null +++ b/Core/Src/CommCallbacks.cpp @@ -0,0 +1,52 @@ +/* + * CommCallbacks.cpp + * + * Created on: May 25, 2025 + * Author: maxch + */ + + + +#include "CommCallbacks.hpp" +#include "main.h" +#include "modelec.cpp" +#include "motors.h" +#include "pid.h" +#include "pidPosition.h" + +void Comm_GetPosition(float* x, float* y, float* theta) { + *x = currentPoint.getX(); + *y = currentPoint.getY(); + *theta = currentPoint.getTheta(); +} + +void Comm_SetPosition(float x, float y, float theta) { + currentPoint.setX(x); + currentPoint.setY(y); + currentPoint.setTheta(theta); +} + +void Comm_GetSpeed(float* vx, float* vy, float* omega){ + *vx = vitesseLeft; + *vy = vitesseRight; + *omega = vitesseAngulaire; +} + +void Comm_GetPID(float* p, float* i, float* d) { + PIDController::getInstance().getCoefficients(*p, *i, *d); +} + +void Comm_SetPID(float p, float i, float d) { + PIDController::getInstance().setCoefficients(p, i, d); +} + +void Comm_StartOdometry(bool on) { + if (on) Odometry::getInstance().start(); + else Odometry::getInstance().stop(); +} + +void Comm_AddWaypoint(int id, int type, float x, float y, float theta) { + Waypoint wp(id, type, x, y, theta); + WaypointManager::getInstance().addWaypoint(wp); +} + diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp index e551bc7..b89db13 100644 --- a/Core/Src/commSTM.cpp +++ b/Core/Src/commSTM.cpp @@ -5,13 +5,12 @@ * Author: maxch */ - - - #include "commSTM.h" #include "usbd_cdc.h" #include #include +#include +#include "CommCallbacks.hpp" // ⚠️ fourni par CubeMX dans usb_device.c extern USBD_HandleTypeDef hUsbDeviceFS; @@ -27,7 +26,6 @@ static char parse_buffer[RX_BUFFER_SIZE]; // Pour parser la ligne complète // Fonction à appeler depuis CDC_Receive_FS void USB_Comm_OnReceive(uint8_t* Buf, uint32_t Len) { if (Len + usb_rx_index >= RX_BUFFER_SIZE) { - // Trop de données, on reset le buffer usb_rx_index = 0; return; } @@ -35,7 +33,6 @@ void USB_Comm_OnReceive(uint8_t* Buf, uint32_t Len) { for (uint32_t i = 0; i < Len; i++) { char c = Buf[i]; if (c == '\n' || c == '\r') { - // Fin de commande usb_rx_buffer[usb_rx_index] = '\0'; memcpy(parse_buffer, usb_rx_buffer, usb_rx_index + 1); usb_rx_index = 0; @@ -57,21 +54,84 @@ static void USB_Comm_Send(const char* message) { // Analyse et réponse aux commandes reçues void USB_Comm_Process(void) { if (!data_ready) return; - data_ready = 0; - if (strncmp(parse_buffer, "GET;POS", 7) == 0) { - USB_Comm_Send("SET;POS;123;456;1.57\n"); + char *token = strtok(parse_buffer, ";"); + if (!token) return; + + if (strcmp(token, "GET") == 0) { + token = strtok(NULL, ";"); + if (!token) return; + + if (strcmp(token, "POS") == 0) { + float x, y, t; + Comm_GetPosition(&x, &y, &t); + char response[64]; + snprintf(response, sizeof(response), "SET;POS;%.2f;%.2f;%.2f\n", x, y, t); + USB_Comm_Send(response); + } + else if (strcmp(token, "SPEED") == 0) { + float vx, vy, omega; + Comm_GetSpeed(&vx, &vy, &omega); // 3 valeurs + char response[64]; + snprintf(response, sizeof(response), "SET;SPEED;%.2f;%.2f;%.2f\n", vx, vy, omega); + USB_Comm_Send(response); + } + else if (strcmp(token, "PID") == 0) { + float p, i, d; + Comm_GetPID(&p, &i, &d); + char response[64]; + snprintf(response, sizeof(response), "SET;PID;%.2f;%.2f;%.2f\n", p, i, d); + USB_Comm_Send(response); + } + else if (strcmp(token, "DIST") == 0) { + token = strtok(NULL, ";"); + if (!token) return; + int n = atoi(token); + float dist = Comm_GetDistance(n); + char response[64]; + snprintf(response, sizeof(response), "SET;DIST;%d;%.2f\n", n, dist); + USB_Comm_Send(response); + } + else { + USB_Comm_Send("KO;UNKNOWN\n"); + } } - else if (strncmp(parse_buffer, "GET;SPEED", 9) == 0) { - USB_Comm_Send("SET;SPEED;12;34;0.1\n"); - } - else if (strncmp(parse_buffer, "GET;PID", 7) == 0) { - USB_Comm_Send("SET;PID;1.0;0.1;0.05\n"); - } - else if (strncmp(parse_buffer, "SET;POS;", 8) == 0) { - // Tu peux parser les valeurs et mettre à jour ta position ici - USB_Comm_Send("OK;POS\n"); + else if (strcmp(token, "SET") == 0) { + token = strtok(NULL, ";"); + if (!token) return; + + if (strcmp(token, "POS") == 0) { + float x = atof(strtok(NULL, ";")); + float y = atof(strtok(NULL, ";")); + float t = atof(strtok(NULL, ";")); + Comm_SetPosition(x, y, t); + USB_Comm_Send("OK;POS\n"); + } + else if (strcmp(token, "PID") == 0) { + float p = atof(strtok(NULL, ";")); + float i = atof(strtok(NULL, ";")); + float d = atof(strtok(NULL, ";")); + Comm_SetPID(p, i, d); + USB_Comm_Send("OK;PID\n"); + } + else if (strcmp(token, "WAYPOINT") == 0) { + int id = atoi(strtok(NULL, ";")); + int type = atoi(strtok(NULL, ";")); + float x = atof(strtok(NULL, ";")); + float y = atof(strtok(NULL, ";")); + float t = atof(strtok(NULL, ";")); + Comm_AddWaypoint(id, type, x, y, t); + USB_Comm_Send("OK;WAYPOINT\n"); + } + else if (strcmp(token, "START") == 0) { + int val = atoi(strtok(NULL, ";")); + Comm_StartOdometry(val != 0); + USB_Comm_Send("OK;START\n"); + } + else { + USB_Comm_Send("KO;UNKNOWN\n"); + } } else { USB_Comm_Send("KO;UNKNOWN\n"); diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index a87dc4d..26bf9c1 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include "pidVitesse.h" #include "pid.h" #include "point.h" @@ -36,6 +37,12 @@ uint16_t lastPosRight, lastPosLeft; // x et y sont en mètres float x, y, theta; +Point currentPoint(0, 0,0, StatePoint::INTERMEDIAIRE); +float vitesseLineaire; +float vitesseAngulaire; +float vitesseLeft; +float vitesseRight; + uint32_t lastTick = 0; bool isDelayPassedFrom(uint32_t delay, uint32_t *lastTick) { @@ -50,12 +57,16 @@ bool isDelayPassed(uint32_t delay) { } //PID -void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition& pid, PidVitesse& pidG, PidVitesse& pidD, float vitGauche, float vitDroit){ +void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition& pid, PidVitesse& pidG, PidVitesse& pidD, float vitGauche, float vitDroit, int cnt){ //PidPosition pid(0,0,0,0,0,0,objectifPoint); pid.setConsignePositionFinale(objectifPoint); - std::array vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit); + //std::array vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit); + std::array vitesse = {0, 0}; + if(cnt<18){ + vitesse = {0.235, 0.235}; + } char debug_msg[128]; sprintf(debug_msg, "[CONS] G: %.3f m/s | D: %.3f m/s\r\n", vitesse[0], vitesse[1]); @@ -79,19 +90,17 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPositi int erreurG = pidG.getPWMCommand(nouvelOrdreG); int erreurD = pidD.getPWMCommand(nouvelOrdreD); - const int maxErreur = 5; + // 1. On récupère l'erreur de vitesse actuelle pour chaque PID + float erreurVitG = pidG.getErreurVitesse(); + float erreurVitD = pidD.getErreurVitesse(); - if (erreurG > maxErreur) { - erreurG = maxErreur; - } else if (erreurG < -maxErreur) { - erreurG = -maxErreur; - } + // 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 (erreurD > maxErreur) { - erreurD = maxErreur; - } else if (erreurD < -maxErreur) { - erreurD = -maxErreur; - } + // 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); int ordrePWMG = motor.getLeftCurrentPWM() + erreurG; int ordrePWMD = motor.getRightCurrentPWM() + erreurD; @@ -113,14 +122,16 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) { //motor.accelerer(300); *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 + 0.6, // kp — réduit pour adoucir la réaction + 0.0, // ki — on évite encore pour l’instant + 0.03, // kd — un peu de dérivée pour stabiliser + + 0.4, // kpTheta — moins agressif sur la rotation + 0.0, // kiTheta + 0.2, // kdTheta — diminue les surcorrections d'angle 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); @@ -167,12 +178,12 @@ void ModelecOdometryUpdate() { float dt = 0.01f; // 10 ms // Calcul des vitesses des roues - float vitesseLeft = distLeft / dt; - float vitesseRight = distRight / dt; + vitesseLeft = distLeft / dt; + vitesseRight = distRight / dt; // Vitesse linéaire et angulaire du robot - float vitesseLineaire = (vitesseLeft + vitesseRight) / 2.0f; - float vitesseAngulaire = (vitesseRight - vitesseLeft) / WHEEL_BASE; + vitesseLineaire = (vitesseLeft + vitesseRight) / 2.0f; + vitesseAngulaire = (vitesseRight - vitesseLeft) / WHEEL_BASE; // Affichage pour debug sprintf(msg, "Vitesse G: %.3f m/s | D: %.3f m/s | Lin: %.3f m/s | Ang: %.3f rad/s\r\n", @@ -200,14 +211,17 @@ 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(); //USB_Comm_Process(); //HAL_Delay(1000); - Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE); + currentPoint.setX(x); + currentPoint.setY(y); + currentPoint.setTheta(theta); + //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", @@ -215,9 +229,10 @@ 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()); //TODO vérification inversement target et current + determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed(), cnt); //HAL_Delay(1000); motor.update(); + cnt++; diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp index 5535ed6..c23c65f 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/pidPosition.cpp b/Core/Src/pidPosition.cpp index c12dcb7..159cd12 100644 --- a/Core/Src/pidPosition.cpp +++ b/Core/Src/pidPosition.cpp @@ -112,81 +112,101 @@ Point PidPosition::calculErreurPosition(Point p) { // Mise à jour et calcul du nouvel ordre de vitesse pour les moteurs std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, float vitGauche, float vitDroit) { - char log[128]; + char log[128]; this->updateErreurPosition(pointActuel); + + // Affichage position actuelle vs consigne 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 + // Affichage erreur 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 - // 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(); + // Passage repère global -> repère robot + double errX = erreurPosition.getX(); + double errY = erreurPosition.getY(); + double theta = pointActuel.getTheta(); + + double erreurAvant = cos(theta) * errX + sin(theta) * errY; + double erreurLat = -sin(theta) * errX + cos(theta) * errY; + + // Terme intégral + float i1 = this->i[0] + erreurAvant; + float i2 = this->i[1] + erreurLat; 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]); + sprintf(log, "[PID] Terme Intégral | iAvant: %.3f | iLat: %.3f | iTheta: %.3f\r\n", i1, i2, i3); 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(); + // Terme dérivé + double errX_old = erreurPosition_old.getX(); + double errY_old = erreurPosition_old.getY(); - sprintf(log, "[PID] Terme Dérivé | dX: %.3f | dY: %.3f | dTheta: %.3f\r\n", d[0], d[1], d[2]); + double erreurAvant_old = cos(theta) * errX_old + sin(theta) * errY_old; + double erreurLat_old = -sin(theta) * errX_old + cos(theta) * errY_old; + + double d1 = erreurAvant - erreurAvant_old; + double d2 = erreurLat - erreurLat_old; + double d3 = erreurPosition.getTheta() - erreurPosition_old.getTheta(); + + sprintf(log, "[PID] Terme Dérivé | dAvant: %.3f | dLat: %.3f | dTheta: %.3f\r\n", d1, d2, d3); CDC_Transmit_FS((uint8_t*)log, strlen(log)); - // 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]; + double erreurTheta = erreurPosition.getTheta(); + constexpr double seuilTheta = 0.05; // rad ≈ 2.8° + if (fabs(erreurTheta) < seuilTheta) { + erreurTheta = 0.0; + d3 = 0.0; + i3 = 0.0; + } - sprintf(log, "[PID] Commandes PID | X: %.3f | Y: %.3f | Theta: %.3f\r\n", commandeX, commandeY, commandeTheta); + // Commandes PID (SANS freinage progressif) + double commandeAvant = kp * erreurAvant + ki * i1 + kd * d1; + double commandeTheta = Kp_theta * erreurPosition.getTheta() + Ki_theta * i3 + Kd_theta * d3; + + sprintf(log, "[PID] Commandes PID | Avant: %.3f | Theta: %.3f\r\n", commandeAvant, commandeTheta); CDC_Transmit_FS((uint8_t*)log, strlen(log)); - // Conversion en vitesse linéaire et angulaire - double vitesseLineaire = sqrt(commandeX * commandeX + commandeY * commandeY); + // Conversion en vitesse + double vitesseLineaire = commandeAvant; double vitesseAngulaire = commandeTheta; - sprintf(log, "[PID] Vitesses Avant Saturation | Linéaire: %.3f m/s | Angulaire: %.3f rad/s\r\n", vitesseLineaire, vitesseAngulaire); + sprintf(log, "[PID] Vitesses Avant Saturation | Linéaire: %.3f | Angulaire: %.3f\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; - 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; + const double tolPosition = 0.01; // 1 cm + const double tolTheta = 0.1; // 0.05 rad ≈ 3° + + if (fabs(erreurAvant) > tolPosition || fabs(erreurLat) > tolPosition || fabs(this->erreurPosition.getTheta()) > tolTheta) { + 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; - - + // Saturation + vitesseGauche = std::max(-0.235, std::min(0.235, vitesseGauche)); + vitesseDroite = std::max(-0.235, std::min(0.235, vitesseDroite)); 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) { + sprintf(log, "[PID] OBJECTIF ATTEINT — Robot à l'arrêt\r\n"); + CDC_Transmit_FS((uint8_t*)log, strlen(log)); + return {0.0, 0.0}; + } + return {vitesseGauche, vitesseDroite}; - //return {0.235, 0.235}; } + + + + + diff --git a/Core/Src/pidVitesse.cpp b/Core/Src/pidVitesse.cpp index 7cf4842..366fa58 100644 --- a/Core/Src/pidVitesse.cpp +++ b/Core/Src/pidVitesse.cpp @@ -80,29 +80,57 @@ void PidVitesse::updateErreurVitesse(float vitesseActuelle) { } void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { - /* - 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 - */ - //vitesseActuelle = 0.0; + // === 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 // Mise à jour de l'erreur de vitesse this->updateErreurVitesse(vitesseActuelle); - // Calcul du terme dérivé + // Zone morte sur l’erreur + if (fabs(this->erreurVitesse) < zoneMorteErreur) { + this->erreurVitesse = 0.0f; + } + if (fabs(this->consigneVitesseFinale) < 0.001f) { + this->integral = 0.0f; + this->derivee = 0.0f; + this->nouvelleConsigneVitesse = 0.0f; + return; + } + + + // Calcul du terme dérivé (bruit possible) this->derivee = this->erreurVitesse - this->erreurVitesse_old; + // Limitation de la dérivée (anti-pics) + if (this->derivee > deriveeMax) this->derivee = deriveeMax; + else if (this->derivee < -deriveeMax) this->derivee = -deriveeMax; + // Mise à jour du terme intégral this->integral += this->erreurVitesse; - // Calcul de la nouvelle consigne de vitesse avec PID + // Anti-windup sur l'intégrale + if (this->integral > integralMax) this->integral = integralMax; + else if (this->integral < -integralMax) this->integral = -integralMax; + + // Calcul de la commande PID float pid = this->getKp() * this->erreurVitesse + - this->getKi() * this->integral + - this->getKd() * this->derivee; + 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; + // === DEBUG === char buffer[128]; snprintf(buffer, sizeof(buffer), "[PID] Cons: %.3f | Mes: %.3f | Err: %.3f | I: %.3f | D: %.3f | Out: %.3f\r\n", @@ -112,5 +140,6 @@ 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)); } +