diff --git a/Core/Src/main.c b/Core/Src/main.c
index 94771da..c23e131 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -24,6 +24,7 @@
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
+
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -33,9 +34,9 @@
 
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN PD */
-void ModelecOdometrySetup();
+void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD);
 
-void ModelecOdometryLoop();
+void ModelecOdometryLoop(void* pid, void* pidG, void* pidD);
 
 /* USER CODE END PD */
 
@@ -103,15 +104,19 @@ int main(void)
   HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
   HAL_TIM_Encoder_Start(&htim21, TIM_CHANNEL_ALL);
   //HAL_TIM_Encoder_Start_IT(&htim2, TIM_CHANNEL_ALL);
-  ModelecOdometrySetup();
+  void *pid;
+  void *pidG;
+  void *pidD;
+  ModelecOdometrySetup(&pid, &pidG, &pidD);
 
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
+
   while (1)
   {
-	ModelecOdometryLoop();
+	ModelecOdometryLoop(pid, pidG, pidD);
 
   }
     /* USER CODE END WHILE */
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index f2115b8..16c216d 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -45,17 +45,21 @@ bool isDelayPassed(uint32_t delay) {
 }
 
 //PID
-void determinationCoefPosition(Point objectifPoint, Point pointActuel){
-	PidPosition pid(0,0,0,0,0,0,objectifPoint);
+void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition pid, PidVitesse pidG, PidVitesse pidD){
+	//PidPosition pid(0,0,0,0,0,0,objectifPoint);
 
 
+	pid.setConsignePositionFinale(objectifPoint);
 	std::array<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel);
-	PidVitesse pidG(0, 0, 0, vitesse[0]);
-	PidVitesse pidD(0, 0, 0, vitesse[1]);
+	//PidVitesse pidG(0, 0, 0, vitesse[0]);
+	//PidVitesse pidD(0, 0, 0, vitesse[1]);
+	pidG.setConsigneVitesseFinale(vitesse[0]);
+	pidD.setConsigneVitesseFinale(vitesse[1]);
 
 	pidG.updateNouvelleVitesse(motor.getLeftCurrentSpeed());
 	pidD.updateNouvelleVitesse(motor.getRightCurrentSpeed());
 
+
 	float nouvelOrdreG = pidG.getNouvelleConsigneVitesse();
 	float nouvelOrdreD = pidD.getNouvelleConsigneVitesse();
 
@@ -70,7 +74,7 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel){
 }
 //Odométrie
 
-void ModelecOdometrySetup() {
+void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) {
 	HAL_UART_Transmit(&huart2, (uint8_t*) "SETUP COMPLETE\n", 15,
 			HAL_MAX_DELAY);
 	lastPosRight = __HAL_TIM_GET_COUNTER(&htim2);
@@ -78,7 +82,14 @@ void ModelecOdometrySetup() {
 	x = 0.0f;
 	y = 0.0f;
 	theta = 0.0f;
-	motor.accelerer(300);
+	//motor.accelerer(300);
+
+    *out_pid = new PidPosition(0,0,0,0,0,0,Point());
+    *out_pidG = new PidVitesse(0, 0, 0, 0);
+    *out_pidD = new PidVitesse(0, 0, 0, 0);
+
+	return;
+
 }
 
 void ModelecOdometryUpdate() {
@@ -126,8 +137,12 @@ void receiveControlParams(){
 
 }
 
-void ModelecOdometryLoop() {
-	receiveControlParams();
+void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) {
+	PidPosition* pidPosition = static_cast<PidPosition*>(pid);
+	PidVitesse* pidVitesseG = static_cast<PidVitesse*>(pidG);
+	PidVitesse* pidVitesseD = static_cast<PidVitesse*>(pidD);
+
+	//receiveControlParams();
 	GPIOC->ODR ^= (1 << 10);
 
 	//On met à jour toutes les 10ms
@@ -135,10 +150,7 @@ void ModelecOdometryLoop() {
 		ModelecOdometryUpdate();
 		Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE);
 		Point targetPoint(0.20, 0.20,0, StatePoint::FINAL);
-		determinationCoefPosition(currentPoint,targetPoint);
-
-		sprintf(msg, "X: %.3f m, Y: %.3f m, Theta: %.3f rad\r\n", x, y, theta);
-		HAL_UART_Transmit(&huart2, (uint8_t*) msg, strlen(msg), HAL_MAX_DELAY);
+		determinationCoefPosition(currentPoint,targetPoint, *pidPosition, *pidVitesseG, *pidVitesseD);
 
 		motor.update();
 
@@ -147,4 +159,4 @@ void ModelecOdometryLoop() {
 	publishStatus();
 }
 
-}
+} //extern C end
diff --git a/Core/Src/pid.cpp b/Core/Src/pid.cpp
index 94d102e..611caec 100644
--- a/Core/Src/pid.cpp
+++ b/Core/Src/pid.cpp
@@ -1,7 +1,11 @@
 
-
 #include "pid.h"
+#include "stm32l0xx_hal.h"      // <--- D'abord
+#include "stm32l0xx_hal_uart.h" // optionnel, déjà dans le précédent
+#include <cstdio>
+#include <cstring>
 
+extern UART_HandleTypeDef huart2;
 
 // Constructeur
 Pid::Pid(float kp, float ki, float kd){
@@ -62,10 +66,15 @@ int Pid::getPWMCommand(float vitesse){
     if (pwm < PWM_MIN){
     	pwm = 0;
     }
+    int32_t pwm_signed = (vitesse >= 0) ? std::floor(pwm) : -std::floor(pwm);
 
+    char msg[64];
+    // Debug UART ici
+    sprintf(msg, "Vitesse: %.3f m/s, PWM: %d\r\n", vitesse, pwm_signed);
+    HAL_UART_Transmit(&huart2, (uint8_t*)msg, strlen(msg), 10);  // timeout court
 
 	    // Renvoi avec le bon signe
-    return (vitesse >= 0) ? std::floor(pwm) : -std::floor(pwm);
+    return pwm_signed;
 }
 
 
diff --git a/Core/Src/pidPosition.cpp b/Core/Src/pidPosition.cpp
index e125d72..ceb921e 100644
--- a/Core/Src/pidPosition.cpp
+++ b/Core/Src/pidPosition.cpp
@@ -137,6 +137,9 @@ std::array<double, 2> PidPosition::updateNouvelOrdreVitesse(Point pointActuel) {
     if (vitesseLineaire > this->Vmax){
     	vitesseLineaire = this->Vmax;
     }
+    if (vitesseLineaire < -this->Vmax){
+       	vitesseLineaire = -this->Vmax;
+    }
     if (vitesseAngulaire > this->Wmax){
     	vitesseAngulaire = this->Wmax;
     }