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 pull request #312 from madcowswe/preroll

Browse Source 
Unify lockin drive modes
This commit is contained in:
Oskar Weigl
2019-02-25 20:23:57 -08:00
committed by GitHub
parent d25a376f66 1178bf4412
commit 80b4582df9
8 changed files with 219 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+3
View File
@@ -7,6 +7,9 @@ Please add a note of your changes below this heading if you make a Pull Request.
* Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop.
* Thermal current limit with linear derating.
### Changed
* Unified lockin drive modes. Current for index searching and encoder offset calibration now moved to axis.lockin.current.
### Fixed
* Added required 1.5 cycle phase shift between ADC and PWM, lack thereof caused unstable current controller at high eRPM.
Firmware/Board/v3/Src/stm32f4xx_it.c
+23 -11
View File
@@ -91,22 +91,34 @@ void NMI_Handler(void)
/* USER CODE END NonMaskableInt_IRQn 1 */
}
void get_regs(void** stack_ptr) {
void* volatile r0 __attribute__((unused)) = stack_ptr[0];
void* volatile r1 __attribute__((unused)) = stack_ptr[1];
void* volatile r2 __attribute__((unused)) = stack_ptr[2];
void* volatile r3 __attribute__((unused)) = stack_ptr[3];
void* volatile r12 __attribute__((unused)) = stack_ptr[4];
void* volatile lr __attribute__((unused)) = stack_ptr[5]; // Link register
void* volatile pc __attribute__((unused)) = stack_ptr[6]; // Program counter
void* volatile psr __attribute__((unused)) = stack_ptr[7]; // Program status register
volatile bool stay_looping = true;
while(stay_looping);
}
/**
* @brief This function handles Hard fault interrupt.
*/
__attribute__((naked))
void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
/* USER CODE END W1_HardFault_IRQn 0 */
}
/* USER CODE BEGIN HardFault_IRQn 1 */
/* USER CODE END HardFault_IRQn 1 */
__asm(
" tst lr, #4 \n\t"
" ite eq \n\t"
" mrseq r0, msp \n\t"
" mrsne r0, psp \n\t"
" b get_regs \n\t"
);
}
/**
Firmware/MotorControl/axis.cpp
+96 -41
View File
@@ -141,36 +141,66 @@ bool Axis::do_updates() {
return check_for_errors();
}
bool Axis::run_sensorless_spin_up() {
// Early Spin-up: spiral up current
bool Axis::run_lockin_spin() {
// Spiral up current for softer rotor lock-in
lockin_state_ = LOCKIN_STATE_RAMP;
float x = 0.0f;
run_control_loop([&](){
float phase = wrap_pm_pi(config_.ramp_up_distance * x);
float I_mag = config_.spin_up_current * x;
x += current_meas_period / config_.ramp_up_time;
run_control_loop([&]() {
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, 0.0f))
return error_ |= ERROR_MOTOR_FAILED, false;
return false;
return x < 1.0f;
});
if (error_ != ERROR_NONE)
return false;
// Late Spin-up: accelerate
float vel = config_.ramp_up_distance / config_.ramp_up_time;
float phase = wrap_pm_pi(config_.ramp_up_distance);
run_control_loop([&](){
vel += config_.spin_up_acceleration * current_meas_period;
// Spin states
float distance = config_.lockin.ramp_distance;
float phase = wrap_pm_pi(distance);
float vel = distance / config_.lockin.ramp_time;
// Function of states to check if we are done
auto spin_done = [&](bool vel_override = false) -> bool {
bool done = false;
if (config_.lockin.finish_on_vel || vel_override)
done = done || fabsf(vel) >= fabsf(config_.lockin.vel);
if (config_.lockin.finish_on_distance)
done = done || fabsf(distance) >= fabsf(config_.lockin.finish_distance);
if (config_.lockin.finish_on_enc_idx)
done = done || encoder_.index_found_;
return done;
};
// Accelerate
lockin_state_ = LOCKIN_STATE_ACCELERATE;
run_control_loop([&]() {
vel += config_.lockin.accel * current_meas_period;
distance += vel * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
float I_mag = config_.spin_up_current;
if (!motor_.update(I_mag, phase, vel))
return error_ |= ERROR_MOTOR_FAILED, false;
return vel < config_.spin_up_target_vel;
if (!motor_.update(config_.lockin.current, phase, vel))
return false;
return !spin_done(true); //vel_override to go to next phase
});
// 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_.spin_up_target_vel;
if (!encoder_.index_found_)
encoder_.set_idx_subscribe(true);
// Constant speed
if (!spin_done()) {
lockin_state_ = LOCKIN_STATE_CONST_VEL;
vel = config_.lockin.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);
if (!motor_.update(config_.lockin.current, phase, vel))
return false;
return !spin_done();
});
}
lockin_state_ = LOCKIN_STATE_INACTIVE;
return check_for_errors();
}
@@ -270,44 +300,69 @@ void Axis::run_state_machine_loop() {
// Note that current_state is a reference to task_chain_[0]
// Validate the state before running it
if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_)
current_state_ = AXIS_STATE_UNDEFINED;
if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_)
current_state_ = AXIS_STATE_UNDEFINED;
// Run the specified state
// Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
bool status;
switch (current_state_) {
case AXIS_STATE_MOTOR_CALIBRATION:
case AXIS_STATE_MOTOR_CALIBRATION: {
status = motor_.run_calibration();
break;
} break;
case AXIS_STATE_ENCODER_INDEX_SEARCH: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0)
goto invalid_state_label;
case AXIS_STATE_ENCODER_INDEX_SEARCH:
status = encoder_.run_index_search();
break;
} break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION:
case AXIS_STATE_ENCODER_DIR_FIND: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
status = encoder_.run_direction_find();
} break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
status = encoder_.run_offset_calibration();
break;
} break;
case AXIS_STATE_SENSORLESS_CONTROL:
status = run_sensorless_spin_up(); // TODO: restart if desired
if (status)
case AXIS_STATE_LOCKIN_SPIN: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
status = run_lockin_spin();
} break;
case AXIS_STATE_SENSORLESS_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
status = run_lockin_spin(); // 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_.lockin.vel;
status = run_sensorless_control_loop();
break;
}
} break;
case AXIS_STATE_CLOSED_LOOP_CONTROL:
case AXIS_STATE_CLOSED_LOOP_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
if (!encoder_.is_ready_)
goto invalid_state_label;
status = run_closed_loop_control_loop();
break;
} break;
case AXIS_STATE_IDLE:
case AXIS_STATE_IDLE: {
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
break;
} break;
default:
invalid_state_label:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
break;
Firmware/MotorControl/axis.hpp
+37 -16
View File
@@ -22,8 +22,6 @@ public:
ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
};
// Warning: Do not reorder these enum values.
// The state machine uses ">" comparision on them.
enum State_t {
AXIS_STATE_UNDEFINED = 0, //<! will fall through to idle
AXIS_STATE_IDLE = 1, //<! disable PWM and do nothing
@@ -33,7 +31,21 @@ public:
AXIS_STATE_SENSORLESS_CONTROL = 5, //<! run sensorless control
AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
AXIS_STATE_CLOSED_LOOP_CONTROL = 8 //<! run closed loop control
AXIS_STATE_CLOSED_LOOP_CONTROL = 8, //<! run closed loop control
AXIS_STATE_LOCKIN_SPIN = 9, //<! run lockin spin
AXIS_STATE_ENCODER_DIR_FIND = 10,
};
struct LockinConfig_t {
float current = 10.0f; // [A]
float ramp_time = 0.4f; // [s]
float ramp_distance = 1 * M_PI; // [rad]
float accel = 20.0f; // [rad/s^2]
float vel = 40.0f; // [rad/s]
float finish_distance = 100.0f; // [rad]
bool finish_on_vel = false;
bool finish_on_distance = false;
bool finish_on_enc_idx = false;
};
struct Config_t {
@@ -51,18 +63,20 @@ public:
uint16_t step_gpio_pin = 0;
uint16_t dir_gpio_pin = 0;
// Spinup settings
float ramp_up_time = 0.4f; // [s]
float ramp_up_distance = 4 * M_PI; // [rad]
float spin_up_current = 10.0f; // [A]
float spin_up_acceleration = 400.0f; // [rad/s^2]
float spin_up_target_vel = 400.0f; // [rad/s]
LockinConfig_t lockin;
};
enum thread_signals {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
};
enum LockinState_t {
LOCKIN_STATE_INACTIVE,
LOCKIN_STATE_RAMP,
LOCKIN_STATE_ACCELERATE,
LOCKIN_STATE_CONST_VEL,
};
Axis(const AxisHardwareConfig_t& hw_config,
Config_t& config,
Encoder& encoder,
@@ -87,7 +101,6 @@ public:
bool do_checks();
bool do_updates();
// True if there are no errors
bool inline check_for_errors() {
return error_ == ERROR_NONE;
@@ -151,7 +164,7 @@ public:
}
}
bool run_sensorless_spin_up();
bool run_lockin_spin();
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_idle_loop();
@@ -184,6 +197,7 @@ public:
State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
State_t& current_state_ = task_chain_[0];
uint32_t loop_counter_ = 0;
LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
// Communication protocol definitions
auto make_protocol_definitions() {
@@ -193,6 +207,7 @@ public:
make_protocol_ro_property("current_state", &current_state_),
make_protocol_property("requested_state", &requested_state_),
make_protocol_ro_property("loop_counter", &loop_counter_),
make_protocol_ro_property("lockin_state", &lockin_state_),
make_protocol_object("config",
make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
@@ -205,11 +220,17 @@ public:
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_property("ramp_up_time", &config_.ramp_up_time),
make_protocol_property("ramp_up_distance", &config_.ramp_up_distance),
make_protocol_property("spin_up_current", &config_.spin_up_current),
make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration),
make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel)
make_protocol_object("lockin",
make_protocol_property("current", &config_.lockin.current),
make_protocol_property("ramp_time", &config_.lockin.ramp_time),
make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),
make_protocol_property("accel", &config_.lockin.accel),
make_protocol_property("vel", &config_.lockin.vel),
make_protocol_property("finish_distance", &config_.lockin.finish_distance),
make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
)
),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
Firmware/MotorControl/encoder.cpp
+47 -28
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
Firmware/MotorControl/encoder.hpp
+10 -5
View File
@@ -31,7 +31,6 @@ public:
// be determined by run_offset_calibration.
// In this case the encoder will enter ready
// state as soon as the index is found.
float idx_search_speed = 10.0f; // [rad/s electrical]
bool zero_count_on_find_idx = true;
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // Offset between encoder count and rotor electrical phase
@@ -39,7 +38,9 @@ public:
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 bandwidth = 1000.0f;
bool ignore_illegal_hall_state = false;
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
bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
};
Encoder(const EncoderHardwareConfig_t& hw_config,
@@ -50,13 +51,14 @@ public:
bool do_checks();
void enc_index_cb();
void set_idx_subscribe(bool override_enable = false);
void set_linear_count(int32_t count);
void set_circular_count(int32_t count, bool update_offset);
bool calib_enc_offset(float voltage_magnitude);
bool scan_for_enc_idx(float omega, float voltage_magnitude);
bool run_index_search();
bool run_direction_find();
bool run_offset_calibration();
void sample_now();
bool update();
@@ -104,9 +106,11 @@ public:
// make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
make_protocol_property("mode", &config_.mode),
make_protocol_property("use_index", &config_.use_index),
make_protocol_property("use_index", &config_.use_index,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("idx_search_speed", &config_.idx_search_speed),
make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx),
make_protocol_property("cpr", &config_.cpr),
make_protocol_property("offset", &config_.offset),
@@ -115,6 +119,7 @@ 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("idx_search_unidirectional", &config_.idx_search_unidirectional),
make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
)
);
Firmware/MotorControl/motor.hpp
+1 -1
View File
@@ -63,7 +63,7 @@ public:
float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
float phase_inductance = 0.0f; // to be set by measure_phase_inductance
float phase_resistance = 0.0f; // to be set by measure_phase_resistance
int32_t direction = 1; // 1 or -1
int32_t direction = 0; // 1 or -1 (0 = unspecified)
MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
// Read out max_allowed_current to see max supported value for current_lim.
// float current_lim = 70.0f; //[A]
tools/odrive/enums.py
+2
View File
@@ -10,6 +10,8 @@ AXIS_STATE_SENSORLESS_CONTROL = 5
AXIS_STATE_ENCODER_INDEX_SEARCH = 6
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
AXIS_STATE_CLOSED_LOOP_CONTROL = 8
AXIS_STATE_LOCKIN_SPIN = 9
AXIS_STATE_ENCODER_DIR_FIND = 10
class errors:
class axis: