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

Merge branch 'anticogging_saver' into RazorsEdge

Browse Source
This commit is contained in:
Paul Guenette
2019-05-25 14:16:46 +02:00
parent 820fd72ae8 301d68adc6
commit cc36edeabf
6 changed files with 36 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/controller.cpp
+18 -16
View File
@@ -1,6 +1,7 @@
#include "odrive_main.h"
#include <algorithm>
Controller::Controller(Config_t& config) :
config_(config)
@@ -50,8 +51,8 @@ void Controller::move_incremental(float displacement, bool from_goal_point = tru
void Controller::start_anticogging_calibration() {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) {
anticogging_.calib_anticogging = true;
if (axis_->error_ == Axis::ERROR_NONE) {
config_.anticogging.calib_anticogging = true;
}
}
@@ -78,19 +79,20 @@ bool Controller::home_axis() {
* This holding current is added as a feedforward term in the control loop.
*/
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) {
float pos_err = anticogging_.index - pos_estimate;
if (fabsf(pos_err) <= anticogging_.calib_pos_threshold &&
fabsf(vel_estimate) < anticogging_.calib_vel_threshold) {
anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_;
if (config_.anticogging.calib_anticogging) {
float pos_err = config_.anticogging.index - pos_estimate;
if (fabsf(pos_err) <= config_.anticogging.calib_pos_threshold &&
fabsf(vel_estimate) < config_.anticogging.calib_vel_threshold) {
config_.anticogging.cogging_map[std::clamp(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_;
}
if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR
pos_setpoint_ = anticogging_.index;
if (config_.anticogging.index < 3600) {
set_pos_setpoint(config_.anticogging.index * config_.anticogging.cogging_ratio, 0.0f, 0.0f);
return false;
} else {
anticogging_.index = 0;
anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging
anticogging_.calib_anticogging = false;
config_.anticogging.index = 0;
set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home
config_.anticogging.use_anticogging = true; // We're good to go, enable anti-cogging
config_.anticogging.calib_anticogging = false;
return true;
}
}
@@ -103,9 +105,9 @@ void Controller::update_filter_gains() {
}
bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
// Only runs if anticogging_.calib_anticogging is true; non-blocking
// Only runs if config_.anticogging.calib_anticogging is true; non-blocking
anticogging_calibration(pos_estimate, vel_estimate);
float anticogging_pos = pos_estimate;
float anticogging_pos = pos_estimate / config_.anticogging.cogging_ratio;
// Update inputs
switch (config_.input_mode) {
@@ -207,8 +209,8 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
// Anti-cogging is enabled after calibration
// We get the current position and apply a current feed-forward
// ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
if (anticogging_.use_anticogging) {
Iq += anticogging_.cogging_map[mod(static_cast<int>(anticogging_pos), axis_->encoder_.config_.cpr)];
if (config_.anticogging.use_anticogging) {
Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast<int>(anticogging_pos), axis_->encoder_.config_.cpr), 0, 3600)];
}
float v_err = vel_des - vel_estimate;