From f2eb9f85aaa3a307c7429baf0e6d24be4d8422d0 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Mar 2026 10:58:00 +0100 Subject: [PATCH] new odo --- Core/Inc/modelec.h | 5 +- Core/Src/modelec.cpp | 468 +++++++++++++++++++++++++++---------------- 2 files changed, 296 insertions(+), 177 deletions(-) diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h index 7842be7..a05ae6c 100644 --- a/Core/Inc/modelec.h +++ b/Core/Inc/modelec.h @@ -63,7 +63,8 @@ public: float preciseAngleFinal = 0.017f; float preciseAngle = 0.39f; - float precisePosFinal = 0.01f; + float precisePosFinal = 0.001f; + float precisePos2 = 0.02f; float precisePos = 0.1f; static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick); @@ -80,7 +81,7 @@ public: void setup(); - void update(float dt); + void update(float dt_actual); void addTarget(int id, int type, float x, float y, float theta); diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 0fe447b..bda03d1 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -1,228 +1,350 @@ - #include "modelec.h" #include #include -bool DiffBot::isDelayPassedFrom(uint32_t delay, uint32_t &lastTick) { - if (HAL_GetTick() - lastTick >= delay) { - lastTick = HAL_GetTick(); - return true; - } - return false; +float normalizeAngle(float angle) +{ + while (angle > M_PI) angle -= 2.0f * (float)M_PI; + while (angle < -M_PI) angle += 2.0f * (float)M_PI; + return angle; } -bool DiffBot::isDelayPassed(uint32_t delay) { - return isDelayPassedFrom(delay, lastTick); +bool DiffBot::isDelayPassedFrom(uint32_t delay, uint32_t& lastTick) +{ + uint32_t currentTick = HAL_GetTick(); + if (currentTick - lastTick >= delay) + { + lastTick = currentTick; + return true; + } + return false; } -float DiffBot::readEncoderLeft() { - int16_t count = __HAL_TIM_GET_COUNTER(&htim2); - int16_t diff = count - prevCountLeft; - prevCountLeft = count; - float revs = static_cast(diff) / ENCODER_RES; - return (2.0f*M_PI*WHEEL_RADIUS*revs); // m +bool DiffBot::isDelayPassed(uint32_t delay) +{ + return isDelayPassedFrom(delay, lastTick); } -float DiffBot::readEncoderRight() { - int16_t count = __HAL_TIM_GET_COUNTER(&htim3); +float DiffBot::readEncoderLeft() +{ + int16_t count = (int16_t)__HAL_TIM_GET_COUNTER(&htim2); + int16_t diff = count - prevCountLeft; + prevCountLeft = count; + + float revs = static_cast(diff) / ENCODER_RES; + float distance = (2.0f * (float)M_PI * WHEEL_RADIUS * revs); + return distance / dt; +} + +float DiffBot::readEncoderRight() +{ + int16_t count = (int16_t)__HAL_TIM_GET_COUNTER(&htim3); int16_t diff = count - prevCountRight; prevCountRight = count; + float revs = static_cast(diff) / ENCODER_RES; - return (2.0f*M_PI*WHEEL_RADIUS*revs); // m + float distance = (2.0f * (float)M_PI * WHEEL_RADIUS * revs); + return distance / dt; } -DiffBot::DiffBot(Point pos, float dt) : pos(pos), dt(dt) { +DiffBot::DiffBot(Point pos, float dt) : pos(pos), dt(dt) +{ }; -void DiffBot::stop(bool stop) { - odo_active = !stop; - motor.stop(stop); +void DiffBot::stop(bool stop) +{ + odo_active = !stop; + motor.stop(stop); } -void DiffBot::setup() { - pidLeft = PID(2, 0, 0.5, -PWM_MAX, PWM_MAX); - pidRight = PID(2, 0, 0.5, -PWM_MAX, PWM_MAX); - pidPos = PID(3, -0.2, 1.0, -V_MAX, V_MAX); - pidTheta = PID(6, 0.5, 0.5, -M_PI, M_PI); +void DiffBot::setup() +{ + pidLeft = PID(2.5f, 0.1f, 0.0f, -PWM_MAX, PWM_MAX); + pidRight = PID(2.5f, 0.1f, 0.0f, -PWM_MAX, PWM_MAX); + pidPos = PID(3.0f, 0.0f, 0.01f, -V_MAX, V_MAX); + pidTheta = PID(5.0f, 0.0f, 0.01f, -M_PI, M_PI); - prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2); - prevCountRight = __HAL_TIM_GET_COUNTER(&htim3); + prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2); + prevCountRight = __HAL_TIM_GET_COUNTER(&htim3); } -void DiffBot::update(float dt) { - if (!isDelayPassed(dt*1000)) return; +void DiffBot::update(float dt_actual) +{ + this->dt = dt_actual; - // read encoder + if (!isDelayPassed(dt * 1000)) return; + + // read encoder float rightVel = readEncoderRight(); float leftVel = readEncoderLeft(); - if (rightVel == 0 && leftVel == 0) { - if (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0) { - if (!no_move) { - no_move = true; - publishNotMoved = HAL_GetTick(); - } - else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) { - motor.stop(true); + float dLeft = leftVel * dt; + float dRight = rightVel * dt; + float dDistance = (dLeft + dRight) / 2.0f; + float dTheta = (dRight - dLeft) / WHEEL_BASE; - if (targets[index].active) - { - char log[64]; - sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); - CDC_Transmit_FS((uint8_t*)log, strlen(log)); + float avgTheta = pos.theta + (dTheta * 0.5f); + pos.x += dDistance * cosf(avgTheta); + pos.y += dDistance * sinf(avgTheta); + pos.theta = normalizeAngle(pos.theta + dTheta); - targets[index].active = false; - } + /*if (rightVel == 0 && leftVel == 0) { + if (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0) { + if (!no_move) { + no_move = true; + publishNotMoved = HAL_GetTick(); + } + else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) { + motor.stop(true); - no_move = false; + if (targets[index].active) + { + char log[64]; + sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, strlen(log)); - if (action != 0) { - switch (action) { - case 1: // LEFT - if (cos(pos.theta) > 0) { - pos.x = 0.1f; // dist fron back to center - } - else { - pos.x = 0.1f; // dist fron center to front - } - break; - case 2: // TOP - if (sin(pos.theta) > 0) { - pos.y = 2.0f - 0.1f; // dist fron center to front - } - else { - pos.y = 2.0f - 0.1f; // dist fron back to center - } - break; - case 3: // RIGHT - if (cos(pos.theta) > 0) { - pos.x = 3.0f - 0.1f; // dist fron center to front - } - else { - pos.x = 3.0f - 0.1f; // dist fron back to center - } - break; - case 4: // BOTTOM - if (sin(pos.theta) > 0) { - pos.y = 0.1f; // dist fron back to center - } - else { - pos.y = 0.1f; // dist fron center to front - } - break; - default: - break; - } + targets[index].active = false; + } - action = 0; + no_move = false; - addTarget(0, 1, pos.x, pos.y, pos.theta); - } - } - } + if (action != 0) { + switch (action) { + case 1: // LEFT + if (cos(pos.theta) > 0) { + pos.x = 0.1f; // dist fron back to center + } + else { + pos.x = 0.1f; // dist fron center to front + } + break; + case 2: // TOP + if (sin(pos.theta) > 0) { + pos.y = 2.0f - 0.1f; // dist fron center to front + } + else { + pos.y = 2.0f - 0.1f; // dist fron back to center + } + break; + case 3: // RIGHT + if (cos(pos.theta) > 0) { + pos.x = 3.0f - 0.1f; // dist fron center to front + } + else { + pos.x = 3.0f - 0.1f; // dist fron back to center + } + break; + case 4: // BOTTOM + if (sin(pos.theta) > 0) { + pos.y = 0.1f; // dist fron back to center + } + else { + pos.y = 0.1f; // dist fron center to front + } + break; + default: + break; + } + + action = 0; + + addTarget(0, 1, pos.x, pos.y, pos.theta); + } + } + } } else { - if (no_move) { - no_move = false; - } + if (no_move) { + no_move = false; + } + }*/ + + if (std::abs(rightVel) < 0.001f && std::abs(leftVel) < 0.001f && + (std::abs(motor.rightTarget_PWM) > 50 || std::abs(motor.leftTarget_PWM) > 50)) + { + if (!no_move) + { + no_move = true; + publishNotMoved = HAL_GetTick(); + } + else if (HAL_GetTick() - publishNotMoved > notMovedMaxTime) + { + char log[64]; + int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, len); + + targets[index].active = false; + + index = (index + 1) % MAX_WAYPOINTS; + + motor.stop(true); + resetPID(); + return; + } } - - // update pos - float v = (leftVel + rightVel) / 2.0f; - float w = (rightVel - leftVel) / WHEEL_BASE; - pos.x += v * cosf(pos.theta - w/2); - pos.y += v * sinf(pos.theta - w/2); - pos.theta += w; - - while (pos.theta > M_PI) pos.theta -= 2*M_PI; - while (pos.theta < -M_PI) pos.theta += 2*M_PI; - - if (odo_active && isDelayPassedFrom(frequencyPublish, publishLastTick)) { - publishStatus(); + else + { + no_move = false; } if (!odo_active || !targets[index].active) { - motor.update(); - return; + motor.update(); + return; } + float dx = targets[index].x - pos.x; + float dy = targets[index].y - pos.y; + float dist = sqrtf(dx*dx + dy*dy); + float angleTarget = atan2f(dy, dx); + float angleError = normalizeAngle(angleTarget - pos.theta); + + float direction = 1.0f; + if (std::fabs(angleError) > (float)M_PI / 2.0f) { + direction = -1.0f; + angleError = normalizeAngle(angleError - (float)M_PI); + } + + float vRef = 0.0f; + float wRef = 0.0f; + + bool reachedPos = (dist < (targets[index].state == FINAL ? precisePosFinal : precisePos)); + + if (reachedPos) { + bool finalized = true; + + if (targets[index].state == FINAL) { + float finalAngleErr = normalizeAngle(targets[index].theta - pos.theta); + if (std::fabs(finalAngleErr) > preciseAngleFinal) { + wRef = pidTheta.compute(0, -finalAngleErr, dt); + vRef = 0; + finalized = false; + } + } + + if (finalized) { + char log[64]; + int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, len); + + targets[index].active = false; + + index = (index + 1) % MAX_WAYPOINTS; + + motor.stop(true); + resetPID(); + return; + } + } else { + wRef = pidTheta.compute(0, -angleError, dt); + + float alignmentScale = cosf(angleError); + if (alignmentScale < 0) alignmentScale = 0; + + vRef = pidPos.compute(0, -dist * direction, dt) * alignmentScale; + } + + float vLeftReq = vRef - (WHEEL_BASE_2 * wRef); + float vRightReq = vRef + (WHEEL_BASE_2 * wRef); + + float pwmLeft = ((vLeftReq / V_MAX) * PWM_MAX) + pidLeft.compute(vLeftReq, leftVel, dt); + float pwmRight = ((vRightReq / V_MAX) * PWM_MAX) + pidRight.compute(vRightReq, rightVel, dt); + + auto applyLimits = [&](float pwm) { + if (std::abs(pwm) < 1.0f) return 0.0f; + float limited = std::max(-PWM_MAX, std::min(pwm, PWM_MAX)); + return limited; + }; + + motor.leftTarget_PWM = static_cast(applyLimits(pwmLeft)); + motor.rightTarget_PWM = static_cast(applyLimits(pwmRight)); + motor.update(); + + if (isDelayPassedFrom(frequencyPublish, publishLastTick)) { + publishStatus(); + } + + /* // pid setup float dx = targets[index].x - pos.x; float dy = targets[index].y - pos.y; - switch (targets[index].state) { + switch (targets[index].state) + { case FINAL: - if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs(targets[index].theta - pos.theta) < preciseAngleFinal) { - targets[index].active = false; - motor.stop(true); + if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs( + targets[index].theta - pos.theta) < preciseAngleFinal) + { + targets[index].active = false; + motor.stop(true); - char log[64]; - sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); - CDC_Transmit_FS((uint8_t*)log, strlen(log)); + char log[64]; + sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, strlen(log)); - resetPID(); + resetPID(); - return; - } + return; + } - break; + break; case INTERMEDIAIRE: - if (std::fabs(dx) < precisePos && std::fabs(dy) < precisePos) { + if (std::fabs(dx) < precisePos && std::fabs(dy) < precisePos) + { + char log[64]; + sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, strlen(log)); - char log[64]; - sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); - CDC_Transmit_FS((uint8_t*)log, strlen(log)); + targets[index].active = false; + index++; - targets[index].active = false; - index++; + if (index >= 9) + { + index = 0; + return; + } - if (index >= 9) { - index = 0; - return; - } + resetPID(); - resetPID(); + dx = targets[index].x - pos.x; + dy = targets[index].y - pos.y; + } - dx = targets[index].x - pos.x; - dy = targets[index].y - pos.y; - - } - - break; + break; default: - break; + break; } - float dist = sqrtf(dx*dx + dy*dy); + float dist = sqrtf(dx * dx + dy * dy); - float angleTarget = atan2f(dy, dx); - float angleError = angleTarget - pos.theta; + float angleTarget = atan2f(dy, dx); + float angleError = angleTarget - pos.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; + 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; - } + 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) <= preciseAngle) angleError = 0; + float distError = dist * cosf(angleError); - float distError = dist * cosf(angleError); - - distError *= direction; + distError *= direction; float vRef = pidPos.compute(0.0, -distError, dt); - float wRef; + float wRef = 0.0f; - if (targets[index].state == FINAL && std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal) { + if (targets[index].state == FINAL && std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal) + { wRef = pidTheta.compute(targets[index].theta, pos.theta, dt); vRef = 0; } - else { + else if (precisePos2 < std::abs(distError)) + { wRef = pidTheta.compute(0.0, -angleError, dt); } @@ -238,42 +360,38 @@ void DiffBot::update(float dt) { float pwmLeft = pwm_ff_left + pwm_corr_left; float pwmRight = pwm_ff_right + pwm_corr_right; - const float pwm_deadzone = 50.0f; - if (std::fabs(pwmLeft) > 0 && std::fabs(pwmLeft) < pwm_deadzone) - pwmLeft = (pwmLeft > 0) ? pwm_deadzone : -pwm_deadzone; - if (std::fabs(pwmRight) > 0 && std::fabs(pwmRight) < pwm_deadzone) - pwmRight = (pwmRight > 0) ? pwm_deadzone : -pwm_deadzone; - // saturation - pwmLeft = std::max(-PWM_MAX, std::min(pwmLeft, PWM_MAX)); + 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(pwmLeft); motor.rightTarget_PWM = static_cast(pwmRight); - motor.update(); + motor.update();*/ } -void DiffBot::addTarget(int id, int type, float x, float y, float theta) { +void DiffBot::addTarget(int id, int type, float x, float y, float theta) +{ + if (id >= MAX_WAYPOINTS) return; - if (id >= MAX_WAYPOINTS) return; + targets[id] = Point(id, static_cast(type), x, y, theta); + targets[id].active = true; - targets[id] = Point(id, static_cast(type), x, y, theta); - targets[id].active = true; - - if (id <= index) index = 0; + if (id <= index) index = 0; arrive = false; } -void DiffBot::resetPID() { - pidLeft.reset(); - pidRight.reset(); - pidPos.reset(); - pidTheta.reset(); +void DiffBot::resetPID() +{ + pidLeft.reset(); + pidRight.reset(); + pidPos.reset(); + pidTheta.reset(); } -void DiffBot::publishStatus() { +void DiffBot::publishStatus() +{ char response[64]; - snprintf(response, sizeof(response), "SET;POS;%.4f;%.4f;%.4f\n", pos.x*1000, pos.y*1000, pos.theta); + snprintf(response, sizeof(response), "SET;POS;%.4f;%.4f;%.4f\n", pos.x * 1000, pos.y * 1000, pos.theta); CDC_Transmit_FS((uint8_t*)response, strlen(response)); }