comm en cours de modif (variables globales rajoutées si jamais ça marche plus

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

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);
+}
+

Core/Src/commSTM.cpp

@@ -5,13 +5,12 @@
  *      Author: maxch
  */
 
-
-
-
 #include "commSTM.h"
 #include "usbd_cdc.h"
 #include <string.h>
 #include <stdio.h>
+#include <stdlib.h>
+#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) {
+    char *token = strtok(parse_buffer, ";");
-        USB_Comm_Send("SET;POS;123;456;1.57\n");
+    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) {
+    else if (strcmp(token, "SET") == 0) {
-        USB_Comm_Send("SET;SPEED;12;34;0.1\n");
+        token = strtok(NULL, ";");
-    }
+        if (!token) return;
-    else if (strncmp(parse_buffer, "GET;PID", 7) == 0) {
+
-        USB_Comm_Send("SET;PID;1.0;0.1;0.05\n");
+        if (strcmp(token, "POS") == 0) {
-    }
+            float x = atof(strtok(NULL, ";"));
-    else if (strncmp(parse_buffer, "SET;POS;", 8) == 0) {
+            float y = atof(strtok(NULL, ";"));
-        // Tu peux parser les valeurs et mettre à jour ta position ici
+            float t = atof(strtok(NULL, ";"));
-        USB_Comm_Send("OK;POS\n");
+            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");

Core/Src/modelec.cpp

@@ -7,6 +7,7 @@
 #include <cstdio>
 #include <cstring>
 #include <math.h>
+#include <algorithm>
 #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<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit);
+	//std::array<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit);
+	std::array<double, 2> 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) {
+	// 2. On détermine dynamiquement la limite du delta PWM à appliquer
-	    erreurG = maxErreur;
+	int maxErreurG = std::min(std::max((int)(fabs(erreurVitG) * 300.0f), 20), 150);
-	} else if (erreurG < -maxErreur) {
+	int maxErreurD = std::min(std::max((int)(fabs(erreurVitD) * 300.0f), 20), 150);
-	    erreurG = -maxErreur;
-	}
 
-	if (erreurD > maxErreur) {
+	// 3. On applique la limite dynamique sur les erreurs de PWM
-	    erreurD = maxErreur;
+	erreurG = std::min(std::max(erreurG, -maxErreurG), maxErreurG);
-	} else if (erreurD < -maxErreur) {
+	erreurD = std::min(std::max(erreurD, -maxErreurD), maxErreurD);
-	    erreurD = -maxErreur;
-	}
 
 	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.6,   // kp — réduit pour adoucir la réaction
-	    0.05,  // ki
+	    0.0,   // ki — on évite encore pour l’instant
-	    0.5,   // kd
+	    0.03,  // kd — un peu de dérivée pour stabiliser
-	    0.5,   // Kp_theta
+
-	    0.02,  // Ki_theta
+	    0.4,   // kpTheta — moins agressif sur la rotation
-	    0.7,   // Kd_theta
+	    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;
+	vitesseLeft = distLeft / dt;
-	float vitesseRight = distRight / dt;
+	vitesseRight = distRight / dt;
 
 	// Vitesse linéaire et angulaire du robot
-	float vitesseLineaire = (vitesseLeft + vitesseRight) / 2.0f;
+	vitesseLineaire = (vitesseLeft + vitesseRight) / 2.0f;
-	float vitesseAngulaire = (vitesseRight - vitesseLeft) / WHEEL_BASE;
+	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++;
 
 
 

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));
 }
 

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<double, 2> 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));
 
-
+    // Affichage erreur
-
-    // 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
+    // Passage repère global -> repère robot
-    // Si la commande est saturée, n'accumule pas l'intégrale
+    double errX = erreurPosition.getX();
-    float i1 = this->i[0] + this->erreurPosition.getX();
+    double errY = erreurPosition.getY();
-    float i2 = this->i[1] + this->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é
+    // Terme dérivé
-    this->d[0] = this->erreurPosition.getX() - this->erreurPosition_old.getX();
+    double errX_old = erreurPosition_old.getX();
-    this->d[1] = this->erreurPosition.getY() - this->erreurPosition_old.getY();
+    double errY_old = erreurPosition_old.getY();
-    this->d[2] = this->erreurPosition.getTheta() - this->erreurPosition_old.getTheta();
 
-    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 erreurTheta = erreurPosition.getTheta();
-    double commandeX = kp * this->erreurPosition.getX() + ki * i1 + kd * this->d[0];
+    constexpr double seuilTheta = 0.05;  // rad ≈ 2.8°
-    double commandeY = kp * this->erreurPosition.getY() + ki * i2 + kd * this->d[1];
+    if (fabs(erreurTheta) < seuilTheta) {
-    double commandeTheta = Kp_theta * this->erreurPosition.getTheta() + Ki_theta * i3 + Kd_theta * this->d[2];
+        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
+    // Conversion en vitesse
-    double vitesseLineaire = sqrt(commandeX * commandeX + commandeY * commandeY);
+    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){
+    const double tolPosition = 0.01; // 1 cm
-    	this->i[0] = i1;
+    const double tolTheta = 0.1;    // 0.05 rad ≈ 3°
-    	this->i[1] = i2;
+
-    	this->i[2] = i3;
+    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
+    // Saturation
-    if (vitesseGauche > 0.235){ vitesseGauche = 0.235;}
+    vitesseGauche = std::max(-0.235, std::min(0.235, vitesseGauche));
-    else if (vitesseGauche < -0.235) vitesseGauche = -0.235;
+    vitesseDroite = std::max(-0.235, std::min(0.235, vitesseDroite));
-
-    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));
 
+    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};
 }
 
+
+
+
+
+

Core/Src/pidVitesse.cpp

@@ -80,29 +80,57 @@ void PidVitesse::updateErreurVitesse(float vitesseActuelle) {
 }
 
 void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
-	/*
+    // === Paramètres de sécurité / stabilisation ===
-		 La sortie est calculée en combinant les 3 termes :
+    const float zoneMorteErreur     = 0.02f;   // seuil min pour agir sur l'erreur
-		 	 - proportionnel : on multiplie kp par l'erreur
+    const float zoneMorteSortie    = 0.005f;  // seuil min pour agir sur la sortie
-		 	 - integral 	 : on multiplie ki par la somme des erreurs passées
+    const float integralMax        = 10.0f;   // anti-windup
-		 	 - derivee		 : on multiplie kd par la variation de l'erreur
+    const float deriveeMax         = 1.0f;    // limitation de la dérivée
-	 */
-	//vitesseActuelle = 0.0;
 
     // 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->getKi() * this->integral +
-                                    this->getKd() * this->derivee;
+                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));
 }
+