From 1f9cec27dd07649adf7f964c3c64dda671baddbc Mon Sep 17 00:00:00 2001 From: Maxime Date: Mon, 26 May 2025 11:29:27 +0200 Subject: [PATCH] =?UTF-8?q?communications=20avanc=C3=A9e?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Inc/modelec.h | 58 ++++++++++++++++++++++++++++++++++++++ Core/Src/CommCallbacks.cpp | 24 ++++++++-------- Core/Src/commSTM.cpp | 2 +- Core/Src/main.c | 6 ++-- Core/Src/modelec.cpp | 24 ++++------------ 5 files changed, 80 insertions(+), 34 deletions(-) create mode 100644 Core/Inc/modelec.h diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h new file mode 100644 index 0000000..632cf93 --- /dev/null +++ b/Core/Inc/modelec.h @@ -0,0 +1,58 @@ +/* + * modelec.h + * + * Created on: May 25, 2025 + * Author: maxch + */ + +#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 "usbd_cdc_if.h" +#include "commSTM.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// Prototypes des fonctions accessibles depuis main.cpp ou ailleurs + +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(); + +void determinationCoefPosition( + Point objectifPoint, + Point pointActuel, + PidPosition& pid, + PidVitesse& pidG, + PidVitesse& pidD, + float vitGauche, + float vitDroit, + int cnt +); + +// Fonctions utilitaires (optionnellement utilisables ailleurs) +bool isDelayPassedFrom(uint32_t delay, uint32_t *lastTick); +bool isDelayPassed(uint32_t delay); + +#ifdef __cplusplus +} +#endif + +#endif // MODELEC_H + diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index adaa637..1e0c159 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -9,30 +9,30 @@ #include "CommCallbacks.hpp" #include "main.h" -#include "modelec.cpp" +#include "modelec.h" #include "motors.h" #include "pid.h" #include "pidPosition.h" void Comm_GetPosition(float* x, float* y, float* theta) { - *x = currentPoint.getX(); - *y = currentPoint.getY(); - *theta = currentPoint.getTheta(); + //*x = currentPoint.getX(); + //*y = currentPoint.getY(); + //*theta = currentPoint.getTheta(); } void Comm_SetPosition(float x, float y, float theta) { - currentPoint.setX(x); - currentPoint.setY(y); - currentPoint.setTheta(theta); + //currentPoint.setX(x); + //currentPoint.setY(y); + //currentPoint.setTheta(theta); } void Comm_GetSpeed(float* vx, float* vy, float* omega){ - *vx = vitesseLeft; - *vy = vitesseRight; - *omega = vitesseAngulaire; +// *vx = vitesseLeft; +// *vy = vitesseRight; +// *omega = vitesseAngulaire; } -void Comm_GetPID(float* p, float* i, float* d) { +/*void Comm_GetPID(float* p, float* i, float* d) { PIDController::getInstance().getCoefficients(*p, *i, *d); } @@ -49,4 +49,4 @@ void Comm_AddWaypoint(int id, int type, float x, float y, float theta) { Waypoint wp(id, type, x, y, theta); WaypointManager::getInstance().addWaypoint(wp); } - +*/ diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp index b89db13..d7785f5 100644 --- a/Core/Src/commSTM.cpp +++ b/Core/Src/commSTM.cpp @@ -72,7 +72,7 @@ void USB_Comm_Process(void) { } else if (strcmp(token, "SPEED") == 0) { float vx, vy, omega; - Comm_GetSpeed(&vx, &vy, &omega); // 3 valeurs + Comm_GetSpeed(&vx, &vy, &omega); char response[64]; snprintf(response, sizeof(response), "SET;SPEED;%.2f;%.2f;%.2f\n", vx, vy, omega); USB_Comm_Send(response); diff --git a/Core/Src/main.c b/Core/Src/main.c index 4c48d4d..dd4ef36 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -21,6 +21,7 @@ #include "tim.h" #include "usb_device.h" #include "gpio.h" +#include "modelec.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ @@ -36,7 +37,7 @@ /* USER CODE BEGIN PD */ 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); /* USER CODE END PD */ @@ -50,6 +51,7 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD); /* USER CODE BEGIN PV */ int counter1=0; int counter2=0; +int cnt=0; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ @@ -124,7 +126,7 @@ int main(void) { counter2 = __HAL_TIM_GET_COUNTER(&htim2); counter1 = __HAL_TIM_GET_COUNTER(&htim3); - ModelecOdometryLoop(pid, pidG, pidD); + ModelecOdometryLoop(pid, pidG, pidD, &cnt); /* USER CODE END WHILE */ diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 26bf9c1..f98249d 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -1,19 +1,5 @@ -#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 "usbd_cdc_if.h" -#include "commSTM.h" +#include "modelec.h" extern "C" { @@ -204,14 +190,14 @@ void receiveControlParams(){ } -void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) { +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); //receiveControlParams(); //GPIOC->ODR ^= (1 << 10); - int cnt=0; + //On met à jour toutes les 10ms if (isDelayPassed(10)) { ModelecOdometryUpdate(); @@ -229,10 +215,10 @@ void ModelecOdometryLoop(void* pid, void* pidG, void* pidD) { CDC_Transmit_FS((uint8_t*)debugMsg, strlen(debugMsg)); - determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed(), cnt); + determinationCoefPosition(targetPoint, currentPoint, *pidPosition, *pidVitesseG, *pidVitesseD, motor.getLeftCurrentSpeed(), motor.getRightCurrentSpeed(), *cnt); //HAL_Delay(1000); motor.update(); - cnt++; + (*cnt)++;