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

omg i pray for time like this

Browse Source
This commit is contained in:
acki
2026-03-09 18:33:58 +01:00
parent e2938857fb
commit 74dde8d228
3 changed files with 26 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Inc/modelec.h
View File

@@ -63,7 +63,7 @@ public:
float preciseAngleFinal = 0.017f; float preciseAngleFinal = 0.017f;
float preciseAngle = 0.39f; float preciseAngle = 0.39f;
float precisePosFinal = 0.001f; float precisePosFinal = 0.0005f;
float precisePos2 = 0.02f; float precisePos2 = 0.02f;
float precisePos = 0.1f; float precisePos = 0.1f;

38
Core/Src/modelec.cpp
View File

@@ -48,7 +48,7 @@ float DiffBot::readEncoderRight()
return distance / dt; return distance / dt;
} }
DiffBot::DiffBot(Point pos) : pos(pos) DiffBot::DiffBot(Point pos) : pos(pos), dt(0)
{ {
}; };
@@ -60,10 +60,10 @@ void DiffBot::stop(bool stop)
void DiffBot::setup() void DiffBot::setup()
{ {
pidLeft = PID(2.5f, 0.1f, 0.0f, -PWM_MAX, PWM_MAX); pidLeft = PID(4.f, 0.1f, 0.f, -PWM_MAX, PWM_MAX);
pidRight = PID(2.5f, 0.1f, 0.0f, -PWM_MAX, PWM_MAX); pidRight = PID(4.f, 0.1f, 0.f, -PWM_MAX, PWM_MAX);
pidPos = PID(3.0f, 0.0f, 0.01f, -V_MAX, V_MAX); pidPos = PID(6.0f, 0.0f, 0.1f, -V_MAX, V_MAX);
pidTheta = PID(5.0f, 0.0f, 0.01f, -M_PI, M_PI); pidTheta = PID(10.0f, 0.0f, 0.1f, -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);
@@ -92,7 +92,7 @@ void DiffBot::update(float dt_actual)
pos.y += dDistance * sinf(avgTheta); pos.y += dDistance * sinf(avgTheta);
pos.theta = normalizeAngle(pos.theta + dTheta); pos.theta = normalizeAngle(pos.theta + dTheta);
if (std::abs(rightVel) < 0.0005f && std::abs(leftVel) < 0.0005f && if (std::abs(rightVel) == 0.0f && std::abs(leftVel) == 0.0f &&
(std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0)) (std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0))
{ {
if (!no_move) if (!no_move)
@@ -102,17 +102,20 @@ void DiffBot::update(float dt_actual)
} }
else if (HAL_GetTick() - publishNotMoved > notMovedMaxTime) else if (HAL_GetTick() - publishNotMoved > notMovedMaxTime)
{ {
char log[64]; /*if (targets[index].active)
int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index); {
CDC_Transmit_FS((uint8_t*)log, len); char log[64];
sprintf(log, "SET;WAYPOINT;REACH;%d\n", index);
CDC_Transmit_FS((uint8_t*)log, strlen(log));
targets[index].active = false; targets[index].active = false;
}
index = (index + 1) % MAX_WAYPOINTS; index = (index + 1) % MAX_WAYPOINTS;
motor.stop(true); motor.stop(true);
resetPID(); resetPID();
return; return;*/
} }
} }
else else
@@ -148,7 +151,6 @@ void DiffBot::update(float dt_actual)
CDC_Transmit_FS((uint8_t*)log, len); CDC_Transmit_FS((uint8_t*)log, len);
targets[index].active = false; targets[index].active = false;
index = (index + 1) % MAX_WAYPOINTS;
motor.stop(true); motor.stop(true);
resetPID(); resetPID();
return; return;
@@ -197,8 +199,16 @@ void DiffBot::update(float dt_actual)
return limited; return limited;
}; };
motor.leftTarget_PWM = static_cast<int16_t>(applyLimits(pwmLeft)); auto applyDeadzone = [&](float pwm) {
motor.rightTarget_PWM = static_cast<int16_t>(applyLimits(pwmRight)); const float min_pwm = 35.0f;
if (std::abs(pwm) < 1.0f) return 0.0f;
if (pwm > 0) return pwm + min_pwm;
else return pwm - min_pwm;
};
motor.leftTarget_PWM = static_cast<int16_t>(applyLimits(applyDeadzone(pwmLeft)));
motor.rightTarget_PWM = static_cast<int16_t>(applyLimits(applyDeadzone(pwmRight)));
motor.update(); motor.update();
} }

2
Core/Src/motors.cpp
View File

@@ -14,7 +14,7 @@ float approach(float current, float target, float step) {
void Motor::update() { void Motor::update() {
int16_t max_step = 50; int16_t max_step = 10;
leftTarget_PWM = std::max(-PWM_MAX, std::min(leftTarget_PWM, PWM_MAX)); leftTarget_PWM = std::max(-PWM_MAX, std::min(leftTarget_PWM, PWM_MAX));
rightTarget_PWM = std::max(-PWM_MAX, std::min(rightTarget_PWM, PWM_MAX)); rightTarget_PWM = std::max(-PWM_MAX, std::min(rightTarget_PWM, PWM_MAX));