From a65212e2b7b975370c8427bed62786526fcf9b6c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 2 Oct 2018 00:44:42 -0700 Subject: [PATCH 01/16] move check_for_errors to inline --- Firmware/MotorControl/axis.cpp | 13 ------------- Firmware/MotorControl/axis.hpp | 14 +++++++++++++- Firmware/MotorControl/motor.cpp | 2 +- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 4ff660fd..01892af1 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -96,19 +96,6 @@ void Axis::set_step_dir_enabled(bool enable) { } } -bool Axis::check_for_errors() { - // Maybe we should update this to only trigger on new errors? - // The danger with that is we could fail to bail on uncleared errors that still prevent - // correct opreation. - - // For now: we treat ERROR_INVALID_STATE in idle loop special, or we could never stay - // in idle after this kind of error. - if (current_state_ == AXIS_STATE_IDLE) - return (error_ & ~ERROR_INVALID_STATE) == ERROR_NONE; - else - return error_ == ERROR_NONE; -} - // @brief Do axis level checks and call subcomponent do_checks // Returns true if everything is ok. bool Axis::do_checks() { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 6063ee3a..8306d1f5 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -80,9 +80,21 @@ public: bool check_PSU_brownout(); bool do_checks(); bool do_updates(); - bool check_for_errors(); float get_temp(); + bool inline check_for_errors() { + // Maybe we should update this to only trigger on new errors? + // The danger with that is we could fail to bail on uncleared errors that still prevent + // correct opreation. + + // For now: we treat ERROR_INVALID_STATE in idle loop special, or we could never stay + // in idle after this kind of error. + if (current_state_ == AXIS_STATE_IDLE) + return (error_ & ~ERROR_INVALID_STATE) == ERROR_NONE; + else + return error_ == ERROR_NONE; + } + // @brief Runs the specified update handler at the frequency of the current measurements. // // The loop runs until one of the following conditions: diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 5f518adb..ee71c98a 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -343,7 +343,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { // Inverse park transform float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; + float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl.final_v_alpha = mod_to_V * mod_alpha; From ca83df61558aa063124475e0a701a7ba1c57b2bb Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 2 Oct 2018 01:35:32 -0700 Subject: [PATCH 02/16] can do lockin spin at constant speed --- Firmware/MotorControl/axis.cpp | 59 ++++++++++++++++++++++++---------- Firmware/MotorControl/axis.hpp | 33 +++++++++++-------- tools/odrive/enums.py | 7 ++-- 3 files changed, 65 insertions(+), 34 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 01892af1..43ee9ce2 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -132,13 +132,13 @@ float Axis::get_temp() { return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs); } -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 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; + float phase = wrap_pm_pi(config_.lockin_ramp_time * x); + float I_mag = config_.lockin_current * x; + x += current_meas_period / config_.lockin_ramp_time; if (!motor_.update(I_mag, phase)) return error_ |= ERROR_MOTOR_FAILED, false; return x < 1.0f; @@ -146,21 +146,38 @@ bool Axis::run_sensorless_spin_up() { 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); + // Accelerate + float distance = config_.lockin_ramp_time; + float phase = wrap_pm_pi(distance); + float vel = distance / config_.lockin_ramp_time; + bool vel_done = false; + bool dist_done = false; run_control_loop([&](){ - vel += config_.spin_up_acceleration * current_meas_period; + 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)) + if (!motor_.update(config_.lockin_current, phase)) return error_ |= ERROR_MOTOR_FAILED, false; - return vel < config_.spin_up_target_vel; + vel_done = vel >= config_.lockin_vel; + dist_done = fabsf(distance) >= fabsf(config_.lockin_finish_distance); + if (config_.lockin_finish_on_distance) + return !vel_done && !dist_done; + else + return !vel_done; }); - // 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; + // Constant speed + if (config_.lockin_finish_on_distance) { + vel = config_.lockin_vel; + run_control_loop([&](){ + distance += vel * current_meas_period; + phase = wrap_pm_pi(phase + vel * current_meas_period); + if (!motor_.update(config_.lockin_current, phase)) + return error_ |= ERROR_MOTOR_FAILED, false; + dist_done = fabsf(distance) >= fabsf(config_.lockin_finish_distance); + return !dist_done; + }); + } return check_for_errors(); } @@ -282,10 +299,18 @@ void Axis::run_state_machine_loop() { status = encoder_.run_offset_calibration(); break; + case AXIS_STATE_LOCKIN_SPIN: + status = run_lockin_spin(); + break; + case AXIS_STATE_SENSORLESS_CONTROL: - status = run_sensorless_spin_up(); // TODO: restart if desired - if (status) + 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; case AXIS_STATE_CLOSED_LOOP_CONTROL: diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 8306d1f5..b264d1a1 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -31,9 +31,10 @@ public: AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, // Date: Sun, 7 Oct 2018 23:30:24 -0700 Subject: [PATCH 03/16] hm something causes a hardfault --- Firmware/MotorControl/axis.cpp | 46 ++++++++++++++++++++-------------- Firmware/MotorControl/axis.hpp | 8 ++++-- 2 files changed, 33 insertions(+), 21 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 43ee9ce2..2136ebcb 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -135,47 +135,55 @@ float Axis::get_temp() { bool Axis::run_lockin_spin() { // Spiral up current for softer rotor lock-in float x = 0.0f; - run_control_loop([&](){ + run_control_loop([&]() { float phase = wrap_pm_pi(config_.lockin_ramp_time * x); float I_mag = config_.lockin_current * x; x += current_meas_period / config_.lockin_ramp_time; if (!motor_.update(I_mag, phase)) - return error_ |= ERROR_MOTOR_FAILED, false; + return false; return x < 1.0f; }); if (error_ != ERROR_NONE) return false; - // Accelerate + // Spin states float distance = config_.lockin_ramp_time; float phase = wrap_pm_pi(distance); float vel = distance / config_.lockin_ramp_time; - bool vel_done = false; - bool dist_done = false; - run_control_loop([&](){ + + // 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 || vel >= 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 + run_control_loop([&]() { vel += config_.lockin_accel * current_meas_period; distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); + if (!motor_.update(config_.lockin_current, phase)) - return error_ |= ERROR_MOTOR_FAILED, false; - vel_done = vel >= config_.lockin_vel; - dist_done = fabsf(distance) >= fabsf(config_.lockin_finish_distance); - if (config_.lockin_finish_on_distance) - return !vel_done && !dist_done; - else - return !vel_done; + return false; + return !spin_done(true); //vel_override to go to next phase }); // Constant speed - if (config_.lockin_finish_on_distance) { - vel = config_.lockin_vel; - run_control_loop([&](){ + if (!spin_done()) { + 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)) - return error_ |= ERROR_MOTOR_FAILED, false; - dist_done = fabsf(distance) >= fabsf(config_.lockin_finish_distance); - return !dist_done; + return false; + return !spin_done(); }); } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 7b4c7bf2..31dc1488 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -55,7 +55,9 @@ public: 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_distance = false; // false: finish on target vel + 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] }; @@ -196,8 +198,10 @@ public: make_protocol_property("lockin_ramp_distance", &config_.lockin_ramp_distance), make_protocol_property("lockin_accel", &config_.lockin_accel), make_protocol_property("lockin_vel", &config_.lockin_vel), + make_protocol_property("lockin_finish_distance", &config_.lockin_finish_distance), + make_protocol_property("lockin_finish_on_vel", &config_.lockin_finish_on_vel), make_protocol_property("lockin_finish_on_distance", &config_.lockin_finish_on_distance), - make_protocol_property("lockin_finish_distance", &config_.lockin_finish_distance) + make_protocol_property("lockin_finish_on_enc_idx", &config_.lockin_finish_on_enc_idx) ), make_protocol_function("get_temp", *this, &Axis::get_temp), make_protocol_object("motor", motor_.make_protocol_definitions()), From b967a5ba5d6c9a88f82146ac41ff1ad2470d971f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 8 Oct 2018 01:32:13 -0700 Subject: [PATCH 04/16] add hard fault debug --- Firmware/Board/v3/Src/stm32f4xx_it.c | 34 +++++++++++++++++++--------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 8acc96ee..7ee8eecb 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -88,22 +88,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" + ); } /** From ac8691ec600a48a6c328be3ecb074fc504885610 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 8 Oct 2018 01:38:57 -0700 Subject: [PATCH 05/16] increase comms task stack space --- Firmware/communication/communication.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 622bdefb..0198e6c8 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -84,7 +84,7 @@ void init_communication(void) { printf("hi!\r\n"); // Start command handling thread - osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5500 /* in 32-bit words */); // TODO: fix stack issues comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL); while (!endpoint_list_valid) From 6462651e00e036ffe08fdb6484ee2b96cccb2260 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 8 Oct 2018 01:46:24 -0700 Subject: [PATCH 06/16] generate disasembly in build --- Firmware/build.lua | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/build.lua b/Firmware/build.lua index ea792219..8962e403 100644 --- a/Firmware/build.lua +++ b/Firmware/build.lua @@ -99,6 +99,8 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags) } -- display the size tup.frule{inputs={output_name..'.elf'}, command=prefix..'size %f'} + -- generate disassembly + tup.frule{inputs={output_name..'.elf'}, command=prefix..'objdump %f -dSC > %o', outputs={output_name..'.asm'}} -- create *.hex and *.bin output formats tup.frule{inputs={output_name..'.elf'}, command=prefix..'objcopy -O ihex %f %o', outputs={output_name..'.hex'}} tup.frule{inputs={output_name..'.elf'}, command=prefix..'objcopy -O binary -S %f %o', outputs={output_name..'.bin'}} From 3a61b4a6accb64cfe712629b50b27cf07d6c45a1 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 8 Oct 2018 19:40:44 -0700 Subject: [PATCH 07/16] lockin with enc sense working --- Firmware/MotorControl/axis.cpp | 6 ++++-- Firmware/MotorControl/axis.hpp | 11 ++++++++++- Firmware/MotorControl/encoder.cpp | 2 ++ Firmware/MotorControl/encoder.hpp | 4 +++- 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 2136ebcb..276aa8ca 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -134,6 +134,7 @@ float Axis::get_temp() { 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_.lockin_ramp_time * x); @@ -143,8 +144,6 @@ bool Axis::run_lockin_spin() { return false; return x < 1.0f; }); - if (error_ != ERROR_NONE) - return false; // Spin states float distance = config_.lockin_ramp_time; @@ -164,6 +163,7 @@ bool Axis::run_lockin_spin() { }; // Accelerate + lockin_state_ = LOCKIN_STATE_ACCELERATE; run_control_loop([&]() { vel += config_.lockin_accel * current_meas_period; distance += vel * current_meas_period; @@ -176,6 +176,7 @@ bool Axis::run_lockin_spin() { // 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; @@ -187,6 +188,7 @@ bool Axis::run_lockin_spin() { }); } + lockin_state_ = LOCKIN_STATE_INACTIVE; return check_for_errors(); } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 31dc1488..36edaab7 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -55,7 +55,7 @@ public: 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 = false; + bool lockin_finish_on_vel = true; bool lockin_finish_on_distance = false; bool lockin_finish_on_enc_idx = false; float lockin_finish_distance = 1000.0f; // [rad] @@ -65,6 +65,13 @@ public: 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, @@ -176,6 +183,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() { @@ -185,6 +193,7 @@ public: make_protocol_ro_property("current_state", ¤t_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), diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index f8e1e530..45885c29 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -43,6 +43,8 @@ bool Encoder::do_checks(){ // TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { if (config_.use_index && !index_found_) { + if (config_.find_idx_on_lockin && axis_->lockin_state_ != Axis::LOCKIN_STATE_CONST_VEL) + return; set_circular_count(0, false); set_linear_count(0); // Avoid position control transient after search if (config_.pre_calibrated) { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 6eca0536..baa32ad0 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -36,6 +36,7 @@ public: float offset_float = 0.0f; // Sub-count phase alignment offset float calib_range = 0.02f; float bandwidth = 1000.0f; + bool find_idx_on_lockin = false; }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -104,7 +105,8 @@ public: make_protocol_property("offset_float", &config_.offset_float), 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("calib_range", &config_.calib_range), + make_protocol_property("find_idx_on_lockin", &config_.find_idx_on_lockin) ) ); } From b28f45383333ca8d74861798b50a7dd3342e78e8 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 8 Oct 2018 19:59:18 -0700 Subject: [PATCH 08/16] explicit state prerequisite checks --- Firmware/MotorControl/axis.cpp | 19 +++++++++++++------ Firmware/MotorControl/axis.hpp | 10 ++++------ 2 files changed, 17 insertions(+), 12 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 276aa8ca..05140c06 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -287,12 +287,6 @@ 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; @@ -302,18 +296,26 @@ void Axis::run_state_machine_loop() { break; 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: + if (!motor_.is_calibrated_) + goto invalid_state_label; status = encoder_.run_offset_calibration(); break; case AXIS_STATE_LOCKIN_SPIN: + if (!motor_.is_calibrated_) + goto invalid_state_label; status = run_lockin_spin(); break; case AXIS_STATE_SENSORLESS_CONTROL: + if (!motor_.is_calibrated_) + 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 @@ -324,6 +326,10 @@ void Axis::run_state_machine_loop() { break; 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; @@ -333,6 +339,7 @@ void Axis::run_state_machine_loop() { 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 36edaab7..2755ba43 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, // Date: Wed, 10 Oct 2018 14:23:10 -0700 Subject: [PATCH 09/16] Lockin based index search working, clean out old search --- Firmware/MotorControl/axis.cpp | 38 +++++++++++++++++++------------ Firmware/MotorControl/axis.hpp | 10 ++++---- Firmware/MotorControl/encoder.cpp | 32 -------------------------- Firmware/MotorControl/encoder.hpp | 1 - tools/odrive/enums.py | 8 +++---- 5 files changed, 32 insertions(+), 57 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 05140c06..31e6a354 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -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: diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 2755ba43..909b0e66 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -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 { diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 45885c29..781ea1d2 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -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. diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index baa32ad0..ff0d82b7 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -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(); diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index ca73b315..677bcbed 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -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 From d7644daacaf5e58114bd7832da54850fcaf69e68 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Wed, 10 Oct 2018 15:35:41 -0700 Subject: [PATCH 10/16] make encoder dir find and overspeed check --- Firmware/MotorControl/axis.cpp | 42 +++++++++++++++++++++++++------ Firmware/MotorControl/axis.hpp | 1 + Firmware/MotorControl/encoder.cpp | 9 +++++++ Firmware/MotorControl/encoder.hpp | 7 +++++- Firmware/MotorControl/motor.hpp | 2 +- tools/odrive/enums.py | 1 + 6 files changed, 53 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 31e6a354..457d0279 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -137,7 +137,7 @@ bool Axis::run_lockin_spin() { lockin_state_ = LOCKIN_STATE_RAMP; float x = 0.0f; run_control_loop([&]() { - float phase = wrap_pm_pi(config_.lockin_ramp_time * x); + 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)) @@ -146,7 +146,7 @@ bool Axis::run_lockin_spin() { }); // Spin states - float distance = config_.lockin_ramp_time; + float distance = config_.lockin_ramp_distance; float phase = wrap_pm_pi(distance); float vel = distance / config_.lockin_ramp_time; @@ -154,7 +154,7 @@ bool Axis::run_lockin_spin() { auto spin_done = [&](bool vel_override = false) -> bool { bool done = false; if (config_.lockin_finish_on_vel || vel_override) - done = done || vel >= config_.lockin_vel; + 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) @@ -298,7 +298,10 @@ 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) + goto invalid_state_label; + // TODO: move code body to function in Encoder encoder_.config_.use_index = true; encoder_.index_found_ = false; @@ -306,7 +309,32 @@ void Axis::run_state_machine_loop() { 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_DIR_FIND: { + if (!motor_.is_calibrated_) + goto invalid_state_label; + + // TODO: move code body to function in Encoder + int32_t init_enc_val = encoder_.shadow_count_; + bool orig_setting = config_.lockin_finish_on_distance; + config_.lockin_finish_on_distance = true; + motor_.config_.direction = 1; // Must test spin forwards for direction detect logic + status = run_lockin_spin(); + config_.lockin_finish_on_distance = orig_setting; + + if (status) { + // Check response and direction + if (encoder_.shadow_count_ > init_enc_val + 8) { + // motor same dir as encoder + motor_.config_.direction = 1; + } else if (encoder_.shadow_count_ < init_enc_val - 8) { + // motor opposite dir as encoder + motor_.config_.direction = -1; + } else { + motor_.config_.direction = 0; + } + } } break; case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: { @@ -316,13 +344,13 @@ void Axis::run_state_machine_loop() { } break; case AXIS_STATE_LOCKIN_SPIN: { - if (!motor_.is_calibrated_) + 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_) + if (!motor_.is_calibrated_ || motor_.config_.direction==0) goto invalid_state_label; status = run_lockin_spin(); // TODO: restart if desired if (status) { @@ -334,7 +362,7 @@ void Axis::run_state_machine_loop() { } break; case AXIS_STATE_CLOSED_LOOP_CONTROL: { - if (!motor_.is_calibrated_) + if (!motor_.is_calibrated_ || motor_.config_.direction==0) goto invalid_state_label; if (!encoder_.is_ready_) goto invalid_state_label; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 909b0e66..3d89d3a0 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -33,6 +33,7 @@ public: AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, // config_.overspeed_fault_ratio * axis_->controller_.config_.vel_limit) { + set_error(ERROR_OVERSPEED); + return false; + } + } + //// run encoder count interpolation int32_t corrected_enc = count_in_cpr_ - config_.offset; // if we are stopped, make sure we don't randomly drift diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index ff0d82b7..38889c12 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -15,6 +15,7 @@ public: ERROR_UNSUPPORTED_ENCODER_MODE = 0x08, ERROR_ILLEGAL_HALL_STATE = 0x10, ERROR_INDEX_NOT_FOUND_YET = 0x20, + ERROR_OVERSPEED = 0x40, }; enum Mode_t { @@ -36,7 +37,9 @@ public: float offset_float = 0.0f; // Sub-count phase alignment offset float calib_range = 0.02f; float bandwidth = 1000.0f; + float overspeed_fault_ratio = 1.2f; // ratio of vel_lim, 0.0f = disabled bool find_idx_on_lockin = false; + bool idx_search_unidirectional = false; }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -98,6 +101,7 @@ public: make_protocol_property("mode", &config_.mode), make_protocol_property("use_index", &config_.use_index), make_protocol_property("pre_calibrated", &config_.pre_calibrated), + make_protocol_property("overspeed_fault_ratio", &config_.overspeed_fault_ratio), make_protocol_property("idx_search_speed", &config_.idx_search_speed), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), @@ -105,7 +109,8 @@ 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("find_idx_on_lockin", &config_.find_idx_on_lockin) + make_protocol_property("find_idx_on_lockin", &config_.find_idx_on_lockin), + make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional) ) ); } diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index ebfd3a12..b67d4874 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -58,7 +58,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 677bcbed..ec374bdc 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -11,6 +11,7 @@ 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 AXIS_ERROR_NONE = 0 AXIS_ERROR_INVALID_STATE = 1 From 4b998a8b64965bbf0968a3e2b75f156cdca6bb43 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 10 Feb 2019 20:12:32 -0800 Subject: [PATCH 11/16] add vel argument to constant speed spoolup --- Firmware/MotorControl/axis.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 68edf0fb..baa6c2c6 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -191,7 +191,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(); }); From bade43dc9cd3aa7cbd8f50d5db91f88465e52abf Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 10 Feb 2019 22:05:33 -0800 Subject: [PATCH 12/16] remove old overspeed check from encoder --- Firmware/MotorControl/encoder.cpp | 9 --------- Firmware/MotorControl/encoder.hpp | 3 --- 2 files changed, 12 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 7d0b8714..1425bf7d 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -309,15 +309,6 @@ bool Encoder::update() { snap_to_zero_vel = true; } - // Check overspeed fault - if (config_.overspeed_fault_ratio != 0.0f) { // 0.0f = disabled - // TODO: Use separate encoder, motor, controller vel_lim: take min - if (fabsf(vel_estimate_) > config_.overspeed_fault_ratio * axis_->controller_.config_.vel_limit) { - set_error(ERROR_OVERSPEED); - return false; - } - } - //// run encoder count interpolation int32_t corrected_enc = count_in_cpr_ - config_.offset; // if we are stopped, make sure we don't randomly drift diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index d01d4ed3..e25e6003 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -15,7 +15,6 @@ public: ERROR_UNSUPPORTED_ENCODER_MODE = 0x08, ERROR_ILLEGAL_HALL_STATE = 0x10, ERROR_INDEX_NOT_FOUND_YET = 0x20, - ERROR_OVERSPEED = 0x40, }; enum Mode_t { @@ -39,7 +38,6 @@ public: float offset_float = 0.0f; // Sub-count phase alignment offset float calib_range = 0.02f; float bandwidth = 1000.0f; - float overspeed_fault_ratio = 1.2f; // ratio of vel_lim, 0.0f = disabled bool find_idx_on_lockin = false; bool idx_search_unidirectional = false; bool ignore_illegal_hall_state = false; @@ -108,7 +106,6 @@ public: make_protocol_property("mode", &config_.mode), make_protocol_property("use_index", &config_.use_index), make_protocol_property("pre_calibrated", &config_.pre_calibrated), - make_protocol_property("overspeed_fault_ratio", &config_.overspeed_fault_ratio), 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), From a735adb95ceb96b0cb3b89f539527ebd9f06eacd Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 11 Feb 2019 15:13:30 -0800 Subject: [PATCH 13/16] rename var and add comments --- Firmware/MotorControl/encoder.cpp | 2 +- Firmware/MotorControl/encoder.hpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 1425bf7d..40be59e4 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -43,7 +43,7 @@ bool Encoder::do_checks(){ // TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { if (config_.use_index && !index_found_) { - if (config_.find_idx_on_lockin && axis_->lockin_state_ != Axis::LOCKIN_STATE_CONST_VEL) + if (config_.find_idx_on_lockin_only && axis_->lockin_state_ != Axis::LOCKIN_STATE_CONST_VEL) return; set_circular_count(0, false); if (config_.zero_count_on_find_idx) diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index e25e6003..fa1ddeb3 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -36,11 +36,11 @@ public: int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, int32_t offset = 0; // Offset between encoder count and rotor electrical phase float offset_float = 0.0f; // Sub-count phase alignment offset - float calib_range = 0.02f; + float calib_range = 0.02f; // Accuracy required to pass encoder cpr check float bandwidth = 1000.0f; - bool find_idx_on_lockin = false; - bool idx_search_unidirectional = false; - 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, @@ -114,7 +114,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("find_idx_on_lockin", &config_.find_idx_on_lockin), + make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only), make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional), make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state) ) From f17c96360fd547743e6546396c0ac8940ac676fe Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 11 Feb 2019 17:32:48 -0800 Subject: [PATCH 14/16] push index and dir search into encoder object --- Firmware/MotorControl/axis.cpp | 60 +++++++++---------------------- Firmware/MotorControl/axis.hpp | 45 ++++++++++++----------- Firmware/MotorControl/encoder.cpp | 38 ++++++++++++++++++++ Firmware/MotorControl/encoder.hpp | 1 + 4 files changed, 81 insertions(+), 63 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index baa6c2c6..25034e22 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -146,27 +146,27 @@ bool Axis::run_lockin_spin() { lockin_state_ = LOCKIN_STATE_RAMP; float x = 0.0f; 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; + 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 false; return x < 1.0f; }); // Spin states - float distance = config_.lockin_ramp_distance; + float distance = config_.lockin.ramp_distance; float phase = wrap_pm_pi(distance); - float vel = distance / config_.lockin_ramp_time; + 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) + 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; }; @@ -174,11 +174,11 @@ bool Axis::run_lockin_spin() { // Accelerate lockin_state_ = LOCKIN_STATE_ACCELERATE; run_control_loop([&]() { - vel += config_.lockin_accel * current_meas_period; + vel += config_.lockin.accel * current_meas_period; distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!motor_.update(config_.lockin_current, phase, vel)) + if (!motor_.update(config_.lockin.current, phase, vel)) return false; return !spin_done(true); //vel_override to go to next phase }); @@ -186,12 +186,12 @@ bool Axis::run_lockin_spin() { // 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 + 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)) + if (!motor_.update(config_.lockin.current, phase, vel)) return false; return !spin_done(); }); @@ -311,40 +311,14 @@ void Axis::run_state_machine_loop() { if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0) goto invalid_state_label; - // TODO: move code body to function in Encoder - 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_DIR_FIND: { if (!motor_.is_calibrated_) goto invalid_state_label; - // TODO: move code body to function in Encoder - int32_t init_enc_val = encoder_.shadow_count_; - bool orig_setting = config_.lockin_finish_on_distance; - config_.lockin_finish_on_distance = true; - motor_.config_.direction = 1; // Must test spin forwards for direction detect logic - status = run_lockin_spin(); - config_.lockin_finish_on_distance = orig_setting; - - if (status) { - // Check response and direction - if (encoder_.shadow_count_ > init_enc_val + 8) { - // motor same dir as encoder - motor_.config_.direction = 1; - } else if (encoder_.shadow_count_ < init_enc_val - 8) { - // motor opposite dir as encoder - motor_.config_.direction = -1; - } else { - motor_.config_.direction = 0; - } - } + status = encoder_.run_direction_find(); } break; case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: { @@ -366,7 +340,7 @@ void Axis::run_state_machine_loop() { 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; + controller_.vel_setpoint_ = config_.lockin.vel; status = run_sensorless_control_loop(); } } break; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index ac1ffb3b..6904e79d 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -36,6 +36,18 @@ public: 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 = 10.0f; // [rad/s^2] + float vel = 100.0f; // [rad/s] + float finish_distance = 1000.0f; // [rad] + bool finish_on_vel = false; + bool finish_on_distance = false; + bool finish_on_enc_idx = false; + }; + struct Config_t { bool startup_motor_calibration = false; //(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) + [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), + 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 40be59e4..64ad4042 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -92,6 +92,44 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { cpu_exit_critical(prim); } +bool Encoder::run_index_search() { + config_.use_index = true; + index_found_ = false; + if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) { + axis_->motor_.config_.direction = 1; + } + + 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; +} + +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 // direction in order to find the offset between the electrical phase 0 // and the encoder state 0. diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index fa1ddeb3..10f6a38f 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -57,6 +57,7 @@ public: bool calib_enc_offset(float voltage_magnitude); bool run_index_search(); + bool run_direction_find(); bool run_offset_calibration(); void sample_now(); bool update(); From c0bc2b97d5eef165fa3c8a86c978e1fb6685f0d6 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 11 Feb 2019 17:35:52 -0800 Subject: [PATCH 15/16] update changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 46feb061..95501471 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. + # Releases ## [0.4.7] - 2018-11-28 ### Added From e3785a2aa24a9079da460858fd02498747671987 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 11 Feb 2019 18:42:22 -0800 Subject: [PATCH 16/16] make index only active when required --- Firmware/MotorControl/axis.cpp | 3 +++ Firmware/MotorControl/axis.hpp | 6 +++--- Firmware/MotorControl/encoder.cpp | 19 +++++++++++++++---- Firmware/MotorControl/encoder.hpp | 9 +++++---- 4 files changed, 26 insertions(+), 11 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 25034e22..0ba614c3 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -183,6 +183,9 @@ bool Axis::run_lockin_spin() { return !spin_done(true); //vel_override to go to next phase }); + if (!encoder_.index_found_) + encoder_.set_idx_subscribe(true); + // Constant speed if (!spin_done()) { lockin_state_ = LOCKIN_STATE_CONST_VEL; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 6904e79d..dd8e88ef 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -40,9 +40,9 @@ public: float current = 10.0f; // [A] float ramp_time = 0.4f; // [s] float ramp_distance = 1 * M_PI; // [rad] - float accel = 10.0f; // [rad/s^2] - float vel = 100.0f; // [rad/s] - float finish_distance = 1000.0f; // [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; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 64ad4042..54a16a41 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) { @@ -43,8 +42,6 @@ bool Encoder::do_checks(){ // TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { if (config_.use_index && !index_found_) { - if (config_.find_idx_on_lockin_only && axis_->lockin_state_ != Axis::LOCKIN_STATE_CONST_VEL) - return; set_circular_count(0, false); if (config_.zero_count_on_find_idx) set_linear_count(0); // Avoid position control transient after search @@ -58,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. diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 10f6a38f..a18b4fa4 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 @@ -51,6 +50,7 @@ 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); @@ -105,9 +105,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,7 +117,6 @@ 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("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only), make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional), make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state) )