diff --git a/Core/Inc/CommCallbacks.h b/Core/Inc/CommCallbacks.h index e365da9..6443c49 100644 --- a/Core/Inc/CommCallbacks.h +++ b/Core/Inc/CommCallbacks.h @@ -1,22 +1,25 @@ #ifndef COMM_CALLBACKS_HPP #define COMM_CALLBACKS_HPP -#include - #ifdef __cplusplus extern "C" { #endif // Déclarations des fonctions appelées depuis le C -void Comm_GetPosition(float* x, float* y, float* t); -void Comm_GetSpeed(float* vx, float* vy, float* omega); -void Comm_GetPID(float* p, float* i, float* d); -float Comm_GetDistance(int sensorId); // 👈 AJOUT ICI -void Comm_SetPosition(float x, float y, float t); +void Comm_GetPos(float& x, float& y, float& t); +void Comm_SetPos(float x, float y, float t); + +void Comm_GetSpeed(float& vx, float& vy, float& omega); + +void Comm_GetPID(float& p, float& i, float& d); void Comm_SetPID(float p, float i, float d); -void Comm_AddWaypoint(int id, int type, float x, float y, float t); + void Comm_StartOdometry(bool start); +void Comm_AddWaypoint(int id, int type, float x, float y, float t); + +float Comm_GetDistance(int sensorId); + #ifdef __cplusplus } #endif diff --git a/Core/Inc/commSTM.h b/Core/Inc/commSTM.h index 383335d..16848ab 100644 --- a/Core/Inc/commSTM.h +++ b/Core/Inc/commSTM.h @@ -8,9 +8,6 @@ #ifndef INC_COMMSTM_H_ #define INC_COMMSTM_H_ - -#include -#include #include "usbd_cdc_if.h" #ifdef __cplusplus diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h index 97f98cf..a4db7d5 100644 --- a/Core/Inc/modelec.h +++ b/Core/Inc/modelec.h @@ -7,6 +7,7 @@ #ifndef MODELEC_H #define MODELEC_H + #include "motors.h" #include "main.h" #include "stm32g4xx_hal.h" @@ -56,17 +57,9 @@ public: uint32_t lastTick = 0; - bool isDelayPassedFrom(uint32_t delay, uint32_t *lastTick) { - if (HAL_GetTick() - *lastTick >= delay) { - *lastTick = HAL_GetTick(); - return true; - } - return false; - } + static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick); - bool isDelayPassed(uint32_t delay) { - return isDelayPassedFrom(delay, &lastTick); - } + bool isDelayPassed(uint32_t delay); float readEncoderRight(); diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index 043d97b..fd75b1c 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -15,22 +15,22 @@ extern DiffBot bot; -void Comm_GetPosition(float* x, float* y, float* theta) { - *x = bot.pose.x; - *y = bot.pose.y; - *theta = bot.pose.theta; +void Comm_GetPos(float& x, float& y, float& theta) { + x = bot.pose.x; + y = bot.pose.y; + theta = bot.pose.theta; } -void Comm_SetPosition(float x, float y, float theta) { +void Comm_SetPos(float x, float y, float theta) { bot.pose.x = x; bot.pose.y = y; bot.pose.theta = theta; } -void Comm_GetSpeed(float* vx, float* vy, float* omega){ +void Comm_GetSpeed(float& vx, float& vy, float& omega){ } -void Comm_GetPID(float* p, float* i, float* d) { +void Comm_GetPID(float& p, float& i, float& d) { } void Comm_SetPID(float p, float i, float d) { diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp index 03662ae..5cfef62 100644 --- a/Core/Src/commSTM.cpp +++ b/Core/Src/commSTM.cpp @@ -8,11 +8,8 @@ #include #include "commSTM.h" #include "usbd_cdc.h" -#include -#include -#include -// ⚠️ fourni par CubeMX dans usb_device.c +// fourni par CubeMX dans usb_device.c extern USBD_HandleTypeDef hUsbDeviceFS; // Buffer de réception circulaire @@ -65,21 +62,21 @@ void USB_Comm_Process(void) { if (strcmp(token, "POS") == 0) { float x, y, t; - Comm_GetPosition(&x, &y, &t); + Comm_GetPos(x, y, t); char response[64]; snprintf(response, sizeof(response), "SET;POS;%.4f;%.4f;%.4f\n", x, y, t); USB_Comm_Send(response); } else if (strcmp(token, "SPEED") == 0) { float vx, vy, omega; - Comm_GetSpeed(&vx, &vy, &omega); + Comm_GetSpeed(vx, vy, omega); char response[64]; snprintf(response, sizeof(response), "SET;SPEED;%.4f;%.4f;%.4f\n", vx, vy, omega); USB_Comm_Send(response); } else if (strcmp(token, "PID") == 0) { float p, i, d; - Comm_GetPID(&p, &i, &d); + Comm_GetPID(p, i, d); char response[64]; snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f\n", p, i, d); USB_Comm_Send(response); @@ -105,7 +102,7 @@ void USB_Comm_Process(void) { float x = atof(strtok(NULL, ";")); float y = atof(strtok(NULL, ";")); float t = atof(strtok(NULL, ";")); - Comm_SetPosition(x, y, t); + Comm_SetPos(x, y, t); USB_Comm_Send("OK;POS\n"); } else if (strcmp(token, "PID") == 0) { diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 4578345..5b8a993 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -23,6 +23,17 @@ // *out_pidG = new PidVitesse(0.2, 0.0, 0.01, 0); // *out_pidD = new PidVitesse(0.2, 0.0, 0.01, 0); +bool DiffBot::isDelayPassedFrom(uint32_t delay, uint32_t &lastTick) { + if (HAL_GetTick() - lastTick >= delay) { + lastTick = HAL_GetTick(); + return true; + } + return false; +} + +bool DiffBot::isDelayPassed(uint32_t delay) { + return isDelayPassedFrom(delay, lastTick); +} float DiffBot::readEncoderLeft() { int16_t count = __HAL_TIM_GET_COUNTER(&htim2); @@ -40,6 +51,9 @@ float DiffBot::readEncoderRight() { return (2.0f*M_PI*WHEEL_RADIUS*revs); // m } +DiffBot::DiffBot(Point pose, float dt) : pose(pose), motor(), dt(dt) { +}; + void DiffBot::setup() { pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX); pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX); @@ -59,14 +73,14 @@ void DiffBot::update(float dt) { if (!isDelayPassed(dt*1000)) return; // read encoder - float rightVel = readEncoderRight(); + float rightVel = readEncoderRight(); float leftVel = readEncoderLeft(); // update pos float v = (leftVel + rightVel) / 2.0f; float w = (rightVel - leftVel) / WHEEL_BASE; - pose.x += v * cosf(pose.theta - w/2); - pose.y += v * sinf(pose.theta - w/2); + pose.x += v * cosf(pose.theta - w/2); + pose.y += v * sinf(pose.theta - w/2); pose.theta += w; while (pose.theta > M_PI) pose.theta -= 2*M_PI; @@ -76,6 +90,7 @@ void DiffBot::update(float dt) { motor.update(); return; } + /* * * @@ -96,9 +111,9 @@ void DiffBot::update(float dt) { float dist = sqrtf(dx*dx + dy*dy); float angleTarget = atan2f(dy, dx); - float angleError = angleTarget - pose.theta; + float angleError = angleTarget - pose.theta; - while (angleError > M_PI) angleError -= 2*M_PI; + while (angleError > M_PI) angleError -= 2*M_PI; while (angleError < -M_PI) angleError += 2*M_PI; float direction = 1.0f; @@ -114,10 +129,9 @@ void DiffBot::update(float dt) { distError *= direction; switch (targets[index].state) { - case StatePoint::FINAL: + case FINAL: if (fabs(dx) <= 0.01 && fabs(dy) <= 0.01 && fabs(targets[index].theta - pose.theta) < 0.01) { - // maybe remove that so when stopped and you moved it, the robot compensates itself targets[index].active = false; motor.stop(true); @@ -126,14 +140,16 @@ void DiffBot::update(float dt) { CDC_Transmit_FS((uint8_t*)log, strlen(log)); return; - } else if (targets[index].active == false) { + } + + if (targets[index].active == false) { targets[index].active = true; } break; - case StatePoint::INTERMEDIAIRE: + case INTERMEDIAIRE: - if (fabs(dx) < 0.1 && fabs(dy) < 0.1) { + if (std::fabs(dx) < 0.1 && std::fabs(dy) < 0.1) { char log[32]; sprintf(log, "SET;WAYPOINT;%d\n", index); @@ -147,6 +163,8 @@ void DiffBot::update(float dt) { return; } + + // TODO when patch is done refactor this dx = targets[index].x - pose.x; dy = targets[index].y - pose.y; distError = sqrtf(dx*dx + dy*dy); @@ -169,7 +187,7 @@ void DiffBot::update(float dt) { // float wRef = pidTheta.compute(targets[index].theta, pose.theta) /*+ 2.0f * angleError*/; float wRef; - if (targets[index].state == StatePoint::FINAL && fabs(dx) <= 0.01 && fabs(dy) <= 0.01) { + if (targets[index].state == FINAL && std::fabs(dx) <= 0.01 && std::fabs(dy) <= 0.01) { wRef = pidTheta.compute(targets[index].theta, pose.theta, dt); vRef = 0; } @@ -192,9 +210,9 @@ void DiffBot::update(float dt) { float pwmRight = pwm_ff_right + pwm_corr_right; const float pwm_deadzone = 50.0f; - if (fabs(pwmLeft) > 0 && fabs(pwmLeft) < pwm_deadzone) + if (std::fabs(pwmLeft) > 0 && std::fabs(pwmLeft) < pwm_deadzone) pwmLeft = (pwmLeft > 0) ? pwm_deadzone : -pwm_deadzone; - if (fabs(pwmRight) > 0 && fabs(pwmRight) < pwm_deadzone) + if (std::fabs(pwmRight) > 0 && std::fabs(pwmRight) < pwm_deadzone) pwmRight = (pwmRight > 0) ? pwm_deadzone : -pwm_deadzone; // saturation @@ -206,10 +224,6 @@ void DiffBot::update(float dt) { motor.update(); } -DiffBot::DiffBot(Point pose, float dt) : pose(pose), dt(dt) { - -}; - void DiffBot::addTarget(int id, int type, float x, float y, float theta) { if (id >= 10) return;