From 5df4ceda33ccacf73795958c40cdfbda2b8e484d Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 31 Aug 2019 17:41:32 -0400 Subject: [PATCH] Add current ramping input mode --- Firmware/MotorControl/controller.cpp | 7 +++++++ Firmware/MotorControl/controller.hpp | 2 ++ tools/odrive/enums.py | 1 + 3 files changed, 10 insertions(+) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 088b6d15..47b27db9 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -150,6 +150,13 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s vel_setpoint_ += step; current_setpoint_ = step / current_meas_period * config_.inertia; } break; + case INPUT_MODE_CURRENT_RAMP: { + float max_step_size = std::abs(current_meas_period * config_.current_ramp_rate); + float full_step = input_current_ - current_setpoint_; + float step = std::clamp(full_step, -max_step_size, max_step_size); + + current_setpoint_ += step; + } break; case INPUT_MODE_POS_FILTER: { // 2nd order pos tracking filter float delta_pos = input_pos_ - pos_setpoint_; // Pos error diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index a2370f49..b6a8a339 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -30,6 +30,7 @@ class Controller { INPUT_MODE_POS_FILTER, INPUT_MODE_MIX_CHANNELS, INPUT_MODE_TRAP_TRAJ, + INPUT_MODE_CURRENT_RAMP, }; typedef struct { @@ -53,6 +54,7 @@ class Controller { float vel_limit = 20000.0f; // [counts/s] float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable float vel_ramp_rate = 10000.0f; // [(counts/s) / s] + float current_ramp_rate = 1.0f; // A / sec bool setpoints_in_cpr = false; float inertia = 0.0f; // [A/(count/s^2)] float input_filter_bandwidth = 2.0f; // [1/s] diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 1ac3d67c..c1e1232a 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -73,6 +73,7 @@ INPUT_MODE_VEL_RAMP = 2 INPUT_MODE_POS_FILTER = 3 INPUT_MODE_MIX_CHANNELS = 4 INPUT_MODE_TRAP_TRAJ = 5 +INPUT_MODE_CURRENT_RAMP = 6 ENCODER_MODE_INCREMENTAL = 0x00 ENCODER_MODE_HALL = 0x01