From 830cc89558ef2389e638468a8cc1ade4c2dd4acd Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 20 Sep 2025 13:58:20 +0200 Subject: [PATCH] refactoring everything --- Core/Inc/modelec.h | 75 ++++---- Core/Inc/motors.h | 45 +---- Core/Inc/pid.h | 46 +---- Core/Inc/pidPosition.h | 56 ------ Core/Inc/pidVitesse.h | 40 ---- Core/Inc/point.h | 7 +- Core/Inc/setup.h | 24 +++ Core/Src/CommCallbacks.cpp | 33 ++-- Core/Src/main.c | 28 +-- Core/Src/modelec.cpp | 365 +++++++++++++------------------------ Core/Src/motors.cpp | 190 ++++--------------- Core/Src/pid.cpp | 95 ++-------- Core/Src/pidPosition.cpp | 223 ---------------------- Core/Src/pidVitesse.cpp | 132 -------------- Core/Src/setup.cpp | 28 +++ 15 files changed, 302 insertions(+), 1085 deletions(-) delete mode 100644 Core/Inc/pidPosition.h delete mode 100644 Core/Inc/pidVitesse.h create mode 100644 Core/Inc/setup.h delete mode 100644 Core/Src/pidPosition.cpp delete mode 100644 Core/Src/pidVitesse.cpp create mode 100644 Core/Src/setup.cpp diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h index d19aad1..506544f 100644 --- a/Core/Inc/modelec.h +++ b/Core/Inc/modelec.h @@ -2,25 +2,21 @@ * modelec.h * * Created on: May 25, 2025 - * Author: maxch + * Author: Modelec */ #ifndef MODELEC_H #define MODELEC_H #include "motors.h" #include "main.h" -//#include "stm32l0xx_hal.h" #include "stm32g4xx_hal.h" -//#include "stm32g4xx_hal_uart.h" #include #include #include #include -#include "pidVitesse.h" #include "pid.h" #include "point.h" -#include "pidPosition.h" #include "CommCallbacks.h" #include "usbd_cdc_if.h" #include "commSTM.h" @@ -29,39 +25,52 @@ extern "C" { #endif -// Prototypes des fonctions accessibles depuis main.cpp ou ailleurs +extern TIM_HandleTypeDef htim3; +extern TIM_HandleTypeDef htim2; -void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD); -//void ModelecOdometryLoop(void* pid, void* pidG, void* pidD); -void ModelecOdometryLoop(void* pid, void* pidG, void* pidD, int* cnt); -void ModelecOdometryUpdate(); +class DiffBot { +public: + Point target; + Point targets[10]; + uint8_t index = 0; + Point pose; -void determinationCoefPosition( - Point objectifPoint, - Point pointActuel, - PidPosition& pid, - PidVitesse& pidG, - PidVitesse& pidD, - float vitGauche, - float vitDroit, - int cnt -); + Motor motor; -// Fonctions utilitaires (optionnellement utilisables ailleurs) -bool isDelayPassedFrom(uint32_t delay, uint32_t *lastTick); -bool isDelayPassed(uint32_t delay); -void stopMotorsStep(); -extern Point targetPoint; + float dt; + PID pidLeft, pidRight; + PID pidPos, pidTheta; -//variables : -extern Point currentPoint; -extern float vitesseLineaire; -extern float vitesseAngulaire; -extern float vitesseLeft; -extern float vitesseRight; -extern bool odo_active; -extern bool arrive; + int32_t prevCountLeft = 0; + int32_t prevCountRight = 0; + + bool odo_active = false; + bool arrive = false; + + int16_t PWM_MAX = 626; + float ENCODER_RES = 2048; + float WHEEL_RADIUS = 0.0405f; + float WHEEL_BASE = 0.287f; + float WHEEL_BASE_2 = 0.1435f; + + float readEncoderRight(); + + float readEncoderLeft(); + + DiffBot(Point pose, float dt); + + void stop(bool stop); + + void setup(); + + void setTarget(Point new_target); + + void update(float dt); + + void addTarget(int id, int type, float x, float y, float theta); + +}; #ifdef __cplusplus } diff --git a/Core/Inc/motors.h b/Core/Inc/motors.h index ab22555..220d6b0 100644 --- a/Core/Inc/motors.h +++ b/Core/Inc/motors.h @@ -4,51 +4,14 @@ //#include "stm32l0xx_hal.h" #include "stm32g4xx_hal.h" - class Motor { -private: - TIM_TypeDef *tim; - float rightCurrentSpeed; - float leftCurrentSpeed; - float rightTargetSpeed; - float leftTargetSpeed; - bool isAccelerating; - bool isReversing; - bool isTurningRight; - bool isTurningLeft; - int32_t rightCurrent_PWM; - int32_t leftCurrent_PWM; - int32_t rightTarget_PWM; - int32_t leftTarget_PWM; - public: - Motor(TIM_TypeDef *timer); - void setRightTargetSpeed(int pwm); - void setLeftTargetSpeed(int pwm); - void setRightCurrentSpeed(float vitesse); - void setLeftCurrentSpeed(float vitesse); - float getRightCurrentSpeed(); - float getLeftCurrentSpeed(); + int16_t leftTarget_PWM = 0, rightTarget_PWM = 0; + int16_t leftCurrent_PWM = 0, rightCurrent_PWM = 0; + bool bStop; - void setRightCurrentPWM(int32_t pwm); - void setLeftCurrentPWM(int32_t pwm); - void setRightTargetPWM(int32_t pwm); - void setLeftTargetPWM(int32_t pwm); - - int32_t getRightCurrentPWM() const; - int32_t getLeftCurrentPWM() const; - int32_t getRightTargetPWM() const; - int32_t getLeftTargetPWM() const; - - void accelerer(int speed); - void reculer(int speed); - void stop(); - void stopTurning(); - bool isStopped(); - void tournerDroite(int speed); - void tournerGauche(int speed); void update(); - + void stop(bool stop); }; diff --git a/Core/Inc/pid.h b/Core/Inc/pid.h index ca1610f..95b6895 100644 --- a/Core/Inc/pid.h +++ b/Core/Inc/pid.h @@ -2,56 +2,26 @@ * pid.h * * Created on: Mar 25, 2025 - * Author: CHAUVEAU Maxime + * Author: Modelec */ #ifndef INC_PID_H_ #define INC_PID_H_ #include "point.h" -#include -#include -/* - Pour notre robot, on va faire 3 PID. On va avoir un premier PID qui prend une position et qui - doit donner en sortie deux ordres pour les moteurs (un moteur gauche et un moteur droit). - Ainsi on aura aussi un PID par moteur qui eux recevront la consigne du pid position, donc 3 en tout. - - Le premier PID (position) prend en entrée un Point de la carte (objectif) et renvoie en sortie un tableau de 2 val en m/s. - les pid vitesse de chaque moteurs prennent en entrée une vitesse à atteindre et en sortie une consigne à envoyer au moteur (qu'il faudra traduire en pwm). - */ - -class Pid { +class PID { protected: - float Vmax; //vitesse linéaire max du robot (mps) - float Wmax; //vitesse angulaire max du robot (mps) - float L; //Largeur entre les deux roues (en mètre) - - float kp; - float ki; - float kd; - - - + float kp, ki, kd; + float integral = 0.0f; + float prevError = 0.0f; + float outMin, outMax; public: - // Constructeur - Pid(float kp, float ki, float kd); + PID(float kp = 0.0f, float ki = 0.0f, float kd = 0.0f, float outMin = 0.0f, float outMax = 0.0f); - // Setters - void setKp(float k); - void setKi(float k); - void setKd(float k); - - // Getters - float getKp(); - float getKi(); - float getKd(); - - //Methodes - int getPWMCommand(float vitesse); //convertir des m/s en notre signal pwm + float compute(float setpoint, float measurement); }; - #endif /* INC_PID_H_ */ diff --git a/Core/Inc/pidPosition.h b/Core/Inc/pidPosition.h deleted file mode 100644 index b04d88c..0000000 --- a/Core/Inc/pidPosition.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef INC_PID_POSITION_H_ -#define INC_PID_POSITION_H_ - -#include "pid.h" - -class PidPosition : public Pid { -private: - Point erreurPosition, erreurPosition_old; - Point consignePositionFinale; - - float d[3] = {0.0f, 0.0f, 0.0f}; //valeurs calculé dans les méthodes update (position) - float i[3] = {0.0f, 0.0f, 0.0f}; - - float Kp_theta, Ki_theta, Kd_theta; - -public: - // Constructeur - PidPosition(float kp, float ki, float kd, - float Kp_theta, float Ki_theta, float Kd_theta, - Point consignePosition); - - // Setters - void setConsignePositionFinale(Point consigne); - void setErreurPosition(Point erreur); - void setErreurPosition_old(Point erreur_old); - - void setD(float d[2]); - void setI(float d[2]); - - void setKpTheta(float kp); - void setKiTheta(float ki); - void setKdTheta(float kd); - - // Getters - Point getConsignePositionFinale(); - Point getErreurPosition(); - Point getErreurPosition_old(); - - float getKpTheta(); - float getKiTheta(); - float getKdTheta(); - - std::array getD(); - std::array getI(); - - // Méthodes spécifiques à la position - Point calculErreurPosition(Point p); - void updateErreurPosition(Point pointActuel); - std::array updateNouvelOrdreVitesse(Point pointActuel, float vitGauche, float vitDroit); - - // Surcharge des méthodes virtuelles - void updateErreur(); - void updateNouvelleConsigne(); -}; - -#endif /* INC_PID_POSITION_H_ */ diff --git a/Core/Inc/pidVitesse.h b/Core/Inc/pidVitesse.h deleted file mode 100644 index 25070a5..0000000 --- a/Core/Inc/pidVitesse.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef INC_PIDVITESSE_H_ -#define INC_PIDVITESSE_H_ - -#include "pid.h" - -class PidVitesse : public Pid { -private: - float erreurVitesse, erreurVitesse_old; - float consigneVitesseFinale; - float derivee, integral; - float nouvelleConsigneVitesse; - -public: - // Constructeur - PidVitesse(float kp, float ki, float kd, float consigneVitesseFinale); - - // Setters - void setErreurVitesse(float e); - void setErreurVitesse_old(float e_old); - void setConsigneVitesseFinale(float vf); - void setNouvelleConsigneVitesse(float v); - void setDerivee(float d); - void setIntegral(float i); - - // Getters - float getErreurVitesse(); - float getErreurVitesse_old(); - float getConsigneVitesseFinale(); - float getNouvelleConsigneVitesse(); - float getDerivee(); - float getIntegral(); - - // Méthodes spécifiques au PID vitesse - float calculErreurVitesse(float vitesseActuelle); - void updateErreurVitesse(float vitesseActuelle); - void updateNouvelleVitesse(float vitesseActuelle); - -}; - -#endif /* INC_PIDVITESSE_H_ */ diff --git a/Core/Inc/point.h b/Core/Inc/point.h index eccb2cc..184f5ab 100644 --- a/Core/Inc/point.h +++ b/Core/Inc/point.h @@ -19,13 +19,14 @@ typedef enum StatePoint { class Point { private: - float x; - float y; - float theta; uint32_t id; StatePoint state; public: + float x; + float y; + float theta; + // Constructeur Point(float x = 0.0, float y = 0.0, float theta = 0.0, StatePoint state = StatePoint::INTERMEDIAIRE); diff --git a/Core/Inc/setup.h b/Core/Inc/setup.h new file mode 100644 index 0000000..0801fce --- /dev/null +++ b/Core/Inc/setup.h @@ -0,0 +1,24 @@ +/* + * setup.h + * + * Created on: Sep 18, 2025 + * Author: Modelec + */ + +#ifndef INC_SETUP_H_ +#define INC_SETUP_H_ + +#include "modelec.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void ModelecOdometrySetup(); +void ModelecOdometryLoop(float dt); + +#ifdef __cplusplus +} +#endif + +#endif /* INC_SETUP_H_ */ diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index f1d2bcb..043d97b 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -12,49 +12,38 @@ #include "modelec.h" #include "motors.h" #include "pid.h" -#include "pidPosition.h" + +extern DiffBot bot; void Comm_GetPosition(float* x, float* y, float* theta) { - *x = currentPoint.getX() * 1000.0f; // convertit 0.7 m en 700.0 mm - *y = currentPoint.getY() * 1000.0f; - *theta = currentPoint.getTheta(); + *x = bot.pose.x; + *y = bot.pose.y; + *theta = bot.pose.theta; } void Comm_SetPosition(float x, float y, float theta) { - currentPoint.setX(x / 1000.0f); // mm → m - currentPoint.setY(y / 1000.0f); // mm → m - currentPoint.setTheta(theta); + bot.pose.x = x; + bot.pose.y = y; + bot.pose.theta = theta; } void Comm_GetSpeed(float* vx, float* vy, float* omega){ - *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_SetPID(float p, float i, float d) { - //PIDController::getInstance().setCoefficients(p, i, d); } void Comm_StartOdometry(bool on) { - odo_active = on; - - - + bot.stop(!on); } - void Comm_AddWaypoint(int id, int type, float x, float y, float theta) { - targetPoint.setX(x / 1000.0f); // conversion mm → m - targetPoint.setY(y / 1000.0f); - targetPoint.setTheta(theta); - arrive = false; + bot.addTarget(id, type, x / 1000.0f, y / 1000.0f, theta); } float Comm_GetDistance(int n) { - //return 0.0f; // TODO: remplacer par la bonne logique + return 0.0f; } diff --git a/Core/Src/main.c b/Core/Src/main.c index d22e4b4..a51870f 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -35,10 +35,8 @@ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ -void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD); - -//void ModelecOdometryLoop(void* pid, void* pidG, void* pidD); - +void ModelecOdometrySetup(); +void ModelecOdometryLoop(float dt); /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ @@ -49,9 +47,6 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD); /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ -int counter1=0; -int counter2=0; -int cnt=0; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ @@ -109,24 +104,21 @@ int main(void) HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); - void *pid; - void *pidG; - void *pidD; - ModelecOdometrySetup(&pid, &pidG, &pidD); - + ModelecOdometrySetup(); /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ - //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) { - counter2 = __HAL_TIM_GET_COUNTER(&htim2); - counter1 = __HAL_TIM_GET_COUNTER(&htim3); - ModelecOdometryLoop(pid, pidG, pidD, &cnt); + + float delay = 0.01f; + + ModelecOdometryLoop(delay); + + HAL_Delay(delay * 1000); /* USER CODE END WHILE */ diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 86eba2c..5d1ac6f 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -1,117 +1,14 @@ #include "modelec.h" -#ifdef __cplusplus -extern "C" { -#endif - -extern TIM_HandleTypeDef htim3; -extern TIM_HandleTypeDef htim2; -//extern TIM_HandleTypeDef htim21; -//extern UART_HandleTypeDef huart2; - // Variables globales // Constants -#define COUNTS_PER_REV 2400.0f // 600 PPR × 4 -#define WHEEL_DIAMETER 0.081f // meters -#define WHEEL_BASE 0.287f // meters -#define WHEEL_CIRCUMFERENCE (M_PI * WHEEL_DIAMETER) +// #define COUNTS_PER_REV 2400.0f // 600 PPR × 4 +// #define WHEEL_DIAMETER 0.081f // meters +// #define WHEEL_BASE 0.287f // meters +// #define WHEEL_CIRCUMFERENCE (M_PI * WHEEL_DIAMETER) -// Contrôle des moteurs -Motor motor(TIM2); - -// Données odométriques -uint16_t lastPosRight, lastPosLeft; -// x et y sont en mètres -float x, y, theta; - -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; - -bool isDelayPassedFrom(uint32_t delay, uint32_t *lastTick) { - if (HAL_GetTick() - *lastTick >= delay) { - *lastTick = HAL_GetTick(); - return true; - } - return false; -} -bool isDelayPassed(uint32_t delay) { - return isDelayPassedFrom(delay, &lastTick); -} - -//PID -void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPosition& pid, PidVitesse& pidG, PidVitesse& pidD, float vitGauche, float vitDroit, int cnt){ - //PidPosition pid(0,0,0,0,0,0,objectifPoint); - - - pid.setConsignePositionFinale(objectifPoint); - std::array vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit); - //std::array vitesse = {0, 0}; - - /*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));*/ - - pidG.setConsigneVitesseFinale(vitesse[0]); - pidD.setConsigneVitesseFinale(vitesse[1]); - - - pidG.updateNouvelleVitesse(motor.getLeftCurrentSpeed()); - pidD.updateNouvelleVitesse(motor.getRightCurrentSpeed()); - - - - 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));*/ - - int erreurG = pidG.getPWMCommand(nouvelOrdreG); - int erreurD = pidD.getPWMCommand(nouvelOrdreD); - - - const int MAX_ERREUR_PWM = 50; - - 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; - - motor.setLeftTargetPWM(ordrePWMG); - motor.setRightTargetPWM(ordrePWMD); - - -} -//Odométrie - -void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) { - CDC_Transmit_FS((uint8_t*)"SETUP COMPLETE\n", strlen("SETUP COMPLETE\n")); - lastPosRight = __HAL_TIM_GET_COUNTER(&htim2); - lastPosLeft = __HAL_TIM_GET_COUNTER(&htim3); - x = 0.0f; - y = 0.0f; - theta = 0.0f; - //motor.accelerer(300); - - *out_pid = new PidPosition( + /**out_pid = new PidPosition( 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é @@ -119,153 +16,137 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) { 0.5, // kpTheta — peut rester soft pour éviter les oscillations d’orientation 0.0, // kiTheta 0.15, // kdTheta — un peu moins de frein sur la rotation + 2, Point() - ); + );*/ - //*out_pid = new PidPosition(1.2,0.02,0.8,0, 0, 0, Point()); - *out_pidG = new PidVitesse(0.2, 0.0, 0.01, 0); - *out_pidD = new PidVitesse(0.2, 0.0, 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; +float DiffBot::readEncoderLeft() { + int32_t count = __HAL_TIM_GET_COUNTER(&htim3); + int32_t diff = count - prevCountRight; + prevCountRight = count; + float revs = static_cast(diff) / ENCODER_RES; + return (2*M_PI*WHEEL_RADIUS*revs); // m } -void stopMotorsStep() { - const uint16_t step = 200; +float DiffBot::readEncoderRight() { + int32_t count = __HAL_TIM_GET_COUNTER(&htim2); + int32_t diff = count - prevCountLeft; + prevCountLeft = count; + float revs = static_cast(diff) / ENCODER_RES; + return (2*M_PI*WHEEL_RADIUS*revs); // m +} - // 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; +void DiffBot::setup() { + pidLeft = PID(0.2, 0.0, 0.01, -PWM_MAX, PWM_MAX); + pidRight = PID(0.2, 0.0, 0.01, -PWM_MAX, PWM_MAX); + pidPos = PID(0.8, 0.0, 0.01, -2, 2); + pidTheta = PID(0.5, 0.0, 0.01, -M_PI_2, M_PI_2); +} + +void DiffBot::setTarget(Point new_target) { + target = new_target; +} + +void DiffBot::stop(bool stop) { + odo_active = !stop; + motor.stop(stop); +} + +void DiffBot::update(float dt) { + // read encoder + float leftVel = readEncoderLeft(); + float rightVel = readEncoderRight(); + + // update pos + float v = (rightVel + leftVel) / 2.0f; + float w = (rightVel - leftVel) / WHEEL_BASE; + pose.x += v * cosf(pose.theta); + pose.y += v * sinf(pose.theta); + pose.theta += w; + + // pid setup + float dx = targets[index].x - pose.x; + float dy = targets[index].y - pose.y; + float distError = sqrtf(dx*dx + dy*dy); + float angleTarget = atan2f(dy, dx); + float angleError = angleTarget - pose.theta; + + while (angleError > M_PI) angleError -= 2*M_PI; + while (angleError < -M_PI) angleError += 2*M_PI; + + + /* CHECK IF ON POS THERE + * IF final target check if every things is on purpose like x, y, theta + * IF not final target check if x AND y are close and if so index++ + */ + + switch (targets[index].getState()) { + case StatePoint::FINAL: + + if (fabs(dx) < 0.005 && fabs(dy) < 0.005 && fabs(angleError) < 0.08 /* 5deg */) { + stop(true); + + char log[128]; + sprintf(log, "SET;WAYPOINT;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, strlen(log)); + } + + break; + case StatePoint::INTERMEDIAIRE: + + if (fabs(dx) < 0.05 && fabs(dy) < 0.05) { + index++; + + dx = targets[index].x - pose.x; + dy = targets[index].y - pose.y; + distError = sqrtf(dx*dx + dy*dy); + angleTarget = atan2f(dy, dx); + angleError = angleTarget - pose.theta; + + while (angleError > M_PI) angleError -= 2*M_PI; + while (angleError < -M_PI) angleError += 2*M_PI; + + char log[128]; + sprintf(log, "SET;WAYPOINT;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, strlen(log)); + } + + break; + default: + break; } - // 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; - } + float vRef = pidPos.compute(0.0, -distError); + float wRef = pidTheta.compute(targets[index].theta, pose.theta) + 2.0f * angleError; + + float vLeft = vRef - (WHEEL_BASE_2) * wRef; + float vRight = vRef + (WHEEL_BASE_2) * wRef; + + float pwmLeft = pidLeft.compute(vLeft, leftVel); + float pwmRight = pidRight.compute(vRight, rightVel); + + motor.leftTarget_PWM = static_cast(pwmLeft); + motor.rightTarget_PWM = static_cast(pwmRight); + motor.update(); } +DiffBot::DiffBot(Point pose, float dt) : pose(pose), dt(dt) { +}; -void ModelecOdometryUpdate() { - //On récupère la valeur des compteurs - uint16_t posRight = __HAL_TIM_GET_COUNTER(&htim2); - uint16_t posLeft = __HAL_TIM_GET_COUNTER(&htim3); +void DiffBot::addTarget(int id, int type, float x, float y, float theta) { + targets[id].setX(x); + targets[id].setY(y); + targets[id].setTheta(theta); + targets[id].setID(id); + targets[id].setState(type == 1 ? StatePoint::FINAL : StatePoint::INTERMEDIAIRE); - //On calcule les deltas - int16_t deltaLeft = (int16_t) (posLeft - lastPosLeft); - int16_t deltaRight = (int16_t) (posRight - lastPosRight); + // if (type == StatePoint::FINAL) index = 0; + index = 0; - //On met à jour la dernière position - lastPosLeft = posLeft; - lastPosRight = posRight; - - //On convertit en distance (mètres) - float distLeft = (deltaLeft / COUNTS_PER_REV) * WHEEL_CIRCUMFERENCE; - float distRight = (deltaRight / COUNTS_PER_REV) * WHEEL_CIRCUMFERENCE; - - //On calcule les déplacements - float linear = (distLeft + distRight) / 2.0f; - float deltaTheta = (distRight - distLeft) / WHEEL_BASE; - - //On met à jour la position - float avgTheta = theta + deltaTheta / 2.0f; - x += linear * cosf(avgTheta); - y += linear * sinf(avgTheta); - theta += deltaTheta; - - //On normalise theta - theta = fmodf(theta, 2.0f * M_PI); - 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)); - float dt = 0.01f; // 10 ms - - // Calcul des vitesses des roues - vitesseLeft = distLeft / dt; - vitesseRight = distRight / dt; - - // Vitesse linéaire et angulaire du robot - vitesseLineaire = (vitesseLeft + vitesseRight) / 2.0f; - 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); - //CDC_Transmit_FS((uint8_t*) msg, strlen(msg)); - - //motor.setLeftCurrentSpeed(vitesseLeft); - //motor.setRightCurrentSpeed(vitesseRight); - motor.setLeftCurrentSpeed(vitesseLeft); - motor.setRightCurrentSpeed(vitesseRight); + arrive = false; } - -void publishStatus(){ - -} - -void receiveControlParams(){ - -} - -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); - - //On met à jour toutes les 10ms - if (isDelayPassed(10)) { - ModelecOdometryUpdate(); - USB_Comm_Process(); - - //HAL_Delay(1000); - currentPoint.setX(x); - currentPoint.setY(y); - currentPoint.setTheta(theta); - //Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE); - - if(odo_active == 1){ - - - //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)++; - - - - - } - - publishStatus(); -} - -#ifdef __cplusplus -} //extern C end -#endif diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp index 0f1d121..52eb8c6 100644 --- a/Core/Src/motors.cpp +++ b/Core/Src/motors.cpp @@ -3,167 +3,47 @@ #include #include "usbd_cdc_if.h" -Motor::Motor(TIM_TypeDef *timer) : - tim(timer), rightCurrentSpeed(0),leftCurrentSpeed(0), rightTargetSpeed(0),leftTargetSpeed(0), isAccelerating(false), isReversing( - false), isTurningRight(false), isTurningLeft(false) { -} - -void Motor::accelerer(int speed) { - speed = (speed <= 626) ? speed : 626; - rightTargetSpeed = speed; - leftTargetSpeed = speed; - isAccelerating = true; - isReversing = false; - this->stopTurning(); -} - -void Motor::reculer(int speed) { - speed = (speed <= 626) ? speed : 626; - rightTargetSpeed = speed; - leftTargetSpeed = speed; - isReversing = true; - isAccelerating = false; - this->stopTurning(); -} - -void Motor::setRightTargetSpeed(int pwm){ - if(pwm < 626){ - this->rightTargetSpeed = pwm; - }else{ - this->rightTargetSpeed = 626; - } - -} -void Motor::setLeftTargetSpeed(int pwm){ - if(pwm < 626){ - this->leftTargetSpeed = pwm; - }else{ - this->leftTargetSpeed = 626; - } - -} - - -void Motor::setLeftCurrentSpeed(float vitesse){ - this->leftCurrentSpeed = vitesse; -} - -void Motor::setRightCurrentSpeed(float vitesse){ - this->rightCurrentSpeed = vitesse; -} -float Motor::getRightCurrentSpeed(){ - return this->rightCurrentSpeed; -} -float Motor::getLeftCurrentSpeed(){ - return this->leftCurrentSpeed; -} - -void Motor::setRightCurrentPWM(int32_t pwm) { - this->rightCurrent_PWM = pwm; -} - -void Motor::setLeftCurrentPWM(int32_t pwm) { - this->leftCurrent_PWM = pwm; -} - -void Motor::setRightTargetPWM(int32_t pwm) { - this->rightTarget_PWM = pwm; -} - -void Motor::setLeftTargetPWM(int32_t pwm) { - this->leftTarget_PWM = pwm; -} - -int32_t Motor::getRightCurrentPWM() const { - return this->rightCurrent_PWM; -} - -int32_t Motor::getLeftCurrentPWM() const { - return this->leftCurrent_PWM; -} - -int32_t Motor::getRightTargetPWM() const { - return this->rightTarget_PWM; -} - -int32_t Motor::getLeftTargetPWM() const { - return this->leftTarget_PWM; -} - - -void Motor::stop() { - rightTargetSpeed = 0; - leftTargetSpeed = 0; - this->stopTurning(); -} - -bool Motor::isStopped() { - return rightCurrentSpeed == 0 && leftCurrentSpeed == 0 && rightTargetSpeed == 0 && leftTargetSpeed == 0; -} - -void Motor::tournerDroite(int speed) { - speed = (speed <= 626) ? speed : 626; - rightTargetSpeed = speed / 2; - leftTargetSpeed = speed; - isTurningRight = true; - isTurningLeft = false; - isAccelerating = true; -} - -void Motor::tournerGauche(int speed) { - speed = (speed <= 626) ? speed : 626; - rightTargetSpeed = speed; - leftTargetSpeed = speed / 2; - isTurningRight = false; - isTurningLeft = true; - isAccelerating = true; -} -void Motor::stopTurning() { - isTurningLeft = false; - isTurningRight = false; -} - void Motor::update() { - const int16_t PWM_MAX = 626; + if (bStop) return; + int16_t PWM_MAX_M = 626; - if (this->leftTarget_PWM > PWM_MAX) this->leftTarget_PWM = PWM_MAX; - if (this->leftTarget_PWM < -PWM_MAX) this->leftTarget_PWM = -PWM_MAX; + if (leftTarget_PWM > PWM_MAX_M) leftTarget_PWM = PWM_MAX_M; + if (leftTarget_PWM < -PWM_MAX_M) leftTarget_PWM = -PWM_MAX_M; + if (rightTarget_PWM > PWM_MAX_M) rightTarget_PWM = PWM_MAX_M; + if (rightTarget_PWM < -PWM_MAX_M) rightTarget_PWM = -PWM_MAX_M; - if (this->rightTarget_PWM > PWM_MAX) this->rightTarget_PWM = PWM_MAX; - if (this->rightTarget_PWM < -PWM_MAX) this->rightTarget_PWM = -PWM_MAX; + leftCurrent_PWM = leftTarget_PWM; + rightCurrent_PWM = rightTarget_PWM; - // Appliquer targetSpeed dans currentSpeed - this->leftCurrent_PWM = this->leftTarget_PWM; - this->rightCurrent_PWM = this->rightTarget_PWM; + // moteur gauche -> TIM8 CH1/CH2 + if (leftCurrent_PWM >= 0) { + TIM8->CCR1 = static_cast(leftCurrent_PWM); + TIM8->CCR2 = 0; + } else { + TIM8->CCR2 = static_cast(-leftCurrent_PWM); + TIM8->CCR1 = 0; + } - // Contrôle moteur A - TIM8 CH1/CH2 - if (this->leftCurrent_PWM >= 0) { - TIM8->CCR1 = static_cast(this->leftCurrent_PWM); // IN1A (avant) - TIM8->CCR2 = 0; // IN2A - } else { - TIM8->CCR2 = static_cast(-this->leftCurrent_PWM); // IN2A (arrière) - TIM8->CCR1 = 0; // IN1A - } - - // Contrôle moteur B - TIM1 CH1/CH2 - if (this->rightCurrent_PWM >= 0) { - TIM1->CCR1 = static_cast(this->rightCurrent_PWM); // IN1B (avant) - TIM1->CCR2 = 0; // IN2B - } else { - TIM1->CCR2 = static_cast(-this->rightCurrent_PWM); // IN2B (arrière) - TIM1->CCR1 = 0; // IN1B - } - //[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), - "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));*/ + // moteur droit -> TIM1 CH1/CH2 + if (rightCurrent_PWM >= 0) { + TIM1->CCR1 = static_cast(rightCurrent_PWM); + TIM1->CCR2 = 0; + } else { + TIM1->CCR2 = static_cast(-rightCurrent_PWM); + TIM1->CCR1 = 0; + } } +void Motor::stop(bool stop) { + bStop = stop; + + if (stop) { + TIM1->CCR2 = 0; + TIM1->CCR1 = 0; + + TIM8->CCR1 = 0; + TIM8->CCR2 = 0; + } +} diff --git a/Core/Src/pid.cpp b/Core/Src/pid.cpp index 45fb0b0..98d50d8 100644 --- a/Core/Src/pid.cpp +++ b/Core/Src/pid.cpp @@ -1,89 +1,20 @@ #include "pid.h" -//#include "stm32l0xx_hal.h" // <--- D'abord -#include "stm32g4xx_hal.h" -//#include "stm32g4xx_hal_uart.h" -#include "usbd_cdc_if.h" -//#include "stm32l0xx_hal_uart.h" // optionnel, déjà dans le précédent -#include -#include -//extern UART_HandleTypeDef huart2; -// Constructeur -Pid::Pid(float kp, float ki, float kd){ - this->kp = kp; - this->ki = ki; - this->kd = kd; - - this->Vmax = 0.235; - this->Wmax = 3.11; - this->L = 0.151; +PID::PID(float kp, float ki, float kd, float outMin, float outMax) + : kp(kp), ki(ki), kd(kd), outMin(outMin), outMax(outMax) { } -// Setters -void Pid::setKp(float k){ - this->kp = k; +float PID::compute(float setpoint, float measurement) { + float error = setpoint - measurement; + integral += error; + float derivative = (error - prevError); + float output = kp * error + ki * integral + kd * derivative; + + // saturation + output = std::min(std::max(output, outMin), outMax); + + prevError = error; + return output; } - -void Pid::setKi(float k){ - this->ki = k; -} - -void Pid::setKd(float k){ - this->kp = k; -} - -// Getters -float Pid::getKp(){ - return this->kp; -} - -float Pid::getKi(){ - return this->ki; -} - -float Pid::getKd(){ - return this->kd; -} - - -//Methodes -int Pid::getPWMCommand(float vitesse){ - constexpr float VITESSE_MAX = 0.643f; // 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; - }*/ - int32_t pwm_signed = (vitesse >= 0) ? std::floor(pwm) : -std::floor(pwm); - - char msg[64]; - //sprintf(msg, "Vitesse à atteindre: %.3f m/s, équivalent PWM: %d\r\n", vitesse, pwm_signed); - //CDC_Transmit_FS((uint8_t*)msg, strlen(msg)); - - - // Renvoi avec le bon signe - return pwm_signed; -} - - - - diff --git a/Core/Src/pidPosition.cpp b/Core/Src/pidPosition.cpp deleted file mode 100644 index 0f645bc..0000000 --- a/Core/Src/pidPosition.cpp +++ /dev/null @@ -1,223 +0,0 @@ - -#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; - this->Ki_theta = Ki_theta; - this->Kd_theta = Kd_theta; - - this->erreurPosition = consignePosition; // à l'init, la position est à 0,0,0, donc on a consigne - position actuelle qui vaut juste consigne -} - - - - - -// Setters -void PidPosition::setConsignePositionFinale(Point consigne) { - this->consignePositionFinale = consigne; -} - -void PidPosition::setErreurPosition(Point erreur) { - this->erreurPosition = erreur; -} - -void PidPosition::setErreurPosition_old(Point erreur_old) { - this->erreurPosition_old = erreur_old; -} - -void PidPosition::setD(float d[2]) { - this->d[0] = d[0]; - this->d[1] = d[1]; -} - -void PidPosition::setI(float i[2]) { - this->i[0] = i[0]; - this->i[1] = i[1]; -} - -void PidPosition::setKpTheta(float kp) { - this->Kp_theta = kp; -} - -void PidPosition::setKiTheta(float ki) { - this->Ki_theta = ki; -} - -void PidPosition::setKdTheta(float kd) { - this->Kd_theta = kd; -} - - - - - -// Getters -Point PidPosition::getConsignePositionFinale() { - return this->consignePositionFinale; -} - -Point PidPosition::getErreurPosition() { - return this->erreurPosition; -} - -Point PidPosition::getErreurPosition_old() { - return this->erreurPosition_old; -} - -float PidPosition::getKpTheta() { - return this->Kp_theta; -} - -float PidPosition::getKiTheta() { - return this->Ki_theta; -} - -float PidPosition::getKdTheta() { - return this->Kd_theta; -} - -std::array PidPosition::getD() { - return {this->d[0], this->d[1]}; -} - -std::array PidPosition::getI() { - return {this->i[0], this->i[1]}; -} - - -//Méthodes - -// Mise à jour de l'erreur de position -void PidPosition::updateErreurPosition(Point PointActuel) { - this->erreurPosition_old = erreurPosition; - this->erreurPosition = calculErreurPosition(PointActuel); -} - -// Calcul de l'erreur de position -// Calcul de l'erreur de position -Point PidPosition::calculErreurPosition(Point p) { - return Point( - this->consignePositionFinale.getX() - p.getX(), - this->consignePositionFinale.getY() - p.getY(), - atan2(sin(this->consignePositionFinale.getTheta() - p.getTheta()), - cos(this->consignePositionFinale.getTheta() - p.getTheta())), - StatePoint::NONDETERMINE - ); -} - - -// Mise à jour et calcul du nouvel ordre de vitesse pour les moteurs -std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel, float vitGauche, float vitDroit) { - char log[128]; - this->updateErreurPosition(pointActuel); - - // Affichage position actuelle vs consigne - /*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));*/ - - // Passage repère global -> repère robot - double errX = erreurPosition.getX(); - double errY = erreurPosition.getY(); - double theta = pointActuel.getTheta(); - - double erreurAvant = cos(theta) * errX + sin(theta) * errY; - double erreurLat = -sin(theta) * errX + cos(theta) * errY; - - // Terme intégral - float i1 = this->i[0] + erreurAvant; - 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));*/ - - // Terme dérivé - double errX_old = erreurPosition_old.getX(); - double errY_old = erreurPosition_old.getY(); - - double erreurAvant_old = cos(theta) * errX_old + sin(theta) * errY_old; - double erreurLat_old = -sin(theta) * errX_old + cos(theta) * errY_old; - - double d1 = erreurAvant - erreurAvant_old; - 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));*/ - - double erreurTheta = erreurPosition.getTheta(); - constexpr double seuilTheta = 0.05; // rad ≈ 2.8° - if (fabs(erreurTheta) < seuilTheta) { - erreurTheta = 0.0; - d3 = 0.0; - i3 = 0.0; - } - - // Commandes PID (SANS freinage progressif) - 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));*/ - - // 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));*/ - - // Conversion en vitesse des roues - double vitesseGauche = vitesseLineaire - (this->L / 2) * vitesseAngulaire; - double vitesseDroite = vitesseLineaire + (this->L / 2) * vitesseAngulaire; - - const double tolPosition = 0.01; // 1 cm - const double tolTheta = 0.1; // 0.05 rad ≈ 3° - - if (fabs(erreurAvant) > tolPosition || fabs(erreurLat) > tolPosition || fabs(this->erreurPosition.getTheta()) > tolTheta) { - this->i[0] = i1; - this->i[1] = i2; - this->i[2] = i3; - } - - // Saturation - 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));*/ - - 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}; -} - - - - - - - - - - diff --git a/Core/Src/pidVitesse.cpp b/Core/Src/pidVitesse.cpp deleted file mode 100644 index ce96d87..0000000 --- a/Core/Src/pidVitesse.cpp +++ /dev/null @@ -1,132 +0,0 @@ -#include "pidVitesse.h" -#include -#include -#include "usbd_cdc_if.h" - - -// Constructeur -PidVitesse::PidVitesse(float kp, float ki, float kd, float consigneVitesseFinale) : Pid(kp, ki, kd) { - - this->erreurVitesse = 0; - this->erreurVitesse_old = 0; - - this->consigneVitesseFinale = consigneVitesseFinale; - - this->derivee = 0; - this->integral = 0; - - this->nouvelleConsigneVitesse = 0; -} - -// Setters -void PidVitesse::setErreurVitesse(float e) { - this->erreurVitesse = e; -} - -void PidVitesse::setErreurVitesse_old(float e_old) { - this->erreurVitesse_old = e_old; -} - -void PidVitesse::setConsigneVitesseFinale(float vf) { - this->consigneVitesseFinale = vf; -} - -void PidVitesse::setNouvelleConsigneVitesse(float v) { - this->nouvelleConsigneVitesse = v; -} - -void PidVitesse::setDerivee(float d){ - this->derivee = d; -} - -void PidVitesse::setIntegral(float i){ - this->integral = i; -} - -// Getters -float PidVitesse::getErreurVitesse() { - return this->erreurVitesse; -} - -float PidVitesse::getErreurVitesse_old() { - return this->erreurVitesse_old; -} - -float PidVitesse::getConsigneVitesseFinale() { - return this->consigneVitesseFinale; -} - -float PidVitesse::getNouvelleConsigneVitesse() { - return this->nouvelleConsigneVitesse; -} - -float PidVitesse::getDerivee(){ - return this->derivee; -} - -float PidVitesse::getIntegral(){ - return this->integral; -} - -// Méthodes spécifiques - -float PidVitesse::calculErreurVitesse(float vitesseActuelle) { - return this->consigneVitesseFinale - vitesseActuelle; -} - -void PidVitesse::updateErreurVitesse(float vitesseActuelle) { - this->erreurVitesse_old = this->erreurVitesse; - this->erreurVitesse = this->calculErreurVitesse(vitesseActuelle); -} - -void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { - // === 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); - - // Si la consigne est ~0, on neutralise tout - if (fabs(this->consigneVitesseFinale) < 0.001f) { - this->integral = 0.0f; - this->derivee = 0.0f; - this->nouvelleConsigneVitesse = 0.0f; - return; - } - - // Calcul du terme dérivé - this->derivee = this->erreurVitesse - this->erreurVitesse_old; - - // Limitation de la dérivée (anti-pics) - if (this->derivee > deriveeMax) this->derivee = deriveeMax; - else if (this->derivee < -deriveeMax) this->derivee = -deriveeMax; - - // Mise à jour du terme intégral - this->integral += this->erreurVitesse; - - // Anti-windup sur l'intégrale - if (this->integral > integralMax) this->integral = integralMax; - else if (this->integral < -integralMax) this->integral = -integralMax; - - // Calcul de la commande PID - float pid = this->getKp() * this->erreurVitesse + - this->getKi() * this->integral + - this->getKd() * this->derivee; - - // Application de la commande - this->nouvelleConsigneVitesse = pid; - - // === DEBUG === - char buffer[128]; - snprintf(buffer, sizeof(buffer), - "[PID] Cons: %.3f | Mes: %.3f | Err: %.3f | I: %.3f | D: %.3f | Out: %.3f\r\n", - this->consigneVitesseFinale, - vitesseActuelle, - this->erreurVitesse, - this->integral, - this->derivee, - this->nouvelleConsigneVitesse); - // CDC_Transmit_FS((uint8_t*)buffer, strlen(buffer)); -} - diff --git a/Core/Src/setup.cpp b/Core/Src/setup.cpp new file mode 100644 index 0000000..8c87c41 --- /dev/null +++ b/Core/Src/setup.cpp @@ -0,0 +1,28 @@ +/* + * setup.cpp + * + * Created on: Sep 18, 2025 + * Author: guich + */ + +#include + + +#ifdef __cplusplus +extern "C" { +#endif + +DiffBot bot(Point(0.0f,0.0f,0.0f), 0.01f); + +void ModelecOdometrySetup() { + bot.setup(); +} + +void ModelecOdometryLoop(float dt) { + USB_Comm_Process(); + bot.update(dt); +} + +#ifdef __cplusplus +} //extern C end +#endif