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

working odometry

Browse Source
This commit is contained in:
acki
2025-09-30 18:56:46 +02:00
parent 70c48a25a4
commit 8b4db1c18c
13 changed files with 208 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
Core/Inc/CommCallbacks.h
View File

@@ -11,8 +11,9 @@ 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);
bool Comm_GetPID(char *pid, float &p, float &i, float &d);
bool Comm_SetPID(char *pid, float p, float i, float d);
void Comm_StartOdometry(bool start);
@@ -20,6 +21,8 @@ void Comm_AddWaypoint(int id, int type, float x, float y, float t);
float Comm_GetDistance(int sensorId);
void Comm_SetPWM(float left, float right);
#ifdef __cplusplus
}
#endif

2
Core/Inc/commSTM.h
View File

@@ -8,7 +8,7 @@
#ifndef INC_COMMSTM_H_
#define INC_COMMSTM_H_
#include "usbd_cdc_if.h"
#include <stdint.h>
#ifdef __cplusplus
extern "C" {

40
Core/Inc/modelec.h
View File

@@ -8,23 +8,23 @@
#ifndef MODELEC_H
#define MODELEC_H
#include "motors.h"
#include "main.h"
#include "stm32g4xx_hal.h"
#include "usbd_cdc_if.h"
#include <cstdio>
#include <cstring>
#include <cmath>
#include <algorithm>
#include "motors.h"
#include "pid.h"
#include "point.h"
#include "CommCallbacks.h"
#include "usbd_cdc_if.h"
#include "commSTM.h"
#ifdef __cplusplus
extern "C" {
#endif
#define MAX_WAYPOINTS 16
#define PWM_MAX 626.0f
#define ENCODER_RES 2400.0f
#define WHEEL_DIAMETER 0.081f
#define WHEEL_RADIUS (WHEEL_DIAMETER/2.0f)
#define WHEEL_BASE 0.287f
#define WHEEL_BASE_2 (WHEEL_BASE/2.0f)
#define PRECISE_ANGLE 0.017f // radians
#define PRECISE_POS_FINAL 0.005f // meters
#define PRECISE_POS 0.1f // meters
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim2;
@@ -33,11 +33,13 @@ class DiffBot {
public:
Point pose;
Point targets[10];
Point targets[MAX_WAYPOINTS];
uint8_t index = 0;
Motor motor;
float vx, vy, vtheta;
float dt;
PID pidLeft, pidRight;
@@ -48,13 +50,6 @@ public:
bool odo_active = false;
bool arrive = false;
int16_t PWM_MAX = 626;
float ENCODER_RES = 2400.0f;
float WHEEL_DIAMETER = 0.081f;
float WHEEL_RADIUS = 0.0405f;
float WHEEL_BASE = 0.287f;
float WHEEL_BASE_2 = 0.1435f;
uint32_t lastTick = 0;
static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick);
@@ -75,10 +70,7 @@ public:
void addTarget(int id, int type, float x, float y, float theta);
void resetPID();
};
#ifdef __cplusplus
}
#endif
#endif // MODELEC_H

9
Core/Inc/motors.h
View File

@@ -1,14 +1,13 @@
#ifndef INC_MOTORS_H_
#define INC_MOTORS_H_
//#include "stm32l0xx_hal.h"
#include "stm32g4xx_hal.h"
float approach(float current, float target, float step);
class Motor {
public:
int16_t leftTarget_PWM = 0, rightTarget_PWM = 0;
int16_t leftCurrent_PWM = 0, rightCurrent_PWM = 0;
bool bStop;
float leftTarget_PWM = 0, rightTarget_PWM = 0;
float leftCurrent_PWM = 0, rightCurrent_PWM = 0;
bool bStop = false;
void update();
void stop(bool stop);

14
Core/Inc/pid.h
View File

@@ -8,8 +8,6 @@
#ifndef INC_PID_H_
#define INC_PID_H_
#include <algorithm>
class PID {
protected:
@@ -22,6 +20,18 @@ public:
PID(float kp = 0.0f, float ki = 0.0f, float kd = 0.0f, float outMin = 0.0f, float outMax = 0.0f);
float compute(float setpoint, float measurement, float dt);
void reset();
void setTunings(float kp, float ki, float kd) {
this->kp = kp;
this->ki = ki;
this->kd = kd;
}
float getKp() const { return kp; }
float getKi() const { return ki; }
float getKd() const { return kd; }
};

2
Core/Inc/point.h
View File

@@ -24,7 +24,7 @@ public:
float x;
float y;
float theta;
bool active = false;
bool active;
Point(uint8_t id = 0, StatePoint state = StatePoint::INTERMEDIAIRE, float x = 0.0, float y = 0.0, float theta = 0.0, bool active = false);
};

49
Core/Src/CommCallbacks.cpp
View File

@@ -30,10 +30,50 @@ void Comm_SetPos(float x, float y, float theta) {
void Comm_GetSpeed(float& vx, float& vy, float& omega){
}
void Comm_GetPID(float& p, float& i, float& d) {
bool Comm_GetPID(char *pid, float &p, float &i, float &d) {
if (strcmp(pid, "LEFT") == 0) {
p = bot.pidLeft.getKp();
i = bot.pidLeft.getKi();
d = bot.pidLeft.getKd();
}
else if (strcmp(pid, "RIGHT") == 0) {
p = bot.pidRight.getKp();
i = bot.pidRight.getKi();
d = bot.pidRight.getKd();
}
else if (strcmp(pid, "POS") == 0) {
p = bot.pidPos.getKp();
i = bot.pidPos.getKi();
d = bot.pidPos.getKd();
}
else if (strcmp(pid, "THETA") == 0) {
p = bot.pidTheta.getKp();
i = bot.pidTheta.getKi();
d = bot.pidTheta.getKd();
}
else {
return false;
}
return true;
}
void Comm_SetPID(float p, float i, float d) {
bool Comm_SetPID(char *pid, float p, float i, float d) {
if (strcmp(pid, "LEFT") == 0) {
bot.pidLeft.setTunings(p, i, d);
}
else if (strcmp(pid, "RIGHT") == 0) {
bot.pidRight.setTunings(p, i, d);
}
else if (strcmp(pid, "POS") == 0) {
bot.pidPos.setTunings(p, i, d);
}
else if (strcmp(pid, "THETA") == 0) {
bot.pidTheta.setTunings(p, i, d);
}
else {
return false;
}
return true;
}
void Comm_StartOdometry(bool on) {
@@ -47,3 +87,8 @@ void Comm_AddWaypoint(int id, int type, float x, float y, float theta) {
float Comm_GetDistance(int n) {
return 0.0f;
}
void Comm_SetPWM(float left, float right) {
bot.motor.leftTarget_PWM = left;
bot.motor.rightTarget_PWM = right;
}

42
Core/Src/commSTM.cpp
View File

@@ -5,9 +5,11 @@
* Author: maxch
*/
#include <CommCallbacks.h>
#include "CommCallbacks.h"
#include "commSTM.h"
#include "usbd_cdc.h"
#include "modelec.h"
#include "usbd_cdc_if.h"
// fourni par CubeMX dans usb_device.c
extern USBD_HandleTypeDef hUsbDeviceFS;
@@ -75,11 +77,16 @@ void USB_Comm_Process(void) {
USB_Comm_Send(response);
}
else if (strcmp(token, "PID") == 0) {
char* pid = strtok(NULL, ";");
float 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);
if (Comm_GetPID(pid, p, i, d)) {
char response[64];
snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f\n", p, i, d);
USB_Comm_Send(response);
}
else {
USB_Comm_Send("KO;PID;UNKNOWN\n");
}
}
else if (strcmp(token, "DIST") == 0) {
token = strtok(NULL, ";");
@@ -106,11 +113,17 @@ void USB_Comm_Process(void) {
USB_Comm_Send("OK;POS\n");
}
else if (strcmp(token, "PID") == 0) {
char* pid = strtok(NULL, ";");
float p = atof(strtok(NULL, ";"));
float i = atof(strtok(NULL, ";"));
float d = atof(strtok(NULL, ";"));
Comm_SetPID(p, i, d);
USB_Comm_Send("OK;PID\n");
if (Comm_SetPID(pid, p, i, d)) {
USB_Comm_Send("OK;PID\n");
}
else {
USB_Comm_Send("KO;PID;UNKNOWN\n");
}
}
else if (strcmp(token, "WAYPOINT") == 0) {
@@ -142,6 +155,19 @@ void USB_Comm_Process(void) {
USB_Comm_Send("OK;START\n");
}
else if (strcmp(token, "MOTOR") == 0) {
float left = atof(strtok(NULL, ";"));
float right = atof(strtok(NULL, ";"));
if (left < -PWM_MAX || left > PWM_MAX || right < -PWM_MAX || right > PWM_MAX) {
USB_Comm_Send("KO;MOTOR;RANGE\n");
return;
}
Comm_SetPWM(left, right);
USB_Comm_Send("OK;MOTOR\n");
}
else {
USB_Comm_Send("KO;UNKNOWN\n");
}

125
Core/Src/modelec.cpp
View File

@@ -1,28 +1,14 @@
#include "modelec.h"
// Variables globales
// Constants
#include <algorithm>
#include <cmath>
// #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)
/**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é
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_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();
@@ -54,6 +40,11 @@ float DiffBot::readEncoderRight() {
DiffBot::DiffBot(Point pose, float dt) : pose(pose), motor(), dt(dt) {
};
void DiffBot::stop(bool stop) {
odo_active = !stop;
motor.stop(stop);
}
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);
@@ -64,11 +55,6 @@ void DiffBot::setup() {
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
}
void DiffBot::stop(bool stop) {
odo_active = !stop;
motor.stop(stop);
}
void DiffBot::update(float dt) {
if (!isDelayPassed(dt*1000)) return;
@@ -91,47 +77,14 @@ void DiffBot::update(float dt) {
return;
}
/*
*
*
* TODO
* check les valeurs htim (encodeur) et TIM (moteur)
*
*
* */
// pid setup
float dx = targets[index].x - pose.x;
float dy = targets[index].y - pose.y;
const float minRes = 0.01f;
if (fabsf(dx) < minRes) dx = 0;
if (fabsf(dy) < minRes) dy = 0;
float dist = 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;
float direction = 1.0f;
if (fabs(angleError) > M_PI/2) {
direction = -1.0f;
angleError > 0 ? angleError -= M_PI : angleError += M_PI;
}
if (fabs(angleError) <= 0.001) angleError = 0;
float distError = dist * cosf(angleError);
distError *= direction;
switch (targets[index].state) {
case FINAL:
if (fabs(dx) <= 0.01 && fabs(dy) <= 0.01 && fabs(targets[index].theta - pose.theta) < 0.01) {
if (std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL && std::fabs(targets[index].theta - pose.theta) < PRECISE_ANGLE) {
targets[index].active = false;
motor.stop(true);
@@ -139,6 +92,8 @@ void DiffBot::update(float dt) {
sprintf(log, "SET;WAYPOINT;%d\n", index);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
resetPID();
return;
}
@@ -149,7 +104,7 @@ void DiffBot::update(float dt) {
break;
case INTERMEDIAIRE:
if (std::fabs(dx) < 0.1 && std::fabs(dy) < 0.1) {
if (std::fabs(dx) < PRECISE_POS && std::fabs(dy) < PRECISE_POS) {
char log[32];
sprintf(log, "SET;WAYPOINT;%d\n", index);
@@ -163,16 +118,10 @@ void DiffBot::update(float dt) {
return;
}
resetPID();
// 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);
angleTarget = atan2f(dy, dx);
angleError = angleTarget - pose.theta;
while (angleError > M_PI) angleError -= 2*M_PI;
while (angleError < -M_PI) angleError += 2*M_PI;
}
@@ -181,29 +130,46 @@ void DiffBot::update(float dt) {
break;
}
// check if final x and y are close but not theta so only turn
float dist = 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;
float direction = 1.0f;
if (std::fabs(angleError) > M_PI/2) {
direction = -1.0f;
angleError > 0 ? angleError -= M_PI : angleError += M_PI;
}
if (std::fabs(angleError) <= PRECISE_POS_FINAL) angleError = 0;
float distError = dist * cosf(angleError);
distError *= direction;
float vRef = pidPos.compute(0.0, -distError, dt);
// float wRef = pidTheta.compute(targets[index].theta, pose.theta) /*+ 2.0f * angleError*/;
float wRef;
float wRef;
if (targets[index].state == FINAL && std::fabs(dx) <= 0.01 && std::fabs(dy) <= 0.01) {
if (targets[index].state == FINAL && std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL) {
wRef = pidTheta.compute(targets[index].theta, pose.theta, dt);
vRef = 0;
}
else {
wRef = pidTheta.compute(0.0, angleError, dt);
wRef = pidTheta.compute(0.0, -angleError, dt);
}
float vLeft = vRef - (WHEEL_BASE_2) * wRef;
float vLeft = vRef - (WHEEL_BASE_2) * wRef;
float vRight = vRef + (WHEEL_BASE_2) * wRef;
float v_max = 0.643f; // m/s
float pwm_ff_left = (vLeft / v_max) * PWM_MAX;
float pwm_ff_left = (vLeft / v_max) * PWM_MAX;
float pwm_ff_right = (vRight / v_max) * PWM_MAX;
float pwm_corr_left = pidLeft.compute(vLeft, leftVel, dt);
float pwm_corr_left = pidLeft.compute(vLeft, leftVel, dt);
float pwm_corr_right = pidRight.compute(vRight, rightVel, dt);
float pwmLeft = pwm_ff_left + pwm_corr_left;
@@ -216,17 +182,17 @@ void DiffBot::update(float dt) {
pwmRight = (pwmRight > 0) ? pwm_deadzone : -pwm_deadzone;
// saturation
pwmLeft = MAX(-PWM_MAX, MIN(PWM_MAX, pwmLeft));
pwmRight = MAX(-PWM_MAX, MIN(PWM_MAX, pwmRight));
pwmLeft = std::max(-PWM_MAX, std::min(pwmLeft, PWM_MAX));
pwmRight = std::max(-PWM_MAX, std::min(pwmRight, PWM_MAX));
motor.leftTarget_PWM = static_cast<int16_t>(pwmLeft);
motor.leftTarget_PWM = static_cast<int16_t>(pwmLeft);
motor.rightTarget_PWM = static_cast<int16_t>(pwmRight);
motor.update();
}
void DiffBot::addTarget(int id, int type, float x, float y, float theta) {
if (id >= 10) return;
if (id >= MAX_WAYPOINTS) return;
targets[id] = Point(id, static_cast<StatePoint>(type), x, y, theta);
targets[id].active = true;
@@ -235,3 +201,10 @@ void DiffBot::addTarget(int id, int type, float x, float y, float theta) {
arrive = false;
}
void DiffBot::resetPID() {
pidLeft.reset();
pidRight.reset();
pidPos.reset();
pidTheta.reset();
}

70
Core/Src/motors.cpp
View File

@@ -1,56 +1,44 @@
#include <algorithm>
#include <motors.h>
#include <cstring>
#include <cstdio>
#include "usbd_cdc_if.h"
#include <modelec.h>
float approach(float current, float target, float step) {
if (current < target)
return std::min(current + step, target);
if (current > target)
return std::max(current - step, target);
return current;
}
void Motor::update() {
int16_t PWM_MAX_M = 626;
int16_t max_step = 50;
uint8_t max_step = 25;
leftTarget_PWM = std::max(-PWM_MAX, std::min(leftTarget_PWM, PWM_MAX));
rightTarget_PWM = std::max(-PWM_MAX, std::min(rightTarget_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;
leftCurrent_PWM = approach(leftCurrent_PWM, leftTarget_PWM, max_step);
rightCurrent_PWM = approach(rightCurrent_PWM, rightTarget_PWM, max_step);
if (leftCurrent_PWM < leftTarget_PWM) {
leftCurrent_PWM += max_step;
if (leftCurrent_PWM > leftTarget_PWM)
leftCurrent_PWM = leftTarget_PWM;
} else if (leftCurrent_PWM > leftTarget_PWM) {
leftCurrent_PWM -= max_step;
if (leftCurrent_PWM < leftTarget_PWM)
leftCurrent_PWM = leftTarget_PWM;
}
if (rightCurrent_PWM < rightTarget_PWM) {
rightCurrent_PWM += max_step;
if (rightCurrent_PWM > rightTarget_PWM)
rightCurrent_PWM = rightTarget_PWM;
} else if (rightCurrent_PWM > rightTarget_PWM) {
rightCurrent_PWM -= max_step;
if (rightCurrent_PWM < rightTarget_PWM)
rightCurrent_PWM = rightTarget_PWM;
}
// moteur gauche -> TIM1 CH1/CH2
// left motor -> TIM1 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;
}
// moteur droit -> TIM8 CH1/CH2
if (rightCurrent_PWM >= 0) {
TIM1->CCR1 = static_cast<uint16_t>(rightCurrent_PWM);
TIM1->CCR1 = static_cast<uint16_t>(leftCurrent_PWM);
TIM1->CCR2 = 0;
} else {
TIM1->CCR2 = static_cast<uint16_t>(-rightCurrent_PWM);
TIM1->CCR2 = static_cast<uint16_t>(-leftCurrent_PWM);
TIM1->CCR1 = 0;
}
// right motor -> TIM8 CH1/CH2
if (rightCurrent_PWM >= 0) {
TIM8->CCR1 = static_cast<uint16_t>(rightCurrent_PWM);
TIM8->CCR2 = 0;
} else {
TIM8->CCR2 = static_cast<uint16_t>(-rightCurrent_PWM);
TIM8->CCR1 = 0;
}
}
void Motor::stop(bool stop) {

11
Core/Src/pid.cpp
View File

@@ -1,9 +1,11 @@
#include "pid.h"
#include <algorithm>
PID::PID(float kp, float ki, float kd, float outMin, float outMax)
: kp(kp), ki(ki), kd(kd), integral(0.0), outMin(outMin), outMax(outMax) {
: kp(kp), ki(ki), kd(kd), outMin(outMin), outMax(outMax) {
reset();
}
float PID::compute(float setpoint, float measurement, float dt) {
@@ -12,8 +14,13 @@ float PID::compute(float setpoint, float measurement, float dt) {
float derivative = (error - prevError) / dt;
float output = kp * error + ki * integral + kd * derivative;
output = std::min(std::max(output, outMin), outMax);
output = std::max(outMin, std::min(output, outMax));
prevError = error;
return output;
}
void PID::reset() {
integral = 0.0f;
prevError = 0.0f;
}

1
Core/Src/point.cpp
View File

@@ -3,5 +3,4 @@
#include "point.h"
Point::Point(uint8_t id, StatePoint state, float x, float y, float theta, bool active) : id(id), state(state), x(x), y(y), theta(theta), active(active) {
}

1
Core/Src/setup.cpp
View File

@@ -7,6 +7,7 @@
#include <setup.h>
#include <modelec.h>
#include "commSTM.h"
DiffBot bot(Point(), 0.01f);