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

Browse Source
This commit is contained in:
acki
2025-09-27 18:46:35 +02:00
parent a16615e34a
commit 70c48a25a4
6 changed files with 57 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
Core/Inc/CommCallbacks.h
View File

@@ -1,22 +1,25 @@
#ifndef COMM_CALLBACKS_HPP
#define COMM_CALLBACKS_HPP
#include <stdbool.h>
#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

3
Core/Inc/commSTM.h
View File

@@ -8,9 +8,6 @@
#ifndef INC_COMMSTM_H_
#define INC_COMMSTM_H_
#include <stdint.h>
#include <stddef.h>
#include "usbd_cdc_if.h"
#ifdef __cplusplus

13
Core/Inc/modelec.h
View File

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

14
Core/Src/CommCallbacks.cpp
View File

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

13
Core/Src/commSTM.cpp
View File

@@ -8,11 +8,8 @@
#include <CommCallbacks.h>
#include "commSTM.h"
#include "usbd_cdc.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
// ⚠️ 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) {

48
Core/Src/modelec.cpp
View File

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