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

refactoring everything

Browse Source
This commit is contained in:
acki
2025-09-20 13:58:20 +02:00
parent e3a205314f
commit 830cc89558
15 changed files with 302 additions and 1085 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
Core/Inc/modelec.h
View File

@@ -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 <cstdio>
#include <cstring>
#include <math.h>
#include <algorithm>
#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
}

45
Core/Inc/motors.h
View File

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

46
Core/Inc/pid.h
View File

@@ -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 <cmath>
#include <array>
/*
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_ */

56
Core/Inc/pidPosition.h
View File

@@ -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<float, 2> getD();
std::array<float, 2> getI();
// Méthodes spécifiques à la position
Point calculErreurPosition(Point p);
void updateErreurPosition(Point pointActuel);
std::array<double, 2> updateNouvelOrdreVitesse(Point pointActuel, float vitGauche, float vitDroit);
// Surcharge des méthodes virtuelles
void updateErreur();
void updateNouvelleConsigne();
};
#endif /* INC_PID_POSITION_H_ */

40
Core/Inc/pidVitesse.h
View File

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

7
Core/Inc/point.h
View File

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

24
Core/Inc/setup.h Normal file
View File

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

33
Core/Src/CommCallbacks.cpp
View File

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

28
Core/Src/main.c
View File

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

365
Core/Src/modelec.cpp
View File

@@ -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<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel, vitGauche, vitDroit);
//std::array<double, 2> 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<float>(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<float>(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<int16_t>(pwmLeft);
motor.rightTarget_PWM = static_cast<int16_t>(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<PidPosition*>(pid);
PidVitesse* pidVitesseG = static_cast<PidVitesse*>(pidG);
PidVitesse* pidVitesseD = static_cast<PidVitesse*>(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

190
Core/Src/motors.cpp
View File

@@ -3,167 +3,47 @@
#include <cstdio>
#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<uint16_t>(leftCurrent_PWM);
TIM8->CCR2 = 0;
} else {
TIM8->CCR2 = static_cast<uint16_t>(-leftCurrent_PWM);
TIM8->CCR1 = 0;
}
// Contrôle moteur A - TIM8 CH1/CH2
if (this->leftCurrent_PWM >= 0) {
TIM8->CCR1 = static_cast<uint16_t>(this->leftCurrent_PWM); // IN1A (avant)
TIM8->CCR2 = 0; // IN2A
} else {
TIM8->CCR2 = static_cast<uint16_t>(-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<uint16_t>(this->rightCurrent_PWM); // IN1B (avant)
TIM1->CCR2 = 0; // IN2B
} else {
TIM1->CCR2 = static_cast<uint16_t>(-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<uint16_t>(rightCurrent_PWM);
TIM1->CCR2 = 0;
} else {
TIM1->CCR2 = static_cast<uint16_t>(-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;
}
}

95
Core/Src/pid.cpp
View File

@@ -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 <cstdio>
#include <cstring>
//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;
}

223
Core/Src/pidPosition.cpp
View File

@@ -1,223 +0,0 @@
#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;
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<float, 2> PidPosition::getD() {
return {this->d[0], this->d[1]};
}
std::array<float, 2> 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<double, 2> 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};
}

132
Core/Src/pidVitesse.cpp
View File

@@ -1,132 +0,0 @@
#include "pidVitesse.h"
#include <cstring>
#include <cstdio>
#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));
}

28
Core/Src/setup.cpp Normal file
View File

@@ -0,0 +1,28 @@
/*
* setup.cpp
*
* Created on: Sep 18, 2025
* Author: guich
*/
#include <setup.h>
#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