diff --git a/CHANGELOG.md b/CHANGELOG.md index 68d2d159..03e03b93 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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. diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index f0fe1d3e..7746ae87 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -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" + ); } /** diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 8be43d15..0ba614c3 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -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; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index a6b6bdae..dd8e88ef 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -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, //(ctx)->decode_step_dir_pins(); }, this), make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin, [](void* ctx) { static_cast(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()), diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 294c3e0e..0cf13549 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -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 diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 2d2fc749..dff98f28 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -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(ctx)->set_idx_subscribe(); }, this), + make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only, + [](void* ctx) { static_cast(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(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) ) ); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 9742835e..279e6848 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -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] diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 817e39e1..fc5439bd 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -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: