From 38bca5e01c92c99ae3a5dc5437e5e7f80662104a Mon Sep 17 00:00:00 2001
From: Maxime <maxchauveau44@gmail.com>
Date: Wed, 7 May 2025 21:19:21 +0200
Subject: [PATCH] =?UTF-8?q?upgrade=20mis=20=C3=A0=20jour=20pour=20fonction?=
 =?UTF-8?q?ner=20avec=20le=20pid)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Core/Inc/motors.h    | 12 ++++++------
 Core/Src/modelec.cpp |  4 ++++
 Core/Src/motors.cpp  | 28 +++++++++++++++++++++++++---
 Core/Src/pid.cpp     | 33 +++++++++++++++++++++++----------
 4 files changed, 58 insertions(+), 19 deletions(-)

diff --git a/Core/Inc/motors.h b/Core/Inc/motors.h
index 3c567f3..d9c7705 100644
--- a/Core/Inc/motors.h
+++ b/Core/Inc/motors.h
@@ -6,10 +6,10 @@
 class Motor {
 private:
     TIM_TypeDef *tim;
-    uint16_t rightCurrentSpeed;
-    uint16_t leftCurrentSpeed;
-    uint16_t rightTargetSpeed;
-    uint16_t leftTargetSpeed;
+    int16_t rightCurrentSpeed;
+    int16_t leftCurrentSpeed;
+    int16_t rightTargetSpeed;
+    int16_t leftTargetSpeed;
     bool isAccelerating;
     bool isReversing;
     bool isTurningRight;
@@ -19,8 +19,8 @@ public:
     Motor(TIM_TypeDef *timer);
     void setRightTargetSpeed(int pwm);
     void setLeftTargetSpeed(int pwm);
-    float getRightCurrentSpeed();
-    float getLeftCurrentSpeed();
+    int16_t getRightCurrentSpeed();
+    int16_t getLeftCurrentSpeed();
     void accelerer(int speed);
     void reculer(int speed);
     void stop();
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 33a29e8..f2115b8 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -48,6 +48,7 @@ bool isDelayPassed(uint32_t delay) {
 void determinationCoefPosition(Point objectifPoint, Point pointActuel){
 	PidPosition pid(0,0,0,0,0,0,objectifPoint);
 
+
 	std::array<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel);
 	PidVitesse pidG(0, 0, 0, vitesse[0]);
 	PidVitesse pidD(0, 0, 0, vitesse[1]);
@@ -136,6 +137,9 @@ void ModelecOdometryLoop() {
 		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);
+
 		motor.update();
 
 	}
diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp
index 84e27ca..7aa3aaf 100644
--- a/Core/Src/motors.cpp
+++ b/Core/Src/motors.cpp
@@ -39,10 +39,10 @@ void Motor::setLeftTargetSpeed(int pwm){
 	}
 
 }
-float Motor::getRightCurrentSpeed(){
+int16_t Motor::getRightCurrentSpeed(){
 	return this->rightCurrentSpeed;
 }
-float Motor::getLeftCurrentSpeed(){
+int16_t Motor::getLeftCurrentSpeed(){
 	return this->leftCurrentSpeed;
 }
 
@@ -81,7 +81,7 @@ void Motor::stopTurning() {
 void Motor::update() {
     // Mise à jour des vitesses pour chaque moteur
     // Moteur gauche
-    if (leftCurrentSpeed < leftTargetSpeed) {
+    /*if (leftCurrentSpeed < leftTargetSpeed) {
         leftCurrentSpeed++;
     } else if (leftCurrentSpeed > leftTargetSpeed) {
         if (leftCurrentSpeed - 25 >= 0) {
@@ -116,6 +116,28 @@ void Motor::update() {
     if (isStopped()) {
         isAccelerating = false;
         isReversing = false;
+    }*/
+
+	// Appliquer targetSpeed dans currentSpeed
+    this->leftCurrentSpeed = this->leftTargetSpeed;
+    this->rightCurrentSpeed = this->rightTargetSpeed;
+
+    // Contrôle moteur gauche
+    if (this->leftCurrentSpeed >= 0) {
+        this->tim->CCR2 = static_cast<uint16_t>(this->leftCurrentSpeed);  // avant
+        this->tim->CCR1 = 0;
+    } else {
+        this->tim->CCR1 = static_cast<uint16_t>(-this->leftCurrentSpeed); // arrière
+        this->tim->CCR2 = 0;
+    }
+
+    // Contrôle moteur droit
+    if (this->rightCurrentSpeed >= 0) {
+        this->tim->CCR3 = static_cast<uint16_t>(this->rightCurrentSpeed); // avant
+        this->tim->CCR4 = 0;
+    } else {
+        this->tim->CCR4 = static_cast<uint16_t>(-this->rightCurrentSpeed); // arrière
+        this->tim->CCR3 = 0;
     }
 }
 
diff --git a/Core/Src/pid.cpp b/Core/Src/pid.cpp
index d6290ae..94d102e 100644
--- a/Core/Src/pid.cpp
+++ b/Core/Src/pid.cpp
@@ -43,16 +43,29 @@ float Pid::getKd(){
 
 //Methodes
 int Pid::getPWMCommand(float vitesse){
-	/*
-	 * Passer du rpm à m/s : 75/60 = 1.25 (nb tour en 1s). circonférence = pi*diamètre roues. Vitesse en m/s = circonférence * nb tour/s
-	 *
-	 */
-	if(vitesse >= 0.235){		//si on dépasse la vmax qu'on peut atteindre, on bride
-		return 626;
-	} else{
-		float pwm = (626*vitesse) / 0.235;		//produit en croix
-		return std::floor(pwm);
-	}
+	constexpr float VITESSE_MAX = 0.235f;  // m/s
+	constexpr int PWM_MAX = 626;           // commande PWM max
+	constexpr int PWM_MIN = 30;            // zone morte (optionnelle)
+
+	    // Saturation de la vitesse
+    if (vitesse > VITESSE_MAX){
+    	vitesse = VITESSE_MAX;
+    }
+    else if (vitesse < -VITESSE_MAX){
+    	vitesse = -VITESSE_MAX;
+    }
+
+	    // Conversion m/s -> PWM avec conservation du signe
+    float pwm = (PWM_MAX * std::abs(vitesse)) / VITESSE_MAX;
+
+	    // Application d'un seuil minimal pour éviter les très faibles commandes
+    if (pwm < PWM_MIN){
+    	pwm = 0;
+    }
+
+
+	    // Renvoi avec le bon signe
+    return (vitesse >= 0) ? std::floor(pwm) : -std::floor(pwm);
 }