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

add encoder offset calib debug var calib_scan_response

Browse Source
This commit is contained in:
Oskar Weigl
2019-04-01 21:22:27 -07:00
parent c5c792f6c0
commit 1761272cad
2 changed files with 14 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/encoder.cpp
+6 -8
View File
@@ -163,9 +163,7 @@ bool Encoder::run_direction_find() {
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
static const float start_lock_duration = 1.0f;
static const float scan_omega = 4.0f * M_PI;
static const float scan_distance = 16.0f * M_PI;
static const int num_steps = (int)(scan_distance / scan_omega * (float)current_meas_hz);
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -202,7 +200,7 @@ bool Encoder::run_offset_calibration() {
// scan forward
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
@@ -232,9 +230,9 @@ bool Encoder::run_offset_calibration() {
//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)
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
calib_scan_response_ = fabsf(shadow_count_-init_enc_val);
if(fabsf(calib_scan_response_ - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
set_error(ERROR_CPR_OUT_OF_RANGE);
return false;
@@ -243,7 +241,7 @@ bool Encoder::run_offset_calibration() {
// scan backwards
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
Firmware/MotorControl/encoder.hpp
+8 -2
View File
@@ -37,6 +37,8 @@ public:
float offset_float = 0.0f; // Sub-count phase alignment offset
bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
float calib_scan_distance = 16.0f * M_PI; // rad electrical
float calib_scan_omega = 4.0f * M_PI; // rad/s electrical
float bandwidth = 1000.0f;
bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
bool idx_search_unidirectional = false; // Only allow index search in known direction
@@ -83,6 +85,7 @@ public:
float vel_estimate_ = 0.0f; // [count/s]
float pll_kp_ = 0.0f; // [count/s / count]
float pll_ki_ = 0.0f; // [(count/s^2) / count]
float calib_scan_response_ = 0.0f; // debug report from offset calib
int16_t tim_cnt_sample_ = 0; //
// Updated by low_level pwm_adc_cb
@@ -99,11 +102,12 @@ public:
make_protocol_property("shadow_count", &shadow_count_),
make_protocol_property("count_in_cpr", &count_in_cpr_),
make_protocol_property("interpolation", &interpolation_),
make_protocol_property("phase", &phase_),
make_protocol_ro_property("phase", &phase_),
make_protocol_property("pos_estimate", &pos_estimate_),
make_protocol_property("pos_cpr", &pos_cpr_),
make_protocol_property("hall_state", &hall_state_),
make_protocol_ro_property("hall_state", &hall_state_),
make_protocol_property("vel_estimate", &vel_estimate_),
make_protocol_ro_property("calib_scan_response", &calib_scan_response_),
// make_protocol_property("pll_kp", &pll_kp_),
// make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
@@ -122,6 +126,8 @@ public:
make_protocol_property("bandwidth", &config_.bandwidth,
[](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
make_protocol_property("calib_range", &config_.calib_range),
make_protocol_property("calib_scan_distance", &config_.calib_scan_distance),
make_protocol_property("calib_scan_omega", &config_.calib_scan_omega),
make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional),
make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
),