modelec/odo_STM32
1
0
Fork 0
mirror of https://github.com/modelec/odo_STM32.git synced 2026-03-18 21:30:38 +01:00
Code Issues Packages Projects Releases Wiki Activity

PID test

Browse Source
This commit is contained in:
acki
2026-03-06 19:36:18 +01:00
parent 6cf678c221
commit cc42b9815e
3 changed files with 18 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Core/Inc/modelec.h
View File

@@ -61,7 +61,8 @@ public:
uint32_t publishLastTick = 0; uint32_t publishLastTick = 0;
uint32_t frequencyPublish = 100; uint32_t frequencyPublish = 100;
float preciseAngle = 0.017f; float preciseAngleFinal = 0.017f;
float preciseAngle = 0.39f;
float precisePosFinal = 0.01f; float precisePosFinal = 0.01f;
float precisePos = 0.1f; float precisePos = 0.1f;

4
Core/Src/CommCallbacks.cpp
View File

@@ -107,7 +107,7 @@ void Comm_GetPublishFrequency(uint32_t &frequencyPublish) {
} }
float Comm_GetPreciseAngle() { float Comm_GetPreciseAngle() {
return bot.preciseAngle; return bot.preciseAngleFinal;
} }
float Comm_GetPrecisePos() { float Comm_GetPrecisePos() {
@@ -119,7 +119,7 @@ float Comm_GetPrecisePosFinal() {
} }
void Comm_SetPreciseAngle(float d) { void Comm_SetPreciseAngle(float d) {
bot.preciseAngle = d; bot.preciseAngleFinal = d;
} }
void Comm_SetPrecisePos(float d) { void Comm_SetPrecisePos(float d) {

21
Core/Src/modelec.cpp
View File

@@ -41,10 +41,10 @@ void DiffBot::stop(bool stop) {
} }
void DiffBot::setup() { void DiffBot::setup() {
pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX); pidLeft = PID(2, 0, 0.5, -PWM_MAX, PWM_MAX);
pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX); pidRight = PID(2, 0, 0.5, -PWM_MAX, PWM_MAX);
pidPos = PID(1, 0.0, 0.0, -V_MAX, V_MAX); pidPos = PID(3, -0.2, 1.0, -V_MAX, V_MAX);
pidTheta = PID(2, 0.0, 0.0, -M_PI, M_PI); pidTheta = PID(6, 0.5, 0.5, -M_PI, M_PI);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2); prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3); prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -66,7 +66,14 @@ void DiffBot::update(float dt) {
else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) { else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
motor.stop(true); motor.stop(true);
targets[index].active = false; if (targets[index].active)
{
char log[64];
sprintf(log, "SET;WAYPOINT;REACH;%d\n", index);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
targets[index].active = false;
}
no_move = false; no_move = false;
@@ -146,7 +153,7 @@ void DiffBot::update(float dt) {
switch (targets[index].state) { switch (targets[index].state) {
case FINAL: case FINAL:
if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs(targets[index].theta - pos.theta) < preciseAngle) { if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs(targets[index].theta - pos.theta) < preciseAngleFinal) {
targets[index].active = false; targets[index].active = false;
motor.stop(true); motor.stop(true);
@@ -202,7 +209,7 @@ void DiffBot::update(float dt) {
angleError > 0 ? angleError -= M_PI : angleError += M_PI; angleError > 0 ? angleError -= M_PI : angleError += M_PI;
} }
if (std::fabs(angleError) <= precisePosFinal) angleError = 0; if (std::fabs(angleError) <= preciseAngle) angleError = 0;
float distError = dist * cosf(angleError); float distError = dist * cosf(angleError);