normalement le pid marche, du coup faut test de récup les infos des roues codeuses, puis renvoyer dans les moteurs. ce qu'il y a entre les deux pour la vitesse marche (il reste la position)

This commit is contained in:
Maxime
2025-05-11 17:21:13 +02:00
parent 13c3752621
commit cc58aae003
9 changed files with 228 additions and 63 deletions

View File

@@ -8,21 +8,38 @@
class Motor {
private:
TIM_TypeDef *tim;
int16_t rightCurrentSpeed;
int16_t leftCurrentSpeed;
int16_t rightTargetSpeed;
int16_t leftTargetSpeed;
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);
int16_t getRightCurrentSpeed();
int16_t getLeftCurrentSpeed();
void setRightCurrentSpeed(float vitesse);
void setLeftCurrentSpeed(float vitesse);
float getRightCurrentSpeed();
float getLeftCurrentSpeed();
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();

View File

@@ -32,6 +32,9 @@
/* 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);
/* USER CODE END PD */
@@ -67,6 +70,7 @@ static void MX_TIM8_Init(void);
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
@@ -106,26 +110,30 @@ int main(void)
MX_USB_Device_Init();
/* USER CODE BEGIN 2 */
HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); // IN1A
HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2); // IN2A
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); // IN1B
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); // IN2B
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);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
HAL_Delay(3000); // Attends 3 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)
{
ModelecOdometryLoop(pid, pidG, pidD);
/* USER CODE END WHILE */
// LED ON
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_SET);
char msg_on[] = "LED ON\r\n";
while(CDC_Transmit_FS((uint8_t*)msg_on, strlen(msg_on)) == USBD_BUSY);
HAL_Delay(1000);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
char msg_off[] = "LED OFF\r\n";
while(CDC_Transmit_FS((uint8_t*)msg_off, strlen(msg_off)) == USBD_BUSY);
HAL_Delay(1000);
/* USER CODE BEGIN 3 */
}
@@ -460,6 +468,10 @@ static void MX_TIM8_Init(void)
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim8, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
@@ -504,7 +516,7 @@ static void MX_GPIO_Init(void)
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0|GPIO_PIN_6, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET);
@@ -525,8 +537,8 @@ static void MX_GPIO_Init(void)
GPIO_InitStruct.Alternate = GPIO_AF15_EVENTOUT;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pins : PB0 PB6 */
GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_6;
/*Configure GPIO pin : PB0 */
GPIO_InitStruct.Pin = GPIO_PIN_0;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

View File

@@ -3,6 +3,7 @@
//#include "stm32l0xx_hal.h"
#include "stm32g4xx_hal.h"
//#include "stm32g4xx_hal_uart.h"
#include <cstdio>
#include <cstring>
#include <math.h>
@@ -53,24 +54,48 @@ void determinationCoefPosition(Point objectifPoint, Point pointActuel, PidPositi
pid.setConsignePositionFinale(objectifPoint);
std::array<double, 2> vitesse = pid.updateNouvelOrdreVitesse(pointActuel);
//PidVitesse pidG(0, 0, 0, vitesse[0]);
//PidVitesse pidD(0, 0, 0, vitesse[1]);
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();
int ordrePWMG = pidG.getPWMCommand(nouvelOrdreG);
int ordrePWMD = pidD.getPWMCommand(nouvelOrdreD);
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);
motor.setLeftTargetSpeed(ordrePWMG);
motor.setRightTargetSpeed(ordrePWMD);
const int maxErreur = 20;
if (erreurG > maxErreur) {
erreurG = maxErreur;
} else if (erreurG < -maxErreur) {
erreurG = -maxErreur;
}
if (erreurD > maxErreur) {
erreurD = maxErreur;
} else if (erreurD < -maxErreur) {
erreurD = -maxErreur;
}
int ordrePWMG = motor.getLeftCurrentPWM() + erreurG;
int ordrePWMD = motor.getRightCurrentPWM() + erreurD;
motor.setLeftTargetPWM(ordrePWMG);
motor.setRightTargetPWM(ordrePWMD);
}
@@ -86,8 +111,8 @@ void ModelecOdometrySetup(void **out_pid, void **out_pidG, void **out_pidD) {
//motor.accelerer(300);
*out_pid = new PidPosition(0,0,0,0,0,0,Point());
*out_pidG = new PidVitesse(0, 0, 0, 0);
*out_pidD = new PidVitesse(0, 0, 0, 0);
*out_pidG = new PidVitesse(0.2, 0.05, 0.01, 0);
*out_pidD = new PidVitesse(0.2, 0.05, 0.01, 0);
return;
@@ -127,7 +152,26 @@ void ModelecOdometryUpdate() {
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));
//CDC_Transmit_FS((uint8_t*) msg, strlen(msg));
float dt = 0.01f; // 10 ms
// Calcul des vitesses des roues
float vitesseLeft = distLeft / dt;
float vitesseRight = distRight / dt;
// Vitesse linéaire et angulaire du robot
float vitesseLineaire = (vitesseLeft + vitesseRight) / 2.0f;
float 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(motor.getLeftCurrentSpeed()+0.05);
motor.setRightCurrentSpeed(motor.getRightCurrentSpeed()+0.05);
}
void publishStatus(){
@@ -144,16 +188,25 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) {
PidVitesse* pidVitesseD = static_cast<PidVitesse*>(pidD);
//receiveControlParams();
GPIOC->ODR ^= (1 << 10);
//GPIOC->ODR ^= (1 << 10);
//On met à jour toutes les 10ms
if (isDelayPassed(10)) {
ModelecOdometryUpdate();
HAL_Delay(1000);
Point currentPoint(x, y,theta, StatePoint::INTERMEDIAIRE);
Point targetPoint(0.20, 0.20,0, StatePoint::FINAL);
determinationCoefPosition(currentPoint,targetPoint, *pidPosition, *pidVitesseG, *pidVitesseD);
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(currentPoint,targetPoint, *pidPosition, *pidVitesseG, *pidVitesseD);
HAL_Delay(1000);
motor.update();
//motor.update();
}

View File

@@ -1,4 +1,7 @@
#include <motors.h>
#include <cstring>
#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(
@@ -39,13 +42,55 @@ void Motor::setLeftTargetSpeed(int pwm){
}
}
int16_t Motor::getRightCurrentSpeed(){
void Motor::setLeftCurrentSpeed(float vitesse){
this->leftCurrentSpeed = vitesse;
}
void Motor::setRightCurrentSpeed(float vitesse){
this->rightCurrentSpeed = vitesse;
}
float Motor::getRightCurrentSpeed(){
return this->rightCurrentSpeed;
}
int16_t Motor::getLeftCurrentSpeed(){
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;
@@ -81,25 +126,32 @@ void Motor::stopTurning() {
void Motor::update() {
// Appliquer targetSpeed dans currentSpeed
this->leftCurrentSpeed = this->leftTargetSpeed;
this->rightCurrentSpeed = this->rightTargetSpeed;
this->leftCurrent_PWM = this->leftTarget_PWM;
this->rightCurrent_PWM = this->rightTarget_PWM;
// Contrôle moteur A - TIM8 CH1/CH2
if (this->leftCurrentSpeed >= 0) {
TIM8->CCR1 = static_cast<uint16_t>(this->leftCurrentSpeed); // IN1A (avant)
TIM8->CCR2 = 0; // IN2A
} else {
TIM8->CCR2 = static_cast<uint16_t>(-this->leftCurrentSpeed); // IN2A (arrière)
TIM8->CCR1 = 0; // IN1A
}
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->rightCurrentSpeed >= 0) {
TIM1->CCR1 = static_cast<uint16_t>(this->rightCurrentSpeed); // IN1B (avant)
TIM1->CCR2 = 0; // IN2B
} else {
TIM1->CCR2 = static_cast<uint16_t>(-this->rightCurrentSpeed); // IN2B (arrière)
TIM1->CCR1 = 0; // IN1B
}
// 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
}
char msg[128];
snprintf(msg, sizeof(msg),
"TIM8->CCR1: %lu | TIM8->CCR2: %lu | TIM1->CCR1: %lu | TIM1->CCR2: %lu\r\n",
(uint32_t)TIM8->CCR1, (uint32_t)TIM8->CCR2,
(uint32_t)TIM1->CCR1, (uint32_t)TIM1->CCR2);
CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
}

View File

@@ -50,7 +50,7 @@ float Pid::getKd(){
//Methodes
int Pid::getPWMCommand(float vitesse){
constexpr float VITESSE_MAX = 0.235f; // m/s
constexpr float VITESSE_MAX = 0.643f; // m/s
constexpr int PWM_MAX = 626; // commande PWM max
constexpr int PWM_MIN = 30; // zone morte (optionnelle)
@@ -65,16 +65,19 @@ int Pid::getPWMCommand(float vitesse){
// 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){
/*if (pwm < PWM_MIN){
pwm = 0;
}
}*/
int32_t pwm_signed = (vitesse >= 0) ? std::floor(pwm) : -std::floor(pwm);
char msg[64];
// Debug UART ici
sprintf(msg, "Vitesse à atteindre: %.3f m/s, équivalen PWM: %ld\r\n", vitesse, pwm_signed);
CDC_Transmit_FS((uint8_t*)msg, strlen(msg));
//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

View File

@@ -151,5 +151,6 @@ std::array<double, 2> PidPosition::updateNouvelOrdreVitesse(Point pointActuel) {
double vitesseGauche = vitesseLineaire - (this->L / 2) * vitesseAngulaire;
double vitesseDroite = vitesseLineaire + (this->L / 2) * vitesseAngulaire;
return {vitesseGauche, vitesseDroite};
//return {vitesseGauche, vitesseDroite};
return {0.5, 0.5};
}

View File

@@ -1,4 +1,8 @@
#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) {
@@ -82,7 +86,7 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
- integral : on multiplie ki par la somme des erreurs passées
- derivee : on multiplie kd par la variation de l'erreur
*/
//vitesseActuelle = 0.0;
// Mise à jour de l'erreur de vitesse
this->updateErreurVitesse(vitesseActuelle);
@@ -94,7 +98,19 @@ void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) {
this->integral += this->erreurVitesse;
// Calcul de la nouvelle consigne de vitesse avec PID
this->nouvelleConsigneVitesse = this->getKp() * this->erreurVitesse +
float pid = this->getKp() * this->erreurVitesse +
this->getKi() * this->integral +
this->getKd() * this->derivee;
this->nouvelleConsigneVitesse = pid;
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));
}

View File

@@ -304,8 +304,16 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
__HAL_RCC_GPIOB_CLK_ENABLE();
/**TIM8 GPIO Configuration
PB6 ------> TIM8_CH1
PB8-BOOT0 ------> TIM8_CH2
*/
GPIO_InitStruct.Pin = GPIO_PIN_6;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF5_TIM8;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;

View File

@@ -116,7 +116,7 @@ PB4.Signal=S_TIM3_CH1
PB5.Locked=true
PB5.Signal=EVENTOUT
PB6.Locked=true
PB6.Signal=GPIO_Output
PB6.Signal=S_TIM8_CH1
PB7.Locked=true
PB7.Signal=I2C1_SDA
PB8-BOOT0.Locked=true
@@ -153,7 +153,7 @@ ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath=
ProjectManager.UAScriptBeforePath=
ProjectManager.UnderRoot=true
ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_ADC2_Init-ADC2-false-HAL-true,4-MX_TIM2_Init-TIM2-false-HAL-true,5-MX_TIM1_Init-TIM1-false-HAL-true,6-MX_TIM3_Init-TIM3-false-HAL-true,7-MX_TIM8_Init-TIM8-false-HAL-true,8-MX_USB_PCD_Init-USB-false-HAL-true
ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_ADC2_Init-ADC2-false-HAL-true,4-MX_TIM2_Init-TIM2-false-HAL-true,5-MX_TIM1_Init-TIM1-false-HAL-true,6-MX_TIM3_Init-TIM3-false-HAL-true,7-MX_TIM8_Init-TIM8-false-HAL-true,8-MX_USB_Device_Init-USB_DEVICE-false-HAL-false
RCC.AHBFreq_Value=16000000
RCC.APB1Freq_Value=16000000
RCC.APB1TimFreq_Value=16000000
@@ -204,6 +204,8 @@ SH.S_TIM3_CH1.0=TIM3_CH1,Encoder_Interface
SH.S_TIM3_CH1.ConfNb=1
SH.S_TIM3_CH2.0=TIM3_CH2,Encoder_Interface
SH.S_TIM3_CH2.ConfNb=1
SH.S_TIM8_CH1.0=TIM8_CH1,PWM Generation1 CH1
SH.S_TIM8_CH1.ConfNb=1
SH.S_TIM8_CH2.0=TIM8_CH2,PWM Generation2 CH2
SH.S_TIM8_CH2.ConfNb=1
TIM1.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
@@ -212,8 +214,9 @@ TIM1.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
TIM1.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,PeriodNoDither,AutoReloadPreload
TIM1.PeriodNoDither=630
TIM8.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
TIM8.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
TIM8.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
TIM8.IPParameters=Channel-PWM Generation2 CH2,PeriodNoDither,AutoReloadPreload
TIM8.IPParameters=Channel-PWM Generation2 CH2,PeriodNoDither,AutoReloadPreload,Channel-PWM Generation1 CH1
TIM8.PeriodNoDither=630
USB_DEVICE.CLASS_NAME_FS=CDC
USB_DEVICE.IPParameters=VirtualMode,VirtualModeFS,CLASS_NAME_FS