From 0d741dcb57b3b707415c2dd7fbdd2ee2e0ad3b56 Mon Sep 17 00:00:00 2001 From: dd060606 Date: Wed, 23 Apr 2025 08:30:45 +0200 Subject: [PATCH] =?UTF-8?q?ajustement=20contr=C3=B4le=20moteurs=20par=20PI?= =?UTF-8?q?D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Inc/motors.h | 12 +++-- Core/Src/modelec.cpp | 12 ++--- Core/Src/motors.cpp | 118 +++++++++++++++++++++++++------------------ 3 files changed, 82 insertions(+), 60 deletions(-) diff --git a/Core/Inc/motors.h b/Core/Inc/motors.h index 421aa53..3c567f3 100644 --- a/Core/Inc/motors.h +++ b/Core/Inc/motors.h @@ -6,8 +6,10 @@ class Motor { private: TIM_TypeDef *tim; - uint16_t currentSpeed; - uint16_t targetSpeed; + uint16_t rightCurrentSpeed; + uint16_t leftCurrentSpeed; + uint16_t rightTargetSpeed; + uint16_t leftTargetSpeed; bool isAccelerating; bool isReversing; bool isTurningRight; @@ -15,8 +17,10 @@ private: public: Motor(TIM_TypeDef *timer); - void setTargetSpeed(int pwm); - float getCurrentSpeed(); + void setRightTargetSpeed(int pwm); + void setLeftTargetSpeed(int pwm); + float getRightCurrentSpeed(); + float getLeftCurrentSpeed(); void accelerer(int speed); void reculer(int speed); void stop(); diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 8997f93..8fae477 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -46,18 +46,14 @@ bool isDelayPassed(uint32_t delay) { //PID void determinationCoefPosition(Point objectifPoint, Point pointActuel){ - //à AJUSTER CAR NORMALEMENT IL Y A UN SEUL OBJET MOTEUR, il faut contrôler le signal avec CCR1, CCR2,CCR3,CCR4 - Motor motorG(TIM3); - Motor motorD(TIM3); - PidPosition pid(0,0,0,0,0,0,objectifPoint); std::array vitesse = pid.updateNouvelOrdreVitesse(pointActuel); PidVitesse pidG(0, 0, 0, vitesse[0]); PidVitesse pidD(0, 0, 0, vitesse[1]); - pidG.updateNouvelleVitesse(motorG.getCurrentSpeed()); - pidD.updateNouvelleVitesse(motorD.getCurrentSpeed()); + pidG.updateNouvelleVitesse(motor.getLeftCurrentSpeed()); + pidD.updateNouvelleVitesse(motor.getRightCurrentSpeed()); float nouvelOrdreG = pidG.getNouvelleConsigneVitesse(); float nouvelOrdreD = pidD.getNouvelleConsigneVitesse(); @@ -66,8 +62,8 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel){ int ordrePWMD = pidD.getPWMCommand(nouvelOrdreD); - motorG.setTargetSpeed(ordrePWMG); - motorD.setTargetSpeed(ordrePWMD); + motor.setLeftTargetSpeed(ordrePWMG); + motor.setRightTargetSpeed(ordrePWMD); } diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp index 01323b2..84e27ca 100644 --- a/Core/Src/motors.cpp +++ b/Core/Src/motors.cpp @@ -1,54 +1,74 @@ #include Motor::Motor(TIM_TypeDef *timer) : - tim(timer), currentSpeed(0), targetSpeed(0), isAccelerating(false), isReversing( + tim(timer), rightCurrentSpeed(0),leftCurrentSpeed(0), rightTargetSpeed(0),leftTargetSpeed(0), isAccelerating(false), isReversing( false), isTurningRight(false), isTurningLeft(false) { } void Motor::accelerer(int speed) { - targetSpeed = (speed <= 626) ? speed : 626; + speed = (speed <= 626) ? speed : 626; + rightTargetSpeed = speed; + leftTargetSpeed = speed; isAccelerating = true; isReversing = false; this->stopTurning(); } void Motor::reculer(int speed) { - targetSpeed = (speed <= 626) ? speed : 626; + speed = (speed <= 626) ? speed : 626; + rightTargetSpeed = speed; + leftTargetSpeed = speed; isReversing = true; isAccelerating = false; this->stopTurning(); } -void Motor::setTargetSpeed(int pwm){ +void Motor::setRightTargetSpeed(int pwm){ if(pwm < 626){ - this->targetSpeed = pwm; + this->rightTargetSpeed = pwm; }else{ - this->targetSpeed = 626; + this->rightTargetSpeed = 626; } } -float Motor::getCurrentSpeed(){ - return this->currentSpeed; +void Motor::setLeftTargetSpeed(int pwm){ + if(pwm < 626){ + this->leftTargetSpeed = pwm; + }else{ + this->leftTargetSpeed = 626; + } + +} +float Motor::getRightCurrentSpeed(){ + return this->rightCurrentSpeed; +} +float Motor::getLeftCurrentSpeed(){ + return this->leftCurrentSpeed; } void Motor::stop() { - targetSpeed = 0; + rightTargetSpeed = 0; + leftTargetSpeed = 0; this->stopTurning(); } bool Motor::isStopped() { - return currentSpeed == 0; + return rightCurrentSpeed == 0 && leftCurrentSpeed == 0 && rightTargetSpeed == 0 && leftTargetSpeed == 0; } void Motor::tournerDroite(int speed) { - targetSpeed = (speed <= 626) ? speed : 626; + speed = (speed <= 626) ? speed : 626; + rightTargetSpeed = speed / 2; + leftTargetSpeed = speed; isTurningRight = true; isTurningLeft = false; isAccelerating = true; } void Motor::tournerGauche(int speed) { - targetSpeed = (speed <= 626) ? speed : 626; + speed = (speed <= 626) ? speed : 626; + rightTargetSpeed = speed; + leftTargetSpeed = speed / 2; isTurningRight = false; isTurningLeft = true; isAccelerating = true; @@ -59,41 +79,43 @@ void Motor::stopTurning() { } void Motor::update() { - // Gestion de l'accélération/décélération - if ((isAccelerating || isReversing) && currentSpeed < targetSpeed) { - currentSpeed++; - } else if (currentSpeed > targetSpeed) { - if(currentSpeed - 25 >= 0) { - //Ralentir rapidement - currentSpeed-=25; - } - else { - currentSpeed--; - } - } + // Mise à jour des vitesses pour chaque moteur + // Moteur gauche + if (leftCurrentSpeed < leftTargetSpeed) { + leftCurrentSpeed++; + } else if (leftCurrentSpeed > leftTargetSpeed) { + if (leftCurrentSpeed - 25 >= 0) { + leftCurrentSpeed -= 25; + } else { + leftCurrentSpeed--; + } + } - // Mise à jour des registres du timer pour contrôler les moteurs - if (isTurningRight) { - // Tourne à droite en avançant - tim->CCR2 = currentSpeed; - tim->CCR3 = currentSpeed / 2; - } else if (isTurningLeft) { - // Tourne à gauche en avançant - tim->CCR2 = currentSpeed / 2; - tim->CCR3 = currentSpeed; - } else { - // Mouvements standards - if (isAccelerating) { - tim->CCR2 = currentSpeed; - tim->CCR3 = currentSpeed; - } else if (isReversing) { - tim->CCR1 = currentSpeed; - tim->CCR4 = currentSpeed; - } - } - //Moteurs arrêtés - if(targetSpeed == 0 && currentSpeed == 0) { - isAccelerating = 0; - isReversing = 0; - } + // Moteur droit + if (rightCurrentSpeed < rightTargetSpeed) { + rightCurrentSpeed++; + } else if (rightCurrentSpeed > rightTargetSpeed) { + if (rightCurrentSpeed - 25 >= 0) { + rightCurrentSpeed -= 25; + } else { + rightCurrentSpeed--; + } + } + + // Mise à jour des registres PWM du timer + // CCR2 = moteur gauche, CCR3 = moteur droit + if (isAccelerating || isTurningLeft || isTurningRight) { + tim->CCR2 = leftCurrentSpeed; + tim->CCR3 = rightCurrentSpeed; + } else if (isReversing) { + tim->CCR1 = leftCurrentSpeed; // Remplace CCR2 si tu fais marche arrière par CCR1 + tim->CCR4 = rightCurrentSpeed; // Remplace CCR3 par CCR4 pour l'arrière + } + + // Mise à jour de l'état de mouvement + if (isStopped()) { + isAccelerating = false; + isReversing = false; + } } +