From 192c3260f8f7dfb10d362683d1c80b6d10657488 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 2 Dec 2018 19:08:12 -0800 Subject: [PATCH] refactor vel ramp mode to use new controller input handling --- CHANGELOG.md | 1 + Firmware/MotorControl/controller.cpp | 48 ++++++++++++++++++++-------- Firmware/MotorControl/controller.hpp | 24 +++++++++++--- docs/getting-started.md | 18 ++--------- tools/odrive/enums.py | 6 ++++ 5 files changed, 64 insertions(+), 33 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7a4324fc..0f8e8ba0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,7 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Changed * Moved `traptraj.A_per_css` to `controller.inertia` +* Refactored velocity ramp mode into the new general input filtering structure # Releases ## [0.4.7] - 2018-11-28 diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 1e2ad7cd..fea22d5d 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -93,11 +93,46 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) return false; } + + bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { // Only runs if anticogging_.calib_anticogging is true; non-blocking anticogging_calibration(pos_estimate, vel_estimate); float anticogging_pos = pos_estimate; + // Update inputs + switch (config_.input_mode) { + case INPUT_MODE_INACTIVE: { + // do nothing + } break; + case INPUT_MODE_PASSTHROUGH: { + pos_setpoint_ = input_pos_; + vel_setpoint_ = input_vel_; + current_setpoint_ = input_current_; + } break; + case INPUT_MODE_VEL_RAMP: { + float max_step_size = 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; + } + vel_setpoint_ += step; + } break; + case INPUT_MODE_POS_FILTER: { + + } break; + // case INPUT_MODE_MIX_CHANNELS: { + // // NOT YET IMPLEMENTED + // } break; + default: { + set_error(ERROR_INVALID_INPUT_MODE); + return false; + } + } + // Trajectory control if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) { // Note: uint32_t loop count delta is OK across overflow @@ -118,19 +153,6 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } - // Ramp rate limited velocity setpoint - if (config_.control_mode == CTRL_MODE_VELOCITY_CONTROL && vel_ramp_enable_) { - float max_step_size = current_meas_period * config_.vel_ramp_rate; - float full_step = vel_ramp_target_ - vel_setpoint_; - float step; - if (fabsf(full_step) > max_step_size) { - step = std::copysignf(max_step_size, full_step); - } else { - step = full_step; - } - vel_setpoint_ += step; - } - // Position control // TODO Decide if we want to use encoder or pll position here float vel_des = vel_setpoint_; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index a12172a2..b2786bc4 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -10,6 +10,7 @@ public: enum Error_t { ERROR_NONE = 0, ERROR_OVERSPEED = 0x01, + ERROR_INVALID_INPUT_MODE = 0x02, }; // Note: these should be sorted from lowest level of control to @@ -22,8 +23,17 @@ public: CTRL_MODE_TRAJECTORY_CONTROL = 4 }; + enum InputMode_t{ + INPUT_MODE_INACTIVE, + INPUT_MODE_PASSTHROUGH, + INPUT_MODE_VEL_RAMP, + INPUT_MODE_POS_FILTER, + INPUT_MODE_MIX_CHANNELS, + }; + struct Config_t { - ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t + ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: ControlMode_t + InputMode_t input_mode = INPUT_MODE_INACTIVE; //see: InputMode_t float pos_gain = 20.0f; // [(counts/s) / counts] float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] @@ -85,8 +95,10 @@ public: // float vel_setpoint = 800.0f; float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] - float vel_ramp_target_ = 0.0f; - bool vel_ramp_enable_ = false; + + float input_pos_ = 0.0f; + float input_vel_ = 0.0f; + float input_current_ = 0.0f; uint32_t traj_start_loop_count_ = 0; @@ -94,14 +106,16 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), + make_protocol_property("input_pos", &input_pos_), + make_protocol_property("input_vel", &input_vel_), + make_protocol_property("input_current", &input_current_), make_protocol_property("pos_setpoint", &pos_setpoint_), make_protocol_property("vel_setpoint", &vel_setpoint_), make_protocol_property("vel_integrator_current", &vel_integrator_current_), make_protocol_property("current_setpoint", ¤t_setpoint_), - make_protocol_property("vel_ramp_target", &vel_ramp_target_), - make_protocol_property("vel_ramp_enable", &vel_ramp_enable_), make_protocol_object("config", make_protocol_property("control_mode", &config_.control_mode), + make_protocol_property("input_mode", &config_.input_mode), make_protocol_property("pos_gain", &config_.pos_gain), make_protocol_property("vel_gain", &config_.vel_gain), make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), diff --git a/docs/getting-started.md b/docs/getting-started.md index 41fcfd7c..45be4328 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -7,19 +7,7 @@ permalink: / # Getting Started ### Table of contents - - -- [Hardware Requirements](#hardware-requirements) -- [Wiring up the ODrive](#wiring-up-the-odrive) -- [Downloading and Installing Tools](#downloading-and-installing-tools) -- [Firmware](#firmware) -- [Start `odrivetool`](#start-odrivetool) -- [Configure M0](#configure-m0) -- [Position control of M0](#position-control-of-m0) -- [Other control modes](#other-control-modes) -- [What's next?](#whats-next) - - +autoauto- [Hardware Requirements](#hardware-requirements)auto- [Wiring up the ODrive](#wiring-up-the-odrive)auto- [Downloading and Installing Tools](#downloading-and-installing-tools)auto- [Firmware](#firmware)auto- [Start `odrivetool`](#start-odrivetool)auto- [Configure M0](#configure-m0)auto- [Position control of M0](#position-control-of-m0)auto- [Other control modes](#other-control-modes)auto- [What's next?](#whats-next)autoauto ## Hardware Requirements @@ -303,8 +291,8 @@ You can now control the velocity with `axis.controller.vel_setpoint = 5000` [cou ### Ramped velocity control Set `axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL`.
Set the velocity ramp rate (acceleration): `axis.controller.config.vel_ramp_rate = 2000` [counts/s^2]
-Activate the ramped velocity mode: `axis.controller.vel_ramp_enable = True`.
-You can now control the velocity with `axis.controller.vel_ramp_target = 5000` [count/s]. +Activate the ramped velocity mode: `axis.controller.config.input_mode = INPUT_MODE_VEL_RAMP`.
+You can now control the velocity with `axis.controller.input_vel = 5000` [count/s]. ### Current control Set `axis.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL`.
diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 19572c90..038b77cc 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -32,5 +32,11 @@ CTRL_MODE_CURRENT_CONTROL = 1 CTRL_MODE_VELOCITY_CONTROL = 2 CTRL_MODE_POSITION_CONTROL = 3 +INPUT_MODE_INACTIVE = 0 +INPUT_MODE_PASSTHROUGH = 1 +INPUT_MODE_VEL_RAMP = 2 +INPUT_MODE_POS_FILTER = 3 +INPUT_MODE_MIX_CHANNELS = 4 + ENCODER_MODE_INCREMENTAL = 0 ENCODER_MODE_HALL = 1