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

communication et pid ok

Browse Source
This commit is contained in:
Maxime
2025-05-29 03:38:56 +02:00
parent 1f9cec27dd
commit e3a205314f
11 changed files with 179 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Inc/CommCallbacks.hpp → Core/Inc/CommCallbacks.h
View File

@@ -1,6 +1,8 @@
#ifndef COMM_CALLBACKS_HPP
#define COMM_CALLBACKS_HPP
#include <stdbool.h>
#ifdef __cplusplus
extern "C" {
#endif

7
Core/Inc/commSTM.h
View File

@@ -13,11 +13,18 @@
#include <stddef.h>
#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_ */

14
Core/Inc/modelec.h
View File

@@ -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

44
Core/Src/CommCallbacks.cpp
View File

@@ -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
}
*/

14
Core/Src/commSTM.cpp
View File

@@ -5,12 +5,12 @@
* Author: maxch
*/
#include <CommCallbacks.h>
#include "commSTM.h"
#include "usbd_cdc.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#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");

4
Core/Src/main.c
View File

@@ -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)

122
Core/Src/modelec.cpp
View File

@@ -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<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit);
std::array<double, 2> vitesse = {0, 0};
if(cnt<18){
vitesse = {0.235, 0.235};
}
std::array<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit);
//std::array<double, 2> 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<PidPosition*>(pid);
PidVitesse* pidVitesseG = static_cast<PidVitesse*>(pidG);
PidVitesse* pidVitesseD = static_cast<PidVitesse*>(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

4
Core/Src/motors.cpp
View File

@@ -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));*/
}

43
Core/Src/pidPosition.cpp
View File

@@ -2,7 +2,10 @@
#include "pidPosition.h"
#include <cstdio>
#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<double, 2> 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<double, 2> 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<double, 2> 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<double, 2> 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<double, 2> 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<double, 2> PidPosition::updateNouvelOrdreVitesse(Point pointActuel, f

23
Core/Src/pidVitesse.cpp
View File

@@ -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;

4
USB_Device/App/usbd_cdc_if.c
View File

@@ -22,6 +22,9 @@
#include "usbd_cdc_if.h"
/* USER CODE BEGIN INCLUDE */
#include "commSTM.h"
#include <stdbool.h>
/* 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);