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

Lockin based index search working, clean out old search

Browse Source
This commit is contained in:
Oskar Weigl
2018-10-10 14:23:10 -07:00
parent b28f453833
commit 8c286f0a6d
5 changed files with 32 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+23 -15
View File
@@ -291,29 +291,37 @@ void Axis::run_state_machine_loop() {
// 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:
case AXIS_STATE_ENCODER_INDEX_SEARCH: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
status = encoder_.run_index_search();
break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION:
encoder_.config_.use_index = true;
encoder_.index_found_ = false;
bool orig_setting = config_.lockin_finish_on_enc_idx;
config_.lockin_finish_on_enc_idx = true;
status = run_lockin_spin();
config_.lockin_finish_on_enc_idx = orig_setting;
// status = encoder_.run_index_search();
} 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_LOCKIN_SPIN:
case AXIS_STATE_LOCKIN_SPIN: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
status = run_lockin_spin();
break;
} break;
case AXIS_STATE_SENSORLESS_CONTROL:
case AXIS_STATE_SENSORLESS_CONTROL: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
status = run_lockin_spin(); // TODO: restart if desired
@@ -323,20 +331,20 @@ void Axis::run_state_machine_loop() {
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_)
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:
Firmware/MotorControl/axis.hpp
+5 -5
View File
@@ -50,13 +50,13 @@ public:
// Spinup settings
float lockin_current = 10.0f; // [A]
float lockin_ramp_time = 0.4f; // [s]
float lockin_ramp_distance = 4 * M_PI; // [rad]
float lockin_accel = 400.0f; // [rad/s^2]
float lockin_vel = 400.0f; // [rad/s]
bool lockin_finish_on_vel = true;
float lockin_ramp_distance = 1 * M_PI; // [rad]
float lockin_accel = 10.0f; // [rad/s^2]
float lockin_vel = 100.0f; // [rad/s]
float lockin_finish_distance = 1000.0f; // [rad]
bool lockin_finish_on_vel = false;
bool lockin_finish_on_distance = false;
bool lockin_finish_on_enc_idx = false;
float lockin_finish_distance = 1000.0f; // [rad]
};
enum thread_signals {
Firmware/MotorControl/encoder.cpp
-32
View File
@@ -91,38 +91,6 @@ 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;
index_found_ = false;
float phase = 0.0f;
axis_->run_control_loop([&](){
phase = wrap_pm_pi(phase + omega * current_meas_period);
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);
// continue until the index is found
return !index_found_;
});
return true;
}
// @brief Turns the motor in one direction for a bit and then in the other
// direction in order to find the offset between the electrical phase 0
// and the encoder state 0.
Firmware/MotorControl/encoder.hpp
-1
View File
@@ -51,7 +51,6 @@ public:
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_offset_calibration();
tools/odrive/enums.py
+4 -4
View File
@@ -7,10 +7,10 @@ AXIS_STATE_STARTUP_SEQUENCE = 2
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3
AXIS_STATE_MOTOR_CALIBRATION = 4
AXIS_STATE_SENSORLESS_CONTROL = 5
AXIS_STATE_LOCKIN_SPIN = 6
AXIS_STATE_ENCODER_INDEX_SEARCH = 7
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 8
AXIS_STATE_CLOSED_LOOP_CONTROL = 9
AXIS_STATE_ENCODER_INDEX_SEARCH = 6
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
AXIS_STATE_CLOSED_LOOP_CONTROL = 8
AXIS_STATE_LOCKIN_SPIN = 9
AXIS_ERROR_NONE = 0
AXIS_ERROR_INVALID_STATE = 1