From e3a205314f0c7fc3a43ee2eced230d617d70a1a2 Mon Sep 17 00:00:00 2001 From: Maxime Date: Thu, 29 May 2025 03:38:56 +0200 Subject: [PATCH] communication et pid ok --- .../{CommCallbacks.hpp => CommCallbacks.h} | 2 + Core/Inc/commSTM.h | 7 + Core/Inc/modelec.h | 14 +- Core/Src/CommCallbacks.cpp | 44 ++++--- Core/Src/commSTM.cpp | 14 +- Core/Src/main.c | 4 +- Core/Src/modelec.cpp | 122 ++++++++++++------ Core/Src/motors.cpp | 4 +- Core/Src/pidPosition.cpp | 43 +++--- Core/Src/pidVitesse.cpp | 23 +--- USB_Device/App/usbd_cdc_if.c | 4 + 11 files changed, 179 insertions(+), 102 deletions(-) rename Core/Inc/{CommCallbacks.hpp => CommCallbacks.h} (96%) diff --git a/Core/Inc/CommCallbacks.hpp b/Core/Inc/CommCallbacks.h similarity index 96% rename from Core/Inc/CommCallbacks.hpp rename to Core/Inc/CommCallbacks.h index cb8eaad..e365da9 100644 --- a/Core/Inc/CommCallbacks.hpp +++ b/Core/Inc/CommCallbacks.h @@ -1,6 +1,8 @@ #ifndef COMM_CALLBACKS_HPP #define COMM_CALLBACKS_HPP +#include + #ifdef __cplusplus extern "C" { #endif diff --git a/Core/Inc/commSTM.h b/Core/Inc/commSTM.h index 7e2587a..383335d 100644 --- a/Core/Inc/commSTM.h +++ b/Core/Inc/commSTM.h @@ -13,11 +13,18 @@ #include #include "usbd_cdc_if.h" +#ifdef __cplusplus +extern "C" { +#endif + // À appeler régulièrement pour parser ce qui a été reçu void USB_Comm_Process(void); // À appeler dans CDC_Receive_FS (depuis usbd_cdc_if.c) void USB_Comm_OnReceive(uint8_t* Buf, uint32_t Len); +#ifdef __cplusplus +} +#endif #endif /* INC_COMMSTM_H_ */ diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h index 632cf93..d19aad1 100644 --- a/Core/Inc/modelec.h +++ b/Core/Inc/modelec.h @@ -21,6 +21,7 @@ #include "pid.h" #include "point.h" #include "pidPosition.h" +#include "CommCallbacks.h" #include "usbd_cdc_if.h" #include "commSTM.h" @@ -49,10 +50,21 @@ void determinationCoefPosition( // Fonctions utilitaires (optionnellement utilisables ailleurs) bool isDelayPassedFrom(uint32_t delay, uint32_t *lastTick); bool isDelayPassed(uint32_t delay); +void stopMotorsStep(); +extern Point targetPoint; + + +//variables : +extern Point currentPoint; +extern float vitesseLineaire; +extern float vitesseAngulaire; +extern float vitesseLeft; +extern float vitesseRight; +extern bool odo_active; +extern bool arrive; #ifdef __cplusplus } #endif #endif // MODELEC_H - diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index 1e0c159..f1d2bcb 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -7,7 +7,7 @@ -#include "CommCallbacks.hpp" +#include "CommCallbacks.h" #include "main.h" #include "modelec.h" #include "motors.h" @@ -15,38 +15,46 @@ #include "pidPosition.h" void Comm_GetPosition(float* x, float* y, float* theta) { - //*x = currentPoint.getX(); - //*y = currentPoint.getY(); - //*theta = currentPoint.getTheta(); + *x = currentPoint.getX() * 1000.0f; // convertit 0.7 m en 700.0 mm + *y = currentPoint.getY() * 1000.0f; + *theta = currentPoint.getTheta(); } void Comm_SetPosition(float x, float y, float theta) { - //currentPoint.setX(x); - //currentPoint.setY(y); - //currentPoint.setTheta(theta); + currentPoint.setX(x / 1000.0f); // mm → m + currentPoint.setY(y / 1000.0f); // mm → m + currentPoint.setTheta(theta); } void Comm_GetSpeed(float* vx, float* vy, float* omega){ -// *vx = vitesseLeft; -// *vy = vitesseRight; -// *omega = vitesseAngulaire; + *vx = vitesseLeft * 1000.0f; // m/s → mm/s + *vy = vitesseRight * 1000.0f; // m/s → mm/s + *omega = vitesseAngulaire; } -/*void Comm_GetPID(float* p, float* i, float* d) { - PIDController::getInstance().getCoefficients(*p, *i, *d); +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); + //PIDController::getInstance().setCoefficients(p, i, d); } void Comm_StartOdometry(bool on) { - if (on) Odometry::getInstance().start(); - else Odometry::getInstance().stop(); + odo_active = on; + + + } + void Comm_AddWaypoint(int id, int type, float x, float y, float theta) { - Waypoint wp(id, type, x, y, theta); - WaypointManager::getInstance().addWaypoint(wp); + targetPoint.setX(x / 1000.0f); // conversion mm → m + targetPoint.setY(y / 1000.0f); + targetPoint.setTheta(theta); + arrive = false; +} + +float Comm_GetDistance(int n) { + //return 0.0f; // TODO: remplacer par la bonne logique } -*/ diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp index d7785f5..2657884 100644 --- a/Core/Src/commSTM.cpp +++ b/Core/Src/commSTM.cpp @@ -5,12 +5,12 @@ * Author: maxch */ +#include #include "commSTM.h" #include "usbd_cdc.h" #include #include #include -#include "CommCallbacks.hpp" // ⚠️ fourni par CubeMX dans usb_device.c extern USBD_HandleTypeDef hUsbDeviceFS; @@ -125,9 +125,15 @@ void USB_Comm_Process(void) { 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"); + int val = atoi(strtok(NULL, ";")); + Comm_StartOdometry(val != 0); + + USB_Comm_Send("OK;START\n"); + + // facultatif, si tu veux logguer aussi en debug + char debugMsg[128]; + sprintf(debugMsg, "changement etat : %d\n", val); + CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg)); } else { USB_Comm_Send("KO;UNKNOWN\n"); diff --git a/Core/Src/main.c b/Core/Src/main.c index dd4ef36..d22e4b4 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -21,7 +21,7 @@ #include "tim.h" #include "usb_device.h" #include "gpio.h" -#include "modelec.h" +//#include "modelec.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ @@ -119,7 +119,7 @@ int main(void) /* Infinite loop */ /* USER CODE BEGIN WHILE */ - HAL_Delay(5000); // Attends 5 secondes après le boot + //HAL_Delay(5000); // Attends 5 secondes après le boot char test[] = "Hello from STM32\r\n"; CDC_Transmit_FS((uint8_t*)test, strlen(test)); while (1) diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index f98249d..86eba2c 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -1,7 +1,9 @@ #include "modelec.h" +#ifdef __cplusplus extern "C" { +#endif extern TIM_HandleTypeDef htim3; extern TIM_HandleTypeDef htim2; @@ -23,11 +25,15 @@ uint16_t lastPosRight, lastPosLeft; // x et y sont en mètres float x, y, theta; -Point currentPoint(0, 0,0, StatePoint::INTERMEDIAIRE); +Point currentPoint(0.0, 0.0,0, StatePoint::INTERMEDIAIRE); +Point targetPoint(0.5,0.0, 0, StatePoint::INTERMEDIAIRE); + + float vitesseLineaire; float vitesseAngulaire; float vitesseLeft; float vitesseRight; +bool odo_active = 0; uint32_t lastTick = 0; @@ -48,15 +54,12 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPositi pid.setConsignePositionFinale(objectifPoint); - //std::array vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit); - std::array vitesse = {0, 0}; - if(cnt<18){ - vitesse = {0.235, 0.235}; - } + std::array vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit); + //std::array vitesse = {0, 0}; - 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]); - CDC_Transmit_FS((uint8_t*)debug_msg, strlen(debug_msg)); + CDC_Transmit_FS((uint8_t*)debug_msg, strlen(debug_msg));*/ pidG.setConsigneVitesseFinale(vitesse[0]); pidD.setConsigneVitesseFinale(vitesse[1]); @@ -70,23 +73,24 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPositi float nouvelOrdreG = pidG.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)); + /*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 erreurD = pidD.getPWMCommand(nouvelOrdreD); - // 1. On récupère l'erreur de vitesse actuelle pour chaque PID - float erreurVitG = pidG.getErreurVitesse(); - float erreurVitD = pidD.getErreurVitesse(); - // 2. On détermine dynamiquement la limite du delta PWM à appliquer - int maxErreurG = std::min(std::max((int)(fabs(erreurVitG) * 300.0f), 20), 150); - int maxErreurD = std::min(std::max((int)(fabs(erreurVitD) * 300.0f), 20), 150); + const int MAX_ERREUR_PWM = 50; - // 3. On applique la limite dynamique sur les erreurs de PWM - erreurG = std::min(std::max(erreurG, -maxErreurG), maxErreurG); - erreurD = std::min(std::max(erreurD, -maxErreurD), maxErreurD); + if (erreurG > MAX_ERREUR_PWM) + erreurG = MAX_ERREUR_PWM; + else if (erreurG < -MAX_ERREUR_PWM) + erreurG = -MAX_ERREUR_PWM; + + if (erreurD > MAX_ERREUR_PWM) + erreurD = MAX_ERREUR_PWM; + else if (erreurD < -MAX_ERREUR_PWM) + erreurD = -MAX_ERREUR_PWM; int ordrePWMG = motor.getLeftCurrentPWM() + erreurG; int ordrePWMD = motor.getRightCurrentPWM() + erreurD; @@ -108,24 +112,48 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) { //motor.accelerer(300); *out_pid = new PidPosition( - 0.6, // kp — réduit pour adoucir la réaction - 0.0, // ki — on évite encore pour l’instant - 0.03, // kd — un peu de dérivée pour stabiliser + 0.8, // kp — un poil plus agressif, il pousse plus vers la cible + 0.0, // ki — toujours off pour éviter du dépassement imprévu + 0.015, // kd — un peu moins de freinage anticipé - 0.4, // kpTheta — moins agressif sur la rotation + 0.5, // kpTheta — peut rester soft pour éviter les oscillations d’orientation 0.0, // kiTheta - 0.2, // kdTheta — diminue les surcorrections d'angle + 0.15, // kdTheta — un peu moins de frein sur la rotation 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); + *out_pidG = new PidVitesse(0.2, 0.0, 0.01, 0); + *out_pidD = new PidVitesse(0.2, 0.0, 0.01, 0); return; } +void stopMotorsStep() { + const uint16_t step = 200; + + // TIM8 + if (TIM8->CCR1 > 0) { + TIM8->CCR2 = 0; // sécurité : un seul sens actif + TIM8->CCR1 = (TIM8->CCR1 > step) ? TIM8->CCR1 - step : 0; + } else if (TIM8->CCR2 > 0) { + TIM8->CCR1 = 0; + TIM8->CCR2 = (TIM8->CCR2 > step) ? TIM8->CCR2 - step : 0; + } + + // TIM1 + if (TIM1->CCR1 > 0) { + TIM1->CCR2 = 0; + TIM1->CCR1 = (TIM1->CCR1 > step) ? TIM1->CCR1 - step : 0; + } else if (TIM1->CCR2 > 0) { + TIM1->CCR1 = 0; + TIM1->CCR2 = (TIM1->CCR2 > step) ? TIM1->CCR2 - step : 0; + } +} + + + void ModelecOdometryUpdate() { //On récupère la valeur des compteurs uint16_t posRight = __HAL_TIM_GET_COUNTER(&htim2); @@ -158,9 +186,9 @@ void ModelecOdometryUpdate() { if (theta < 0) theta += 2.0f * M_PI; - char msg[128]; - sprintf(msg, " Update current position : X: %.3f m, Y: %.3f m, Theta: %.3f rad\r\n", x, y, theta); - CDC_Transmit_FS((uint8_t*) msg, strlen(msg)); + //char msg[128]; + //sprintf(msg, " Update current position : X: %.3f m, Y: %.3f m, Theta: %.3f rad\r\n", x, y, theta); + //CDC_Transmit_FS((uint8_t*) msg, strlen(msg)); float dt = 0.01f; // 10 ms // Calcul des vitesses des roues @@ -172,8 +200,8 @@ void ModelecOdometryUpdate() { 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", - vitesseLeft, vitesseRight, vitesseLineaire, vitesseAngulaire); + //sprintf(msg, "Vitesse G: %.3f m/s | D: %.3f m/s | Lin: %.3f m/s | Ang: %.3f rad/s\r\n", + // vitesseLeft, vitesseRight, vitesseLineaire, vitesseAngulaire); //CDC_Transmit_FS((uint8_t*) msg, strlen(msg)); //motor.setLeftCurrentSpeed(vitesseLeft); @@ -194,6 +222,7 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD, int* cnt) { PidPosition* pidPosition = static_cast(pid); PidVitesse* pidVitesseG = static_cast(pidG); PidVitesse* pidVitesseD = static_cast(pidD); + USB_Comm_Process(); //receiveControlParams(); //GPIOC->ODR ^= (1 << 10); @@ -201,24 +230,33 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD, int* cnt) { //On met à jour toutes les 10ms if (isDelayPassed(10)) { ModelecOdometryUpdate(); - //USB_Comm_Process(); + USB_Comm_Process(); //HAL_Delay(1000); 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", - motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed()); - CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg)); + + if(odo_active == 1){ - determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed(), *cnt); - //HAL_Delay(1000); - motor.update(); - (*cnt)++; + //char debugMsg[128]; + //sprintf(debugMsg, "Speed avant determination : L=%.3f | R=%.3f\r\n", + //motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed()); + //CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg)); + + + determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed(), *cnt); + //HAL_Delay(1000); + motor.update(); + + + }else{ + stopMotorsStep(); + } + + //(*cnt)++; @@ -228,4 +266,6 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD, int* cnt) { publishStatus(); } +#ifdef __cplusplus } //extern C end +#endif diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp index c23c65f..0f1d121 100644 --- a/Core/Src/motors.cpp +++ b/Core/Src/motors.cpp @@ -158,12 +158,12 @@ void Motor::update() { //[STM32] PWM_LEFT: 600 | PWM_RIGHT: 600 || TIM8->CCR1: 0 | TIM8->CCR2: 0 M1->CCR1: 600 | TIM1->CCR2: 0 char msg[128]; - snprintf(msg, sizeof(msg), + /*snprintf(msg, sizeof(msg), "PWM_LEFT: %d | PWM_RIGHT: %d || TIM8->CCR1: %lu | TIM8->CCR2: %lu | TIM1->CCR1: %lu | TIM1->CCR2: %lu\r\n", this->leftCurrent_PWM, this->rightCurrent_PWM, (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));*/ } diff --git a/Core/Src/pidPosition.cpp b/Core/Src/pidPosition.cpp index 159cd12..0f645bc 100644 --- a/Core/Src/pidPosition.cpp +++ b/Core/Src/pidPosition.cpp @@ -2,7 +2,10 @@ #include "pidPosition.h" #include #include "usbd_cdc_if.h" +#include "modelec.h" #include "commSTM.h" + +bool arrive = false; PidPosition::PidPosition(float kp, float ki, float kd,float Kp_theta, float Ki_theta, float Kd_theta, Point consignePosition): Pid(kp, ki, kd){ this->Kp_theta = Kp_theta; @@ -116,13 +119,13 @@ std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f this->updateErreurPosition(pointActuel); // Affichage position actuelle vs consigne - sprintf(log, "[DEBUG] Position Actuelle | X: %.3f | Consigne X: %.3f\r\n", pointActuel.getX(), consignePositionFinale.getX()); + /*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 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)); + CDC_Transmit_FS((uint8_t*)log, strlen(log));*/ // Passage repère global -> repère robot double errX = erreurPosition.getX(); @@ -137,8 +140,8 @@ std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f float i2 = this->i[1] + erreurLat; float i3 = this->i[2] + this->erreurPosition.getTheta(); - 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)); + /*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));*/ // Terme dérivé double errX_old = erreurPosition_old.getX(); @@ -151,8 +154,8 @@ std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f 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)); + /*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));*/ double erreurTheta = erreurPosition.getTheta(); constexpr double seuilTheta = 0.05; // rad ≈ 2.8° @@ -166,15 +169,15 @@ std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f 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)); + /*sprintf(log, "[PID] Commandes PID | Avant: %.3f | Theta: %.3f\r\n", commandeAvant, commandeTheta); + CDC_Transmit_FS((uint8_t*)log, strlen(log));*/ // Conversion en vitesse double vitesseLineaire = commandeAvant; double vitesseAngulaire = commandeTheta; - 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 Avant Saturation | Linéaire: %.3f | Angulaire: %.3f\r\n", vitesseLineaire, vitesseAngulaire); + CDC_Transmit_FS((uint8_t*)log, strlen(log));*/ // Conversion en vitesse des roues double vitesseGauche = vitesseLineaire - (this->L / 2) * vitesseAngulaire; @@ -193,13 +196,17 @@ std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f vitesseGauche = std::max(-0.235, std::min(0.235, vitesseGauche)); vitesseDroite = std::max(-0.235, std::min(0.235, 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)); + /*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}; + if (fabs(erreurAvant) < 0.007 && fabs(erreurLat) < 0.007 && fabs(erreurPosition.getTheta()) < 0.5) { + if(!arrive){ + sprintf(log, "SET;WAYPOINT;0\n"); + CDC_Transmit_FS((uint8_t*)log, strlen(log)); + arrive = true; + } + //CDC_Transmit_FS((uint8_t*)log, strlen(log)); + //return {0.0, 0.0}; } return {vitesseGauche, vitesseDroite}; @@ -210,3 +217,7 @@ std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f + + + + diff --git a/Core/Src/pidVitesse.cpp b/Core/Src/pidVitesse.cpp index 366fa58..ce96d87 100644 --- a/Core/Src/pidVitesse.cpp +++ b/Core/Src/pidVitesse.cpp @@ -80,19 +80,14 @@ void PidVitesse::updateErreurVitesse(float vitesseActuelle) { } void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { - // === Paramètres de sécurité / stabilisation === - const float zoneMorteErreur = 0.02f; // seuil min pour agir sur l'erreur - const float zoneMorteSortie = 0.005f; // seuil min pour agir sur la sortie - const float integralMax = 10.0f; // anti-windup - const float deriveeMax = 1.0f; // limitation de la dérivée + // === Paramètres de stabilisation === + 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 this->updateErreurVitesse(vitesseActuelle); - // Zone morte sur l’erreur - if (fabs(this->erreurVitesse) < zoneMorteErreur) { - this->erreurVitesse = 0.0f; - } + // Si la consigne est ~0, on neutralise tout if (fabs(this->consigneVitesseFinale) < 0.001f) { this->integral = 0.0f; this->derivee = 0.0f; @@ -100,8 +95,7 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { return; } - - // Calcul du terme dérivé (bruit possible) + // Calcul du terme dérivé this->derivee = this->erreurVitesse - this->erreurVitesse_old; // Limitation de la dérivée (anti-pics) @@ -120,13 +114,6 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { this->getKi() * this->integral + 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; diff --git a/USB_Device/App/usbd_cdc_if.c b/USB_Device/App/usbd_cdc_if.c index c769c92..ad72bea 100644 --- a/USB_Device/App/usbd_cdc_if.c +++ b/USB_Device/App/usbd_cdc_if.c @@ -22,6 +22,9 @@ #include "usbd_cdc_if.h" /* USER CODE BEGIN INCLUDE */ +#include "commSTM.h" +#include + /* USER CODE END INCLUDE */ @@ -261,6 +264,7 @@ static int8_t CDC_Control_FS(uint8_t cmd, uint8_t* pbuf, uint16_t length) static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len) { /* USER CODE BEGIN 6 */ + USB_Comm_OnReceive(Buf,*Len); USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]); USBD_CDC_ReceivePacket(&hUsbDeviceFS); return (USBD_OK);