From 36274b98e6832c6d008bca36f66e7634b60f396b Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 24 Sep 2018 21:17:13 -0400 Subject: [PATCH 01/26] Add move_incremental --- Firmware/MotorControl/controller.cpp | 10 ++++++++++ Firmware/MotorControl/controller.hpp | 4 ++++ 2 files changed, 14 insertions(+) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d7740a5e..7f9e5e51 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -48,6 +48,8 @@ void Controller::move_to_pos(float goal_point) { planned_move_end_time_ = axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_, axis_->trap_.config_.vel_limit, axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); + + goal_point_ = goal_point; config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); pos_setpoint_ = myTraj.Y; @@ -57,6 +59,14 @@ void Controller::move_to_pos(float goal_point) { planned_move_timer_ = axis_->loop_counter_ * current_meas_period; } +void Controller::move_incremental(float displacement, bool from_goal_point = true){ + if(from_goal_point){ + move_to_pos(goal_point_ + displacement); + } else{ + move_to_pos(axis_->encoder_.pos_estimate_ + displacement); + } +} + void Controller::start_anticogging_calibration() { // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) { diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index dd3f76a5..d7155871 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -35,6 +35,7 @@ public: // Trajectory-Planned control void move_to_pos(float goal_point); + void move_incremental(float displacement, bool from_goal_point); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); @@ -78,6 +79,8 @@ public: float planned_move_timer_ = 0.0f; float planned_move_end_time_ = 0.0f; + float goal_point_ = 0.0f; + // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( @@ -102,6 +105,7 @@ public: make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, "current_setpoint"), make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), + make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); } From 12651db3f2f23ee6be206019660192c9580a87ae Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 24 Sep 2018 21:21:49 -0400 Subject: [PATCH 02/26] Use pos_setpoint_ as "current position" in incremental moves --- Firmware/MotorControl/controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 7f9e5e51..a18938b8 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -63,7 +63,7 @@ void Controller::move_incremental(float displacement, bool from_goal_point = tru if(from_goal_point){ move_to_pos(goal_point_ + displacement); } else{ - move_to_pos(axis_->encoder_.pos_estimate_ + displacement); + move_to_pos(pos_setpoint_ + displacement); } } From a65212e2b7b975370c8427bed62786526fcf9b6c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 2 Oct 2018 00:44:42 -0700 Subject: [PATCH 03/26] 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 04/26] 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 05/26] 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 06/26] 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 07/26] 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 08/26] 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 09/26] 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 10/26] 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 11/26] 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 12/26] 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 2f36e68b79ed914b4316cb2427e17df4e38c44bc Mon Sep 17 00:00:00 2001 From: Alex Burka Date: Tue, 29 Jan 2019 22:25:51 -0500 Subject: [PATCH 13/26] double stack space for usb server thread --- Firmware/communication/interface_usb.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index cd44c907..036a8203 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -177,6 +177,6 @@ void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) { void start_usb_server() { // Start USB communication thread - osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512); + osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 1024); usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL); } From add0dd954cda6e93a7268df05a138f90df4b9156 Mon Sep 17 00:00:00 2001 From: Alex Burka Date: Tue, 29 Jan 2019 22:28:42 -0500 Subject: [PATCH 14/26] increase comms stack size again --- 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 625a3cf7..aaff052d 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, 6000 /* in 32-bit words */); // TODO: fix stack issues + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 8000 /* in 32-bit words */); // TODO: fix stack issues comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL); while (!endpoint_list_valid) From 301d0d807ee29e0886c21a958e8357dc55d3d373 Mon Sep 17 00:00:00 2001 From: Alex Burka Date: Tue, 29 Jan 2019 22:31:17 -0500 Subject: [PATCH 15/26] make stack overflows debuggable Now if you break on vApplicationStackOverflowHook in a debugger, you can see which thread overflowed. --- Firmware/MotorControl/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 4146850c..709381e4 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -97,7 +97,7 @@ void enter_dfu_mode() { extern "C" { int odrive_main(void); -void vApplicationStackOverflowHook(void) { +void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) { for (;;); // TODO: safe action } void vApplicationIdleHook(void) { From 1e4b71b67bb2ba8ba0bd10135dd2db08f2d07def Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 9 Feb 2019 21:48:30 -0800 Subject: [PATCH 16/26] add ascii command f for feedback --- Firmware/communication/ascii_protocol.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 380530d1..3ac68499 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -155,6 +155,19 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& axes[motor_number]->controller_.move_to_pos(goal_point); } + } else if (cmd[0] == 'f') { // feedback + unsigned motor_number; + int numscan = sscanf(cmd, "f %u", &motor_number); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + respond(response_channel, use_checksum, "%f %f", + (double)axes[motor_number]->encoder_.pos_estimate_, + (double)axes[motor_number]->encoder_.vel_estimate_); + } + } else if (cmd[0] == 'h') { // Help respond(response_channel, use_checksum, "Please see documentation for more details"); respond(response_channel, use_checksum, ""); From 4b998a8b64965bbf0968a3e2b75f156cdca6bb43 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 10 Feb 2019 20:12:32 -0800 Subject: [PATCH 17/26] 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 7bb8b44fbcb04f3433d81e4b1bedc0ee6b55664d Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 10 Feb 2019 20:16:00 -0800 Subject: [PATCH 18/26] update changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 46feb061..68d2d159 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. +### Fixed +* Added required 1.5 cycle phase shift between ADC and PWM, lack thereof caused unstable current controller at high eRPM. + # Releases ## [0.4.7] - 2018-11-28 ### Added From bade43dc9cd3aa7cbd8f50d5db91f88465e52abf Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 10 Feb 2019 22:05:33 -0800 Subject: [PATCH 19/26] 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 20/26] 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 21/26] 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 22/26] 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 81d9b338630b1699f2b9fc73cde4598e6f488fd3 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 11 Feb 2019 17:54:56 -0800 Subject: [PATCH 23/26] add option to disable phase interpolation --- Firmware/MotorControl/encoder.cpp | 2 +- Firmware/MotorControl/encoder.hpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 6a04dd4f..294c3e0e 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -342,7 +342,7 @@ bool Encoder::update() { //// run encoder count interpolation int32_t corrected_enc = count_in_cpr_ - config_.offset; // if we are stopped, make sure we don't randomly drift - if (snap_to_zero_vel) { + if (snap_to_zero_vel || !config_.enable_phase_interpolation) { interpolation_ = 0.5f; // reset interpolation if encoder edge comes } else if (delta_enc > 0) { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 1cda9800..2d2fc749 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -36,7 +36,8 @@ 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; + 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; }; @@ -110,6 +111,7 @@ public: make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), make_protocol_property("offset_float", &config_.offset_float), + make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation), make_protocol_property("bandwidth", &config_.bandwidth, [](void* ctx) { static_cast(ctx)->update_pll_gains(); }, this), make_protocol_property("calib_range", &config_.calib_range), From e3785a2aa24a9079da460858fd02498747671987 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 11 Feb 2019 18:42:22 -0800 Subject: [PATCH 24/26] 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) ) From a253da299f9be1dd0d796bafc7b8a4ff1f680fbb Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sun, 24 Feb 2019 22:42:03 +0100 Subject: [PATCH 25/26] Add TrapezoidalMove to Arduino library ("t" ASCII command) --- Arduino/ODriveArduino/ODriveArduino.cpp | 4 ++++ Arduino/ODriveArduino/ODriveArduino.h | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Arduino/ODriveArduino/ODriveArduino.cpp b/Arduino/ODriveArduino/ODriveArduino.cpp index 2b2d6feb..00fce19a 100644 --- a/Arduino/ODriveArduino/ODriveArduino.cpp +++ b/Arduino/ODriveArduino/ODriveArduino.cpp @@ -42,6 +42,10 @@ void ODriveArduino::SetCurrent(int motor_number, float current) { serial_ << "c " << motor_number << " " << current << "\n"; } +void ODriveArduino::TrapezoidalMove(int motor_number, float position){ + serial_ << "t " << motor_number << " " << position << "\n"; +} + float ODriveArduino::readFloat() { return readString().toFloat(); } diff --git a/Arduino/ODriveArduino/ODriveArduino.h b/Arduino/ODriveArduino/ODriveArduino.h index 1ebe1e33..86b3aaf1 100644 --- a/Arduino/ODriveArduino/ODriveArduino.h +++ b/Arduino/ODriveArduino/ODriveArduino.h @@ -27,7 +27,7 @@ public: void SetVelocity(int motor_number, float velocity); void SetVelocity(int motor_number, float velocity, float current_feedforward); void SetCurrent(int motor_number, float current); - + void TrapezoidalMove(int motor_number, float position); // General params float readFloat(); int32_t readInt(); From b03ec2e88cd8a29e2aee9cc73828596b9841805b Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 25 Feb 2019 20:00:14 -0800 Subject: [PATCH 26/26] update changelog, improve some ascii commands --- CHANGELOG.md | 8 ++++++- Firmware/communication/ascii_protocol.cpp | 27 +++++++++++++++-------- docs/ascii-protocol.md | 19 ++++++++++++++++ docs/getting-started.md | 13 +++++++++-- 4 files changed, 55 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 68d2d159..bab57274 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,7 +3,13 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added * `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors. +* `f` command to ascii protocol to get encoder position and velocity feedback. * `q` command to ascii protocol. It is like the old `p` command, but velocity and current mean limits, not feed-forward. +* `ss`, `se`, `sr` commands to ascii protocol, for save config, erase config and reboot. +* `move_incremental` function for relative trajectory moves. +* `encoder.config.ignore_illegal_hall_state` option. +* `encoder.config.enable_phase_interpolation` option. Setting to false may reduce jerky pulsations at low speed when using hall sensor feedback. +* Analog input. Used the same way as the PWM input mappings. * Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop. * Thermal current limit with linear derating. @@ -15,7 +21,7 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added * Overspeed fault * Current sense saturation fault. -* Supress startup transients by sampling encoder estimate into position setpoint when entering closed loop control. +* Suppress startup transients by sampling encoder estimate into position setpoint when entering closed loop control. * Make step dir gpio pins configurable. * Configuration variable `encoder.config.zero_count_on_find_idx`, true by default. Set to false to leave the initial encoder count to be where the axis was at boot. * Circular position setpoint mode: position setpoints wrapped [0, cpr). Useful for infinite incremental position control. diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 3ac68499..8a0d3287 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -106,15 +106,17 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& unsigned motor_number; float pos_setpoint, vel_limit, current_lim; int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, ¤t_lim); - if (numscan < 4) { + if (numscan < 2) { respond(response_channel, use_checksum, "invalid command format"); } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { Axis* axis = axes[motor_number]; axis->controller_.pos_setpoint_ = pos_setpoint; - axis->controller_.config_.vel_limit = vel_limit; - axis->motor_.config_.current_lim = current_lim; + if (numscan >= 3) + axis->controller_.config_.vel_limit = vel_limit; + if (numscan >= 4) + axis->motor_.config_.current_lim = current_lim; } } else if (cmd[0] == 'v') { // velocity control @@ -172,6 +174,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Please see documentation for more details"); respond(response_channel, use_checksum, ""); respond(response_channel, use_checksum, "Available commands syntax reference:"); + respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim"); respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff"); respond(response_channel, use_checksum, "Velocity: v axis vel I-ff"); respond(response_channel, use_checksum, "Current: c axis I"); @@ -179,6 +182,10 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state"); respond(response_channel, use_checksum, "Read: r property"); respond(response_channel, use_checksum, "Write: w property value"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Save config: ss"); + respond(response_channel, use_checksum, "Erase config: se"); + respond(response_channel, use_checksum, "Reboot: sr"); } else if (cmd[0] == 'i'){ // Dump device info // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature()); @@ -188,12 +195,14 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION); respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); - } else if (cmd[0] == 's'){ // Save config - save_configuration(); - } else if (cmd[0] == 'e'){ // Erase config - erase_configuration(); - } else if (cmd[0] == 'b'){ // Reboot - NVIC_SystemReset(); + } else if (cmd[0] == 's'){ // System + if(cmd[1] == 's') { // Save config + save_configuration(); + } else if (cmd[1] == 'e'){ // Erase config + erase_configuration(); + } else if (cmd[1] == 'b'){ // Reboot + NVIC_SystemReset(); + } } else if (cmd[0] == 'r') { // read property char name[MAX_LINE_LENGTH]; diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md index 51ff18ea..0c510fe6 100644 --- a/docs/ascii-protocol.md +++ b/docs/ascii-protocol.md @@ -37,6 +37,20 @@ Example: `t 0 -20000` For general moving around of the axis, this is the recommended command. #### Motor Position command +For basic use where you send one setpoint at at a time, use the `q` command. +If you have a realtime controller that is streaming setpoints and tracking a trajectory, use the `p` command. + +``` +q motor position velocity_lim current_lim +``` +* `q` for position +* `motor` is the motor number, `0` or `1`. +* `position` is the desired position, in encoder counts. +* `velocity_lim` is the velocity limit, in counts/s (optional). +* `current_lim` is the current limit, in A (optional). + +Example: `q 0 -20000 10000 10` + ``` p motor position velocity_ff current_ff ``` @@ -90,3 +104,8 @@ Not all parameters can be accessed via the ASCII protocol but at least all param * `property` name of the property, as seen in ODrive Tool * `value` text representation of the value to be written * Example: `w axis0.controller.pos_setpoint -123.456` + +#### System commands: +* `ss` - Save config +* `se` - Erase config +* `sr` - Reboot diff --git a/docs/getting-started.md b/docs/getting-started.md index 1a346bd8..982ec861 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -261,7 +261,7 @@ You can also directly control the current of the motor, which is proportional to ### Trajectory control -Set `axis.controller.config.control_mode = CTRL_MODE_TRAJECTORY_CONTROL`.
+While in position control mode, use the `move_to_pos` or `move_incremental` functions. See the **Usage** section for details
This mode lets you smoothly accelerate, coast, and decelerate the axis from one position to another. With raw position control, the controller simply tries to go to the setpoint as quickly as possible. Using a trajectory lets you tune the feedback gains more aggressively to reject disturbance, while keeping smooth motion. ![Taptraj](TrapTrajPosVel.PNG)
@@ -291,9 +291,18 @@ Keep in mind that you must still set your safety limits as before. I recommend #### Usage Use the `move_to_pos` function to move to an absolute position: ``` -..controller.move_to_pos() +..controller.move_to_pos(your_absolute_pos) ``` +Use the `move_incremental` function to move to a relative position. +To set the goal relative to the current actual position, use `from_goal_point = False` +To set the goal relative to the previous destination, use `from_goal_point = True` +``` +..controller.move_incremental(pos_increment, from_goal_point) +``` + +You can also execute a move with the [appropriate ascii command](ascii-protocol.md#motor-trajectory-command). + ### Circular position control To enable Circular position control, set `axis.controller.config.setpoints_in_cpr = True`