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

Merge branch 'devel' into preroll

Browse Source
This commit is contained in:
Oskar Weigl
2019-02-10 20:11:18 -08:00
parent d7644daaca 1e4b71b67b
commit 2a12c1ac61
72 changed files with 1196 additions and 7410 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+35 -25
View File
@@ -26,6 +26,8 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
controller_.axis_ = this;
motor_.axis_ = this;
trap_.axis_ = this;
decode_step_dir_pins();
}
static void step_cb_wrapper(void* ctx) {
@@ -66,33 +68,46 @@ bool Axis::wait_for_current_meas() {
// step/direction interface
void Axis::step_cb() {
if (enable_step_dir_) {
GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config_.dir_port, hw_config_.dir_pin);
if (step_dir_active_) {
GPIO_PinState dir_pin = HAL_GPIO_ReadPin(dir_port_, dir_pin_);
float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
controller_.pos_setpoint_ += dir * config_.counts_per_step;
}
};
// @brief Enables or disables step/dir input
void Axis::set_step_dir_enabled(bool enable) {
if (enable) {
void Axis::load_default_step_dir_pin_config(
const AxisHardwareConfig_t& hw_config, Config_t* config) {
config->step_gpio_pin = hw_config.step_gpio_pin;
config->dir_gpio_pin = hw_config.dir_gpio_pin;
}
void Axis::decode_step_dir_pins() {
step_port_ = get_gpio_port_by_pin(config_.step_gpio_pin);
step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin);
dir_port_ = get_gpio_port_by_pin(config_.dir_gpio_pin);
dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
}
// @brief (de)activates step/dir input
void Axis::set_step_dir_active(bool active) {
if (active) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = hw_config_.dir_pin;
GPIO_InitStruct.Pin = dir_pin_;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(hw_config_.dir_port, &GPIO_InitStruct);
HAL_GPIO_Init(dir_port_, &GPIO_InitStruct);
// Subscribe to rising edges of the step GPIO
GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN,
GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN,
step_cb_wrapper, this);
enable_step_dir_ = true;
step_dir_active_ = true;
} else {
enable_step_dir_ = false;
step_dir_active_ = false;
// Unsubscribe from step GPIO
GPIO_unsubscribe(hw_config_.step_port, hw_config_.step_pin);
GPIO_unsubscribe(step_port_, step_pin_);
}
}
@@ -126,12 +141,6 @@ bool Axis::do_updates() {
return check_for_errors();
}
float Axis::get_temp() {
float adc = adc_measurements_[hw_config_.thermistor_adc_ch];
float normalized_voltage = adc / adc_full_scale;
return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs);
}
bool Axis::run_lockin_spin() {
// Spiral up current for softer rotor lock-in
lockin_state_ = LOCKIN_STATE_RAMP;
@@ -140,7 +149,7 @@ bool Axis::run_lockin_spin() {
float phase = wrap_pm_pi(config_.lockin_ramp_distance * x);
float I_mag = config_.lockin_current * x;
x += current_meas_period / config_.lockin_ramp_time;
if (!motor_.update(I_mag, phase))
if (!motor_.update(I_mag, phase, 0.0f))
return false;
return x < 1.0f;
});
@@ -169,7 +178,7 @@ bool Axis::run_lockin_spin() {
distance += vel * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
if (!motor_.update(config_.lockin_current, phase))
if (!motor_.update(config_.lockin_current, phase, vel))
return false;
return !spin_done(true); //vel_override to go to next phase
});
@@ -194,7 +203,6 @@ bool Axis::run_lockin_spin() {
// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
bool Axis::run_sensorless_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
@@ -203,26 +211,28 @@ bool Axis::run_sensorless_control_loop() {
float current_setpoint;
if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, sensorless_estimator_.phase_))
if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
return false; // set_error should update axis.error_
return true;
});
set_step_dir_enabled(false);
return check_for_errors();
}
bool Axis::run_closed_loop_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
// To avoid any transient on startup, we intialize the setpoint to be the current position
controller_.pos_setpoint_ = encoder_.pos_estimate_;
set_step_dir_active(config_.enable_step_dir);
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
if (!motor_.update(current_setpoint, encoder_.phase_))
float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
return false; // set_error should update axis.error_
return true;
});
set_step_dir_enabled(false);
set_step_dir_active(false);
return check_for_errors();
}