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

Merge branch 'devel' of https://github.com/madcowswe/ODrive into devel

Browse Source
This commit is contained in:
Oskar Weigl
2019-03-05 21:47:39 -08:00
parent c4c5aaac57 6a9ff00b7d
commit 615ae723c3
18 changed files with 317 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/encoder.cpp
+48 -29
View File
@@ -20,8 +20,7 @@ static void enc_index_cb_wrapper(void* ctx) {
void Encoder::setup() {
HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
enc_index_cb_wrapper, this);
set_idx_subscribe();
}
void Encoder::set_error(Error_t error) {
@@ -56,6 +55,20 @@ void Encoder::enc_index_cb() {
}
index_found_ = true;
}
// Disable interrupt
GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
}
void Encoder::set_idx_subscribe(bool override_enable) {
if (override_enable || (config_.use_index && !config_.find_idx_on_lockin_only)) {
GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN,
enc_index_cb_wrapper, this);
}
if (!config_.use_index || config_.find_idx_on_lockin_only) {
GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
}
}
// Function that sets the current encoder count to a desired 32-bit value.
@@ -90,36 +103,42 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
cpu_exit_critical(prim);
}
// @brief Slowly turns the motor in one direction until the
// encoder index is found.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_index_search() {
float voltage_magnitude;
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
voltage_magnitude = axis_->motor_.config_.calibration_current;
else
return false;
float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed;
config_.use_index = true;
index_found_ = false;
float phase = 0.0f;
axis_->run_control_loop([&](){
phase = wrap_pm_pi(phase + omega * current_meas_period);
if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
axis_->motor_.config_.direction = 1;
}
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))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH);
bool orig_finish_on_enc_idx = axis_->config_.lockin.finish_on_enc_idx;
axis_->config_.lockin.finish_on_enc_idx = true;
bool status = axis_->run_lockin_spin();
axis_->config_.lockin.finish_on_enc_idx = orig_finish_on_enc_idx;
return status;
}
// continue until the index is found
return !index_found_;
});
return true;
bool Encoder::run_direction_find() {
int32_t init_enc_val = shadow_count_;
bool orig_finish_on_distance = axis_->config_.lockin.finish_on_distance;
axis_->config_.lockin.finish_on_distance = true;
axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
bool status = axis_->run_lockin_spin();
axis_->config_.lockin.finish_on_distance = orig_finish_on_distance;
if (status) {
// Check response and direction
if (shadow_count_ > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
} else if (shadow_count_ < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
} else {
axis_->motor_.config_.direction = 0;
}
}
return status;
}
// @brief Turns the motor in one direction for a bit and then in the other
@@ -342,7 +361,7 @@ bool Encoder::update() {
//// run encoder count interpolation
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) {
if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
interpolation_ = 0.5f;
// reset interpolation if encoder edge comes
} else if (delta_enc > 0) {