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

force offset cal after idx search, index now homes axis

Browse Source
This commit is contained in:
Oskar Weigl
2018-08-17 23:39:43 -07:00
parent e097a6d264
commit 19e3c779ae
5 changed files with 77 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/encoder.cpp
+34 -24
View File
@@ -8,7 +8,6 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
config_(config)
{
if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) {
offset_ = config.offset;
is_ready_ = true;
}
}
@@ -38,13 +37,17 @@ bool Encoder::do_checks(){
// Triggered when an encoder passes over the "Index" pin
// TODO: only arm index edge interrupt when we know encoder has powered up
// (maybe by attaching the interrupt on start search, synergistic with following)
// TODO: disable interrupt once we found the index
void Encoder::enc_index_cb() {
if (config_.use_index && !index_found_) {
set_circular_count(0);
set_linear_count(0); // Avoid position control transient after search
if (config_.pre_calibrated) {
offset_ = config_.offset;
is_ready_ = true;
} else {
// Invalidate offset calibration that may have happened before idx search
is_ready_ = false;
}
index_found_ = true;
}
@@ -73,8 +76,13 @@ void Encoder::set_circular_count(int32_t count) {
__disable_irq();
// Offset and state must be shifted by the same amount
offset_ += count - count_in_cpr_;
offset_ = mod(offset_, config_.cpr);
// Note that if the linear count is also cleared before running an update,
// the offset will drift by at least one count. Therefore we shouldn't rely
// on this during index search callback.
// Hence we invalidate calibration in enc_index_cb
config_.offset += count - count_in_cpr_;
config_.offset = mod(config_.offset, config_.cpr);
// Update states
count_in_cpr_ = mod(count, config_.cpr);
pos_cpr_ = (float)count_in_cpr_;
@@ -124,10 +132,11 @@ bool Encoder::run_offset_calibration() {
static const float scan_distance = 16.0f * M_PI;
static const int num_steps = (int)(scan_distance / scan_omega * (float)current_meas_hz);
// Temporarily disable index search so it doesn't mess
// with the offset calibration
bool old_use_index = config_.use_index;
config_.use_index = false;
// Require index found if enabled
if (config_.use_index && !index_found_) {
set_error(ERROR_INDEX_NOT_FOUND_YET);
return false;
}
// We use shadow_count_ to do the calibration, but the offset is used by count_in_cpr_
// Therefore we have to sync them for calibration
@@ -172,16 +181,7 @@ bool Encoder::run_offset_calibration() {
if (axis_->error_ != Axis::ERROR_NONE)
return false;
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = scan_distance / elec_rad_per_enc;
float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val);
if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
set_error(ERROR_CPR_OUT_OF_RANGE);
return false;
}
// check direction
// Check response and direction
if (shadow_count_ > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
@@ -190,7 +190,18 @@ bool Encoder::run_offset_calibration() {
axis_->motor_.config_.direction = -1;
} else {
// Encoder response error
set_error(ERROR_RESPONSE);
set_error(ERROR_NO_RESPONSE);
return false;
}
//TODO avoid recomputing elec_rad_per_enc every time
// Check CPR
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = scan_distance / elec_rad_per_enc;
float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val);
if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
set_error(ERROR_CPR_OUT_OF_RANGE);
return false;
}
@@ -211,12 +222,11 @@ bool Encoder::run_offset_calibration() {
if (axis_->error_ != Axis::ERROR_NONE)
return false;
offset_ = encvaluesum / (num_steps * 2);
config_.offset = offset_;
int32_t residual = encvaluesum - ((int64_t)offset_ * (int64_t)(num_steps * 2));
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
is_ready_ = true;
config_.use_index = old_use_index;
return true;
}
@@ -296,7 +306,7 @@ bool Encoder::update() {
}
//// run encoder count interpolation
int32_t corrected_enc = count_in_cpr_ - offset_;
int32_t corrected_enc = count_in_cpr_ - config_.offset;
// if we are stopped, make sure we don't randomly drift
if (snap_to_zero_vel) {
interpolation_ = 0.5f;