Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 23:44:48 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Modify vel_ramp logic slightly to be more STL-based

Browse Source
This commit is contained in:
Paul Guenette
2019-07-09 21:31:04 +02:00
parent 59cbbbb8f9
commit 0ab104346f
2 changed files with 45 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/controller.cpp
+3 -7
View File
@@ -138,14 +138,10 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
current_setpoint_ = input_current_;
} break;
case INPUT_MODE_VEL_RAMP: {
float max_step_size = current_meas_period * config_.vel_ramp_rate;
float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate);
float full_step = input_vel_ - vel_setpoint_;
float step;
if (fabsf(full_step) > max_step_size) {
step = std::copysignf(max_step_size, full_step);
} else {
step = full_step;
}
float step = std::clamp(full_step, -max_step_size, max_step_size);
vel_setpoint_ += step;
current_setpoint_ = step / current_meas_period * config_.inertia;
} break;
Firmware/Tests/test_runner.cpp
+42
View File
@@ -274,4 +274,46 @@ using doctest::Approx;
CHECK(limitVel(20000.0f, 1000.0f, 5.0E-4f, 30.0f) == 9.5f);
CHECK(limitVel(20000.0f, -1000.0f, 5.0E-4f, 30.0f) == Approx(10.5f));
}
}
TEST_SUITE("vel_ramp") {
float vel_ramp_old(float input_vel_, float vel_setpoint_, float vel_ramp_rate) {
float max_step_size = 0.000125f * vel_ramp_rate;
float full_step = input_vel_ - vel_setpoint_;
float step;
if (fabsf(full_step) > max_step_size) {
step = std::copysignf(max_step_size, full_step);
} else {
step = full_step;
}
return step;
}
float vel_ramp_new(float input_vel_, float vel_setpoint_, float vel_ramp_rate){
float max_step_size = 0.000125f * vel_ramp_rate;
float full_step = input_vel_ - vel_setpoint_;
return std::clamp(full_step, -max_step_size, max_step_size);
}
TEST_CASE("Blah") {
float vel_setpoint = 0.0f;
float vel_ramp_rate = 8000;
float input_vel = 0.0f;
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
input_vel = 10.0f;
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
input_vel = 10000.0f;
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
input_vel = -10000.0f;
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
input_vel = -0.1234f;
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
input_vel = 0.1234f;
CHECK(vel_ramp_old(input_vel, vel_setpoint, vel_ramp_rate) == vel_ramp_new(input_vel, vel_setpoint, vel_ramp_rate));
}
}