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

Formatting pass

Browse Source
This commit is contained in:
Unknown
2019-08-20 23:13:54 -04:00
parent 51e81cc8f1
commit 2b58d15d8d
20 changed files with 877 additions and 917 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+64 -69
View File
@@ -3,9 +3,9 @@
#include <functional>
#include "gpio.h"
#include "communication/interface_can.hpp"
#include "odrive_main.h"
#include "utils.h"
#include "communication/interface_can.hpp"
Axis::Axis(int axis_num,
const AxisHardwareConfig_t& hw_config,
@@ -26,13 +26,12 @@ Axis::Axis(int axis_num,
motor_(motor),
trap_(trap),
min_endstop_(min_endstop),
max_endstop_(max_endstop)
{
encoder_.axis_ = this;
max_endstop_(max_endstop) {
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
trap_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
trap_.axis_ = this;
decode_step_dir_pins();
watchdog_feed();
min_endstop_.axis_ = this;
@@ -41,29 +40,29 @@ Axis::Axis(int axis_num,
Axis::LockinConfig_t Axis::default_calibration() {
Axis::LockinConfig_t config;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 20.0f; // [rad/s^2]
config.vel = 40.0f; // [rad/s]
config.finish_distance = 100.0f * 2.0f * M_PI; // [rad]
config.finish_on_vel = false;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 20.0f; // [rad/s^2]
config.vel = 40.0f; // [rad/s]
config.finish_distance = 100.0f * 2.0f * M_PI; // [rad]
config.finish_on_vel = false;
config.finish_on_distance = true;
config.finish_on_enc_idx = true;
config.finish_on_enc_idx = true;
return config;
}
Axis::LockinConfig_t Axis::default_sensorless() {
Axis::LockinConfig_t config;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 200.0f; // [rad/s^2]
config.vel = 400.0f; // [rad/s]
config.finish_distance = 100.0f; // [rad]
config.finish_on_vel = true;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 200.0f; // [rad/s^2]
config.vel = 400.0f; // [rad/s]
config.finish_distance = 100.0f; // [rad]
config.finish_on_vel = true;
config.finish_on_distance = false;
config.finish_on_enc_idx = false;
config.finish_on_enc_idx = false;
return config;
}
@@ -71,7 +70,6 @@ static void step_cb_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->step_cb();
}
// @brief Sets up all components of the axis,
// such as gate driver and encoder hardware.
void Axis::setup() {
@@ -87,7 +85,7 @@ static void run_state_machine_loop_wrapper(void* ctx) {
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4 * 512);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
@@ -108,27 +106,27 @@ bool Axis::wait_for_current_meas() {
void Axis::step_cb() {
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;
float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
controller_.input_pos_ += dir * config_.counts_per_step;
controller_.input_pos_updated();
}
};
void Axis::load_default_step_dir_pin_config(
const AxisHardwareConfig_t& hw_config, Config_t* 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;
config->dir_gpio_pin = hw_config.dir_gpio_pin;
}
void Axis::load_default_can_id(const int& id, Config_t& config){
void Axis::load_default_can_id(const int& id, Config_t& config) {
config.can_node_id = id;
}
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);
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
@@ -136,7 +134,7 @@ void Axis::set_step_dir_active(bool active) {
if (active) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = dir_pin_;
GPIO_InitStruct.Pin = dir_pin_;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(dir_port_, &GPIO_InitStruct);
@@ -174,9 +172,8 @@ bool Axis::do_checks() {
}
}
if(board_config.power_supply_wattage > 0.0f &&
(Ibus_sum * vbus_voltage) > board_config.power_supply_wattage)
{
if (board_config.power_supply_wattage > 0.0f &&
(Ibus_sum * vbus_voltage) > board_config.power_supply_wattage) {
error_ |= ERROR_DC_BUS_OVER_POWER;
}
@@ -221,10 +218,10 @@ bool Axis::watchdog_check() {
}
}
bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
bool Axis::run_lockin_spin(const LockinConfig_t& lockin_config) {
// Spiral up current for softer rotor lock-in
lockin_state_ = LOCKIN_STATE_RAMP;
float x = 0.0f;
float x = 0.0f;
run_control_loop([&]() {
float phase = wrap_pm_pi(lockin_config.ramp_distance * x);
float I_mag = lockin_config.current * x;
@@ -233,11 +230,11 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
return false;
return x < 1.0f;
});
// Spin states
float distance = lockin_config.ramp_distance;
float phase = wrap_pm_pi(distance);
float vel = distance / lockin_config.ramp_time;
float phase = wrap_pm_pi(distance);
float vel = distance / lockin_config.ramp_time;
// Function of states to check if we are done
auto spin_done = [&](bool vel_override = false) -> bool {
@@ -260,7 +257,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
if (!motor_.update(lockin_config.current, phase, vel))
return false;
return !spin_done(true); //vel_override to go to next phase
return !spin_done(true); //vel_override to go to next phase
});
if (!encoder_.index_found_)
@@ -269,7 +266,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
// Constant speed
if (!spin_done()) {
lockin_state_ = LOCKIN_STATE_CONST_VEL;
vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error
vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error
run_control_loop([&]() {
distance += vel * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
@@ -286,7 +283,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
// 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() {
run_control_loop([this](){
run_control_loop([this]() {
if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
@@ -295,7 +292,7 @@ bool Axis::run_sensorless_control_loop() {
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_, sensorless_estimator_.vel_estimate_))
return false; // set_error should update axis.error_
return false; // set_error should update axis.error_
return true;
});
return check_for_errors();
@@ -305,14 +302,14 @@ bool Axis::run_closed_loop_control_loop() {
// 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](){
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
float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
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 false; // set_error should update axis.error_
// Handle the homing case
if (homing_.homing_state == HOMING_STATE_HOMING) {
@@ -325,21 +322,21 @@ bool Axis::run_closed_loop_control_loop() {
encoder_.set_linear_count(min_endstop_.config_.offset);
controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
controller_.input_pos_ = 0.0f;
controller_.input_pos_updated();
controller_.input_vel_ = 0.0f;
controller_.input_vel_ = 0.0f;
controller_.input_current_ = 0.0f;
homing_.homing_state = HOMING_STATE_MOVE_TO_ZERO;
}
} else if (homing_.homing_state == HOMING_STATE_MOVE_TO_ZERO) {
if(!min_endstop_.getEndstopState() && controller_.trajectory_done_){
if (!min_endstop_.getEndstopState() && controller_.trajectory_done_) {
controller_.config_.control_mode = homing_.storedControlMode;
controller_.config_.input_mode = homing_.storedInputMode;
homing_.homing_state = HOMING_STATE_IDLE;
homing_.isHomed = true;
controller_.config_.input_mode = homing_.storedInputMode;
homing_.homing_state = HOMING_STATE_IDLE;
homing_.isHomed = true;
}
} else {
// Check for endstop presses
@@ -367,7 +364,6 @@ bool Axis::run_idle_loop() {
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
// arm!
motor_.arm();
@@ -382,12 +378,11 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
if (config_.startup_encoder_offset_calibration)
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
if (config_.startup_closed_loop_control){
if(config_.startup_homing)
if (config_.startup_closed_loop_control) {
if (config_.startup_homing)
task_chain_[pos++] = AXIS_STATE_HOMING;
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
}
else if (config_.startup_sensorless_control)
} else if (config_.startup_sensorless_control)
task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
@@ -401,7 +396,7 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
requested_state_ = AXIS_STATE_UNDEFINED;
// Auto-clear any invalid state error
error_ &= ~ERROR_INVALID_STATE;
}
@@ -419,7 +414,7 @@ void Axis::run_state_machine_loop() {
case AXIS_STATE_ENCODER_INDEX_SEARCH: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0)
if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction == 0)
goto invalid_state_label;
status = encoder_.run_index_search();
@@ -443,25 +438,25 @@ void Axis::run_state_machine_loop() {
} break;
case AXIS_STATE_LOCKIN_SPIN: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
if (!motor_.is_calibrated_ || motor_.config_.direction == 0)
goto invalid_state_label;
status = run_lockin_spin(config_.lockin);
} break;
case AXIS_STATE_SENSORLESS_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired
if (!motor_.is_calibrated_ || motor_.config_.direction == 0)
goto invalid_state_label;
status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired
if (status) {
// call to controller.reset() that happend when arming means that vel_setpoint
// is zeroed. So we make the setpoint the spinup target for smooth transition.
controller_.vel_setpoint_ = config_.sensorless_ramp.vel;
status = run_sensorless_control_loop();
status = run_sensorless_control_loop();
}
} break;
case AXIS_STATE_CLOSED_LOOP_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
if (!motor_.is_calibrated_ || motor_.config_.direction == 0)
goto invalid_state_label;
if (!encoder_.is_ready_)
goto invalid_state_label;
@@ -470,7 +465,7 @@ void Axis::run_state_machine_loop() {
case AXIS_STATE_IDLE: {
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
status = motor_.arm(); // done with idling - try to arm the motor
} break;
default: