modelec/odo_STM32
1
0
Fork 0
mirror of https://github.com/modelec/odo_STM32.git synced 2026-01-18 16:27:25 +01:00
Code Issues Packages Projects Releases Wiki Activity

correction : pidvitesse ok

Browse Source
This commit is contained in:
Maxime
2025-05-26 17:55:48 +02:00
parent 67ad5f1160
commit e05f0aa689
4 changed files with 28 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Core/Src/CommCallbacks.cpp
View File

@@ -6,7 +6,7 @@
*/ */
/*
#include "CommCallbacks.hpp" #include "CommCallbacks.hpp"
#include "main.h" #include "main.h"
#include "modelec.cpp" #include "modelec.cpp"
@@ -50,3 +50,4 @@ void Comm_AddWaypoint(int id, int type, float x, float y, float theta) {
WaypointManager::getInstance().addWaypoint(wp); WaypointManager::getInstance().addWaypoint(wp);
} }
*/

41
Core/Src/modelec.cpp
View File

@@ -57,16 +57,13 @@ bool isDelayPassed(uint32_t delay) {
} }
//PID //PID
void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition& pid, PidVitesse& pidG, PidVitesse& pidD, float vitGauche, float vitDroit, int cnt){ void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition& pid, PidVitesse& pidG, PidVitesse& pidD, float vitGauche, float vitDroit){
//PidPosition pid(0,0,0,0,0,0,objectifPoint); //PidPosition pid(0,0,0,0,0,0,objectifPoint);
pid.setConsignePositionFinale(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}; std::array<double, 2> vitesse = {0.05,0.05};
if(cnt<18){
vitesse = {0.235, 0.235};
}
char debug_msg[128]; char debug_msg[128];
sprintf(debug_msg, "[CONS] G: %.3f m/s | D: %.3f m/s\r\n", vitesse[0], vitesse[1]); sprintf(debug_msg, "[CONS] G: %.3f m/s | D: %.3f m/s\r\n", vitesse[0], vitesse[1]);
@@ -84,29 +81,31 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPositi
float nouvelOrdreG = pidG.getNouvelleConsigneVitesse(); float nouvelOrdreG = pidG.getNouvelleConsigneVitesse();
float nouvelOrdreD = pidD.getNouvelleConsigneVitesse(); float nouvelOrdreD = pidD.getNouvelleConsigneVitesse();
sprintf(debug_msg, "[CORR] G: %.3f m/s | D: %.3f m/s\r\n", nouvelOrdreG, nouvelOrdreD);
CDC_Transmit_FS((uint8_t*)debug_msg, strlen(debug_msg));
int erreurG = pidG.getPWMCommand(nouvelOrdreG); int erreurG = pidG.getPWMCommand(nouvelOrdreG);
int erreurD = pidD.getPWMCommand(nouvelOrdreD); int erreurD = pidD.getPWMCommand(nouvelOrdreD);
// 1. On récupère l'erreur de vitesse actuelle pour chaque PID // Limite fixe du delta PWM
float erreurVitG = pidG.getErreurVitesse(); const int MAX_ERREUR_PWM = 50;
float erreurVitD = pidD.getErreurVitesse();
// 2. On détermine dynamiquement la limite du delta PWM à appliquer if (erreurG > MAX_ERREUR_PWM)
int maxErreurG = std::min(std::max((int)(fabs(erreurVitG) * 300.0f), 20), 150); erreurG = MAX_ERREUR_PWM;
int maxErreurD = std::min(std::max((int)(fabs(erreurVitD) * 300.0f), 20), 150); else if (erreurG < -MAX_ERREUR_PWM)
erreurG = -MAX_ERREUR_PWM;
// 3. On applique la limite dynamique sur les erreurs de PWM if (erreurD > MAX_ERREUR_PWM)
erreurG = std::min(std::max(erreurG, -maxErreurG), maxErreurG); erreurD = MAX_ERREUR_PWM;
erreurD = std::min(std::max(erreurD, -maxErreurD), maxErreurD); else if (erreurD < -MAX_ERREUR_PWM)
erreurD = -MAX_ERREUR_PWM;
int ordrePWMG = motor.getLeftCurrentPWM() + erreurG; int ordrePWMG = motor.getLeftCurrentPWM() + erreurG;
int ordrePWMD = motor.getRightCurrentPWM() + erreurD; int ordrePWMD = motor.getRightCurrentPWM() + erreurD;
motor.setLeftTargetPWM(ordrePWMG); motor.setLeftTargetPWM(ordrePWMG);
motor.setRightTargetPWM(ordrePWMD); motor.setRightTargetPWM(ordrePWMD);
sprintf(debug_msg, "[CORR] G: %.3f m/s | D: %.3f m/s\r\n", ordrePWMG, ordrePWMD);
CDC_Transmit_FS((uint8_t*)debug_msg, strlen(debug_msg));
} }
@@ -133,8 +132,8 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) {
); );
//*out_pid = new PidPosition(1.2,0.02,0.8,0, 0, 0, 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_pidG = new PidVitesse(0.2, 0.0, 0.01, 0);
*out_pidD = new PidVitesse(0.2, 0.05, 0.01, 0); *out_pidD = new PidVitesse(0.2, 0.0, 0.01, 0);
return; return;
@@ -211,7 +210,6 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) {
//receiveControlParams(); //receiveControlParams();
//GPIOC->ODR ^= (1 << 10); //GPIOC->ODR ^= (1 << 10);
int cnt=0;
//On met à jour toutes les 10ms //On met à jour toutes les 10ms
if (isDelayPassed(10)) { if (isDelayPassed(10)) {
ModelecOdometryUpdate(); ModelecOdometryUpdate();
@@ -221,7 +219,7 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) {
currentPoint.setX(x); currentPoint.setX(x);
currentPoint.setY(y); currentPoint.setY(y);
currentPoint.setTheta(theta); currentPoint.setTheta(theta);
//Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE); Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE);
Point targetPoint(0.50, 0.0,0, StatePoint::FINAL); Point targetPoint(0.50, 0.0,0, StatePoint::FINAL);
char debugMsg[128]; char debugMsg[128];
sprintf(debugMsg, "Speed avant determination : L=%.3f | R=%.3f\r\n", sprintf(debugMsg, "Speed avant determination : L=%.3f | R=%.3f\r\n",
@@ -229,10 +227,9 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) {
CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg)); CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg));
determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed(), cnt); determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed());
//HAL_Delay(1000); //HAL_Delay(1000);
motor.update(); motor.update();
cnt++;

2
Core/Src/pidPosition.cpp
View File

@@ -196,7 +196,7 @@ std::array<double, 2> PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f
sprintf(log, "[SET] VITESSE SORTIE DE PID POS | G: %.3f m/s | D: %.3f m/s\r\n", vitesseGauche, 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)); CDC_Transmit_FS((uint8_t*)log, strlen(log));
if (fabs(erreurAvant) < 0.1 && fabs(erreurLat) < 0.1 && fabs(erreurPosition.getTheta()) < 2) { if (fabs(erreurAvant) < 0.05 && fabs(erreurLat) < 0.05 && fabs(erreurPosition.getTheta()) < 0.5) {
sprintf(log, "[PID] OBJECTIF ATTEINT — Robot à l'arrêt\r\n"); sprintf(log, "[PID] OBJECTIF ATTEINT — Robot à l'arrêt\r\n");
CDC_Transmit_FS((uint8_t*)log, strlen(log)); CDC_Transmit_FS((uint8_t*)log, strlen(log));
return {0.0, 0.0}; return {0.0, 0.0};

24
Core/Src/pidVitesse.cpp
View File

@@ -80,19 +80,14 @@ void PidVitesse::updateErreurVitesse(float vitesseActuelle) {
} }
void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
// === Paramètres de sécurité / stabilisation === // === Paramètres de stabilisation ===
const float zoneMorteErreur = 0.02f; // seuil min pour agir sur l'erreur const float integralMax = 10.0f; // anti-windup
const float zoneMorteSortie = 0.005f; // seuil min pour agir sur la sortie const float deriveeMax = 1.0f; // limitation de la dérivée
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 // Mise à jour de l'erreur de vitesse
this->updateErreurVitesse(vitesseActuelle); this->updateErreurVitesse(vitesseActuelle);
// Zone morte sur l’erreur // Si la consigne est ~0, on neutralise tout
if (fabs(this->erreurVitesse) < zoneMorteErreur) {
this->erreurVitesse = 0.0f;
}
if (fabs(this->consigneVitesseFinale) < 0.001f) { if (fabs(this->consigneVitesseFinale) < 0.001f) {
this->integral = 0.0f; this->integral = 0.0f;
this->derivee = 0.0f; this->derivee = 0.0f;
@@ -100,8 +95,7 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
return; return;
} }
// Calcul du terme dérivé
// Calcul du terme dérivé (bruit possible)
this->derivee = this->erreurVitesse - this->erreurVitesse_old; this->derivee = this->erreurVitesse - this->erreurVitesse_old;
// Limitation de la dérivée (anti-pics) // Limitation de la dérivée (anti-pics)
@@ -120,13 +114,6 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
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 // Application de la commande
this->nouvelleConsigneVitesse = pid; this->nouvelleConsigneVitesse = pid;
@@ -143,3 +130,4 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
// CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer)); // CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer));
} }