From 0600cb851c5539477937ca1db562dd43cca4c0a4 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 19 Aug 2018 20:51:41 -0400 Subject: [PATCH 01/29] Add selectable intterupt mode to gpio_subscribe --- Firmware/Board/v3/Inc/gpio.h | 4 ++-- Firmware/Board/v3/Src/gpio.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index 7c7a4e63..c7251132 100644 --- a/Firmware/Board/v3/Inc/gpio.h +++ b/Firmware/Board/v3/Inc/gpio.h @@ -72,8 +72,8 @@ void MX_GPIO_Init(void); void SetGPIO12toUART(); bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, - uint32_t pull_up_down, - void (*callback)(void*), void* ctx); + uint32_t pull_up_down, uint32_t interrupt_mode, + void (*callback)(void*), void* ctx); void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); void GPIO_set_to_analog(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 031158d5..dfec1cf2 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -205,8 +205,8 @@ size_t n_subscriptions = 0; // on a rising edge of the GPIO. // @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, - uint32_t pull_up_down, - void (*callback)(void*), void* ctx) { + uint32_t pull_up_down, uint32_t interrupt_mode, + void (*callback)(void*), void* ctx) { // Register handler (or reuse existing registration) // TODO: make thread safe @@ -232,7 +232,7 @@ bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, // Set up GPIO GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Pin = GPIO_pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Mode = interrupt_mode; GPIO_InitStruct.Pull = pull_up_down; HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct); From 92fe075b9eb76ee20d428b619878519261637359 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 19 Aug 2018 20:52:14 -0400 Subject: [PATCH 02/29] Fix existing calls to GPIO_Subscribe --- Firmware/MotorControl/axis.cpp | 4 ++-- Firmware/MotorControl/encoder.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7d79b3d6..d560ae9d 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -81,8 +81,8 @@ void Axis::set_step_dir_enabled(bool enable) { HAL_GPIO_Init(hw_config_.dir_port, &GPIO_InitStruct); // Subscribe to rising edges of the step GPIO - GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN, - step_cb_wrapper, this); + GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN, + GPIO_MODE_IT_FALLING, step_cb_wrapper, this); enable_step_dir_ = true; } else { diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 5b80ed8d..0f2f838a 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -19,7 +19,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); + GPIO_MODE_IT_RISING, enc_index_cb_wrapper, this); } void Encoder::set_error(Encoder::Error_t error) { From 6844cfb86239070f30bd92ed8fc76cbaa1a212e7 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 19 Aug 2018 20:52:55 -0400 Subject: [PATCH 03/29] Add configuration options for endstops --- Firmware/MotorControl/axis.hpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 111f9e26..11b3e269 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -19,6 +19,11 @@ enum AxisState_t { AXIS_STATE_CLOSED_LOOP_CONTROL = 8 // Date: Sun, 19 Aug 2018 20:53:26 -0400 Subject: [PATCH 04/29] Add setup and callback functions for endstops --- Firmware/MotorControl/axis.cpp | 69 ++++++++++++++++++++++++++++++++++ Firmware/MotorControl/axis.hpp | 8 ++++ 2 files changed, 77 insertions(+) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index d560ae9d..3bdb03ce 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -29,6 +29,14 @@ static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } +static void min_endstop_cb_wrapper(void* ctx){ + reinterpret_cast(ctx)->min_endstop_cb(); +} + +static void max_endstop_cb_wrapper(void* ctx){ + reinterpret_cast(ctx)->max_endstop_cb(); +} + // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. void Axis::setup() { @@ -93,6 +101,64 @@ void Axis::set_step_dir_enabled(bool enable) { } } +void Axis::min_endstop_cb(){ + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.min_endstop.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.min_endstop.gpio_num); + + if(config_.min_endstop.enabled){ + min_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + } else { + min_endstop_state_ = false; + } +} + +void Axis::set_min_endstop_enabled(bool enable){ + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.min_endstop.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.min_endstop.gpio_num); + if(enable){ + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = gpio_pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); + + GPIO_subscribe(gpio_port, gpio_pin, GPIO_PULLUP, GPIO_MODE_IT_RISING_FALLING, + min_endstop_cb_wrapper, this); + } + else { + GPIO_unsubscribe(gpio_port, gpio_pin); + } +} + +void Axis::max_endstop_cb(){ + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.max_endstop.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.max_endstop.gpio_num); + + if(config_.max_endstop.enabled){ + max_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + } else { + max_endstop_state_ = false; + } +} + +void Axis::set_max_endstop_enabled(bool enable){ + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.max_endstop.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.max_endstop.gpio_num); + if(enable){ + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = gpio_pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); + + GPIO_subscribe(gpio_port, gpio_pin, GPIO_PULLUP, GPIO_MODE_IT_RISING_FALLING, + max_endstop_cb_wrapper, this); + } + else { + GPIO_unsubscribe(gpio_port, gpio_pin); + } +} + 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 @@ -177,7 +243,10 @@ bool Axis::run_sensorless_spin_up() { // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. bool Axis::run_sensorless_control_loop() { + set_min_endstop_enabled(config_.min_endstop.enabled); + set_max_endstop_enabled(config_.max_endstop.enabled); set_step_dir_enabled(config_.enable_step_dir); + run_control_loop([this](){ if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 11b3e269..8b0095d1 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -81,6 +81,10 @@ public: void step_cb(); void set_step_dir_enabled(bool enable); + void min_endstop_cb(); + void set_min_endstop_enabled(bool enable); + void max_endstop_cb(); + void set_max_endstop_enabled(bool enable); bool check_DRV_fault(); bool check_PSU_brownout(); @@ -168,6 +172,8 @@ public: AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; AxisState_t& current_state_ = task_chain_[0]; uint32_t loop_counter_ = 0; + bool min_endstop_state_ = false; + bool max_endstop_state_ = false; // Communication protocol definitions auto make_protocol_definitions() { @@ -177,6 +183,8 @@ 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("min_endstop_state", &min_endstop_state_), + make_protocol_ro_property("max_endstop_state", &max_endstop_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), From 41a590773ff558db25bd93e513bce8f461c9765e Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 19 Aug 2018 22:34:18 -0400 Subject: [PATCH 05/29] Add axis homing function and AXIS state --- Firmware/MotorControl/axis.cpp | 8 ++++++++ Firmware/MotorControl/axis.hpp | 10 +++++++--- Firmware/MotorControl/controller.cpp | 19 +++++++++++++++++++ Firmware/MotorControl/controller.hpp | 9 +++++++-- 4 files changed, 41 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 3bdb03ce..dc57bdc4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -321,6 +321,10 @@ void Axis::run_state_machine_loop() { else if (config_.startup_sensorless_control) task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL; task_chain_[pos++] = AXIS_STATE_IDLE; + } else if (requested_state_ == AXIS_STATE_HOMING){ + task_chain_[pos++] = AXIS_STATE_HOMING; + task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; + task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) { task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; if (encoder_.config_.use_index) @@ -357,6 +361,10 @@ void Axis::run_state_machine_loop() { status = encoder_.run_index_search(); break; + case AXIS_STATE_HOMING: + status = controller_.home_axis(); + break; + case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: status = encoder_.run_offset_calibration(); break; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 8b0095d1..220a2874 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -16,12 +16,14 @@ enum AxisState_t { AXIS_STATE_SENSORLESS_CONTROL = 5, //config_.min_endstop.enabled) { + set_vel_setpoint(-config_.homing_speed, 0.0f); + } else { + return false; + } + + axis_->run_control_loop([&](){ + if(axis_->min_endstop_state_){ + axis_->encoder_.set_linear_count(axis_->config_.min_endstop.offset); + set_pos_setpoint(0.0f, 0.0f, 0.0f); + } + return !axis_->min_endstop_state_; + }); + return true; +} + /* * This anti-cogging implementation iterates through each encoder position, * waits for zero velocity & position error, diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index f10b6211..abc155ee 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -21,6 +21,7 @@ struct ControllerConfig_t { // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] + float homing_speed = 2000.0f; // [counts/s] }; class Controller { @@ -31,6 +32,8 @@ public: void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); void set_vel_setpoint(float vel_setpoint, float current_feed_forward); void set_current_setpoint(float current_setpoint); + + bool home_axis(); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); @@ -83,7 +86,8 @@ public: make_protocol_property("pos_gain", &config_.pos_gain), make_protocol_property("vel_gain", &config_.vel_gain), make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), - make_protocol_property("vel_limit", &config_.vel_limit) + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("homing_speed", &config_.homing_speed) ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, "pos_setpoint", @@ -94,7 +98,8 @@ public: "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, "current_setpoint"), - make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), + make_protocol_function("home_axis", *this, &Controller::home_axis) ); } }; From ef954aa84d1bd8b85b41480e763a28493c68aed9 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 19 Aug 2018 22:40:17 -0400 Subject: [PATCH 06/29] Add endstop pressed errors in closed_loop_control mode --- Firmware/MotorControl/axis.cpp | 8 ++++++++ Firmware/MotorControl/axis.hpp | 4 +++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index dc57bdc4..dae77bd4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -272,6 +272,14 @@ bool Axis::run_closed_loop_control_loop() { return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error if (!motor_.update(current_setpoint, encoder_.phase_)) return false; // set_error should update axis.error_ + + + // Check for endstop presses + if(config_.min_endstop.enabled && min_endstop_state_) { + return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; + } else if(config_.max_endstop.enabled && max_endstop_state_) { + return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false; + } return true; }); set_step_dir_enabled(false); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 220a2874..e78413e9 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -20,7 +20,7 @@ enum AxisState_t { AXIS_STATE_HOMING = 9 // Date: Sun, 19 Aug 2018 22:44:08 -0400 Subject: [PATCH 07/29] Add homing to python enums --- tools/odrive/enums.py | 1 + 1 file changed, 1 insertion(+) diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 19572c90..5d4d759e 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -10,6 +10,7 @@ AXIS_STATE_SENSORLESS_CONTROL = 5 AXIS_STATE_ENCODER_INDEX_SEARCH = 6 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 AXIS_STATE_CLOSED_LOOP_CONTROL = 8 +AXIS_STATE_HOMING = 9 AXIS_ERROR_NONE = 0 AXIS_ERROR_INVALID_STATE = 1 From ccfdddbc807903cd20cf3392d966f6278144f6fd Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 19 Aug 2018 23:00:19 -0400 Subject: [PATCH 08/29] Add is_active_high config var to endstops --- Firmware/MotorControl/axis.cpp | 5 +++++ Firmware/MotorControl/axis.hpp | 9 ++++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index dae77bd4..f4f2e116 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -110,6 +110,8 @@ void Axis::min_endstop_cb(){ } else { min_endstop_state_ = false; } + if(config_.min_endstop.is_active_high == false) + min_endstop_state_ = !min_endstop_state_; } void Axis::set_min_endstop_enabled(bool enable){ @@ -139,6 +141,9 @@ void Axis::max_endstop_cb(){ } else { max_endstop_state_ = false; } + + if(config_.max_endstop.is_active_high == false) + max_endstop_state_ = !max_endstop_state_; } void Axis::set_max_endstop_enabled(bool enable){ diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index e78413e9..0f4fd53f 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -22,8 +22,9 @@ enum AxisState_t { struct Endstop_t { uint16_t gpio_num; - bool enabled; + bool enabled = false; int32_t offset = 0; + bool is_active_high = false; }; struct AxisConfig_t { @@ -205,12 +206,14 @@ public: make_protocol_object("min_endstop", make_protocol_property("gpio_num", &config_.min_endstop.gpio_num), make_protocol_property("enabled", &config_.min_endstop.enabled), - make_protocol_property("offset", &config_.min_endstop.offset) + make_protocol_property("offset", &config_.min_endstop.offset), + make_protocol_property("is_active_high", &config_.min_endstop.is_active_high) ), make_protocol_object("max_endstop", make_protocol_property("gpio_num", &config_.max_endstop.gpio_num), make_protocol_property("enabled", &config_.max_endstop.enabled), - make_protocol_property("offset", &config_.max_endstop.offset) + make_protocol_property("offset", &config_.max_endstop.offset), + make_protocol_property("is_active_high", &config_.max_endstop.is_active_high) ) ), make_protocol_function("get_temp", *this, &Axis::get_temp), From 725f19dff0b8ecc3cbaf8390d862bf10373b5919 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 19 Aug 2018 23:01:55 -0400 Subject: [PATCH 09/29] Move endstop polarity check to avoid flipping a disabled endstop --- Firmware/MotorControl/axis.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f4f2e116..655747f9 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -107,11 +107,11 @@ void Axis::min_endstop_cb(){ if(config_.min_endstop.enabled){ min_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + if(config_.min_endstop.is_active_high == false) + min_endstop_state_ = !min_endstop_state_; } else { min_endstop_state_ = false; } - if(config_.min_endstop.is_active_high == false) - min_endstop_state_ = !min_endstop_state_; } void Axis::set_min_endstop_enabled(bool enable){ @@ -138,12 +138,11 @@ void Axis::max_endstop_cb(){ if(config_.max_endstop.enabled){ max_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + if(config_.max_endstop.is_active_high == false) + max_endstop_state_ = !max_endstop_state_; } else { max_endstop_state_ = false; } - - if(config_.max_endstop.is_active_high == false) - max_endstop_state_ = !max_endstop_state_; } void Axis::set_max_endstop_enabled(bool enable){ From 965a42a928970857126dd7b12cf507e4f1f3f547 Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 28 Aug 2018 23:33:11 -0400 Subject: [PATCH 10/29] Adjust pullup and interrupt mode based on endstop polarity. --- Firmware/MotorControl/axis.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 655747f9..6358137c 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -124,7 +124,9 @@ void Axis::set_min_endstop_enabled(bool enable){ GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); - GPIO_subscribe(gpio_port, gpio_pin, GPIO_PULLUP, GPIO_MODE_IT_RISING_FALLING, + uint32_t pull_up_down = config_.min_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; + uint32_t interrupt_mode = config_.min_endstop.is_active_high ? GPIO_MODE_IT_RISING : GPIO_MODE_IT_FALLING; + GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, min_endstop_cb_wrapper, this); } else { @@ -155,7 +157,9 @@ void Axis::set_max_endstop_enabled(bool enable){ GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); - GPIO_subscribe(gpio_port, gpio_pin, GPIO_PULLUP, GPIO_MODE_IT_RISING_FALLING, + uint32_t pull_up_down = config_.max_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; + uint32_t interrupt_mode = config_.max_endstop.is_active_high ? GPIO_MODE_IT_RISING : GPIO_MODE_IT_FALLING; + GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, max_endstop_cb_wrapper, this); } else { From 8df5e217b0d880d34ce51739dbb062ac62f3f7df Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 29 Aug 2018 22:39:32 -0400 Subject: [PATCH 11/29] Add missing pin #'s in get_gpio_by_x functions --- Firmware/Board/v3/Src/gpio.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index dfec1cf2..2a7133df 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -285,6 +285,15 @@ GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){ case 4: return GPIO_4_GPIO_Port; break; #ifdef GPIO_5_GPIO_Port case 5: return GPIO_5_GPIO_Port; break; +#endif +#ifdef GPIO_6_GPIO_Port + case 6: return GPIO_6_GPIO_Port; break; +#endif +#ifdef GPIO_7_GPIO_Port + case 7: return GPIO_7_GPIO_Port; break; +#endif +#ifdef GPIO_8_GPIO_Port + case 8: return GPIO_8_GPIO_Port; break; #endif default: return GPIO_1_GPIO_Port; } @@ -298,6 +307,15 @@ uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){ case 4: return GPIO_4_Pin; break; #ifdef GPIO_5_Pin case 5: return GPIO_5_Pin; break; +#endif +#ifdef GPIO_6_Pin + case 6: return GPIO_6_Pin; break; +#endif +#ifdef GPIO_7_Pin + case 7: return GPIO_7_Pin; break; +#endif +#ifdef GPIO_8_Pin + case 8: return GPIO_8_Pin; break; #endif default: return GPIO_1_Pin; } From dd74eed54b140b3f21904fc02348c0292a2dde3c Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 29 Aug 2018 22:49:11 -0400 Subject: [PATCH 12/29] Add .startup_homing configuration value --- Firmware/MotorControl/axis.cpp | 5 ++++- Firmware/MotorControl/axis.hpp | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 6358137c..2275aa97 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -332,8 +332,11 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; if (config_.startup_encoder_offset_calibration) task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - if (config_.startup_closed_loop_control) + if (config_.startup_closed_loop_control){ task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; + if(config_.startup_homing) + task_chain_[pos++] = AXIS_STATE_HOMING; + } else if (config_.startup_sensorless_control) task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL; task_chain_[pos++] = AXIS_STATE_IDLE; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 0f4fd53f..21c8ff16 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -34,6 +34,7 @@ struct AxisConfig_t { bool startup_encoder_offset_calibration = false; // Date: Sun, 2 Sep 2018 01:59:35 -0400 Subject: [PATCH 13/29] Fix bugs in homing by actually testing and using the debugger... --- Firmware/MotorControl/axis.cpp | 41 ++++++++++++++++++---------- Firmware/MotorControl/axis.hpp | 8 ++++++ Firmware/MotorControl/controller.cpp | 9 +----- 3 files changed, 36 insertions(+), 22 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 2275aa97..7896f5b8 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -118,6 +118,7 @@ void Axis::set_min_endstop_enabled(bool enable){ uint16_t gpio_pin = get_gpio_pin_by_pin(config_.min_endstop.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.min_endstop.gpio_num); if(enable){ + HAL_GPIO_DeInit(gpio_port, gpio_pin); GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Pin = gpio_pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; @@ -125,7 +126,7 @@ void Axis::set_min_endstop_enabled(bool enable){ HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); uint32_t pull_up_down = config_.min_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; - uint32_t interrupt_mode = config_.min_endstop.is_active_high ? GPIO_MODE_IT_RISING : GPIO_MODE_IT_FALLING; + uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING; GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, min_endstop_cb_wrapper, this); } @@ -158,7 +159,7 @@ void Axis::set_max_endstop_enabled(bool enable){ HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); uint32_t pull_up_down = config_.max_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; - uint32_t interrupt_mode = config_.max_endstop.is_active_high ? GPIO_MODE_IT_RISING : GPIO_MODE_IT_FALLING; + uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING; // Need to track pin state, not just homing edges GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, max_endstop_cb_wrapper, this); } @@ -251,8 +252,6 @@ bool Axis::run_sensorless_spin_up() { // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. bool Axis::run_sensorless_control_loop() { - set_min_endstop_enabled(config_.min_endstop.enabled); - set_max_endstop_enabled(config_.max_endstop.enabled); set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ @@ -273,20 +272,32 @@ bool Axis::run_sensorless_control_loop() { bool Axis::run_closed_loop_control_loop() { set_step_dir_enabled(config_.enable_step_dir); - run_control_loop([this](){ + run_control_loop([this]() { // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) - return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error + return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error if (!motor_.update(current_setpoint, encoder_.phase_)) - return false; // set_error should update axis.error_ - + return false; // set_error should update axis.error_ - // Check for endstop presses - if(config_.min_endstop.enabled && min_endstop_state_) { - return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; - } else if(config_.max_endstop.enabled && max_endstop_state_) { - return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false; + // Handle the homing case + if (homing_state_ == HOMING_STATE_HOMING) { + if (min_endstop_state_) { + encoder_.set_linear_count(config_.min_endstop.offset); + controller_.set_pos_setpoint(0.0f, 0.0f, 0.0f); + homing_state_ = HOMING_STATE_MOVE_TO_ZERO; + } + } else if (homing_state_ == HOMING_STATE_MOVE_TO_ZERO) { + if(!min_endstop_state_){ + homing_state_ = HOMING_STATE_IDLE; + } + } else { + // Check for endstop presses + if (config_.min_endstop.enabled && min_endstop_state_) { + return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; + } else if (config_.max_endstop.enabled && max_endstop_state_) { + return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false; + } } return true; }); @@ -306,6 +317,8 @@ bool Axis::run_idle_loop() { // Infinite loop that does calibration and enters main control loop as appropriate void Axis::run_state_machine_loop() { + set_min_endstop_enabled(config_.min_endstop.enabled); + set_max_endstop_enabled(config_.max_endstop.enabled); // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f // TODO: Move this somewhere else @@ -333,9 +346,9 @@ void Axis::run_state_machine_loop() { if (config_.startup_encoder_offset_calibration) task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; if (config_.startup_closed_loop_control){ - task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; if(config_.startup_homing) task_chain_[pos++] = AXIS_STATE_HOMING; + task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; } else if (config_.startup_sensorless_control) task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 21c8ff16..5cc2aab7 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -20,6 +20,12 @@ enum AxisState_t { AXIS_STATE_HOMING = 9 //config_.min_endstop.enabled) { set_vel_setpoint(-config_.homing_speed, 0.0f); + axis_->homing_state_ = HOMING_STATE_HOMING; } else { return false; } - - axis_->run_control_loop([&](){ - if(axis_->min_endstop_state_){ - axis_->encoder_.set_linear_count(axis_->config_.min_endstop.offset); - set_pos_setpoint(0.0f, 0.0f, 0.0f); - } - return !axis_->min_endstop_state_; - }); return true; } From 9b0bc6f8407bad998d0940483c834e3a1a335548 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 8 Sep 2018 18:25:51 -0400 Subject: [PATCH 14/29] Make Endstops objects instead of structs in Axis --- CHANGELOG.md | 2 + Firmware/MotorControl/axis.cpp | 97 ++++------------------------ Firmware/MotorControl/axis.hpp | 40 +++--------- Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/endstop.cpp | 43 ++++++++++++ Firmware/MotorControl/endstop.hpp | 37 +++++++++++ Firmware/MotorControl/main.cpp | 20 ++++-- Firmware/MotorControl/odrive_main.h | 1 + Firmware/Tupfile.lua | 1 + 9 files changed, 123 insertions(+), 120 deletions(-) create mode 100644 Firmware/MotorControl/endstop.cpp create mode 100644 Firmware/MotorControl/endstop.hpp diff --git a/CHANGELOG.md b/CHANGELOG.md index a7b7fc42..2f71fc29 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -3,6 +3,8 @@ Please add a note of your changes below this heading if you make a Pull Request. ## Unreleased ### Added +* `min_endstop` and `max_endstop` objects can be configured on GPIO +* Axes can be homed if `min_endstop` is enabled * Encoder position count "homed" to zero when index is found. ### Changed diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7896f5b8..984aeb9f 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -11,31 +11,30 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor) + Motor& motor, + Endstop& min_endstop, + Endstop& max_endstop) : hw_config_(hw_config), config_(config), encoder_(encoder), sensorless_estimator_(sensorless_estimator), controller_(controller), - motor_(motor) + motor_(motor), + min_endstop_(min_endstop), + max_endstop_(max_endstop) { encoder_.axis_ = this; sensorless_estimator_.axis_ = this; controller_.axis_ = this; motor_.axis_ = this; + min_endstop_.axis_ = this; + max_endstop_.axis_ = this; } static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } -static void min_endstop_cb_wrapper(void* ctx){ - reinterpret_cast(ctx)->min_endstop_cb(); -} - -static void max_endstop_cb_wrapper(void* ctx){ - reinterpret_cast(ctx)->max_endstop_cb(); -} // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. @@ -101,73 +100,6 @@ void Axis::set_step_dir_enabled(bool enable) { } } -void Axis::min_endstop_cb(){ - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.min_endstop.gpio_num); - GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.min_endstop.gpio_num); - - if(config_.min_endstop.enabled){ - min_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); - if(config_.min_endstop.is_active_high == false) - min_endstop_state_ = !min_endstop_state_; - } else { - min_endstop_state_ = false; - } -} - -void Axis::set_min_endstop_enabled(bool enable){ - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.min_endstop.gpio_num); - GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.min_endstop.gpio_num); - if(enable){ - HAL_GPIO_DeInit(gpio_port, gpio_pin); - GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = gpio_pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); - - uint32_t pull_up_down = config_.min_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; - uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING; - GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, - min_endstop_cb_wrapper, this); - } - else { - GPIO_unsubscribe(gpio_port, gpio_pin); - } -} - -void Axis::max_endstop_cb(){ - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.max_endstop.gpio_num); - GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.max_endstop.gpio_num); - - if(config_.max_endstop.enabled){ - max_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); - if(config_.max_endstop.is_active_high == false) - max_endstop_state_ = !max_endstop_state_; - } else { - max_endstop_state_ = false; - } -} - -void Axis::set_max_endstop_enabled(bool enable){ - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.max_endstop.gpio_num); - GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.max_endstop.gpio_num); - if(enable){ - GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = gpio_pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); - - uint32_t pull_up_down = config_.max_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; - uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING; // Need to track pin state, not just homing edges - GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, - max_endstop_cb_wrapper, this); - } - else { - GPIO_unsubscribe(gpio_port, gpio_pin); - } -} - 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 @@ -282,20 +214,20 @@ bool Axis::run_closed_loop_control_loop() { // Handle the homing case if (homing_state_ == HOMING_STATE_HOMING) { - if (min_endstop_state_) { - encoder_.set_linear_count(config_.min_endstop.offset); + if (min_endstop_.endstop_state_) { + encoder_.set_linear_count(min_endstop_.config_.offset); controller_.set_pos_setpoint(0.0f, 0.0f, 0.0f); homing_state_ = HOMING_STATE_MOVE_TO_ZERO; } } else if (homing_state_ == HOMING_STATE_MOVE_TO_ZERO) { - if(!min_endstop_state_){ + if(!min_endstop_.endstop_state_){ homing_state_ = HOMING_STATE_IDLE; } } else { // Check for endstop presses - if (config_.min_endstop.enabled && min_endstop_state_) { + if (min_endstop_.config_.enabled && min_endstop_.endstop_state_) { return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; - } else if (config_.max_endstop.enabled && max_endstop_state_) { + } else if (max_endstop_.config_.enabled && max_endstop_.endstop_state_) { return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false; } } @@ -317,9 +249,6 @@ bool Axis::run_idle_loop() { // Infinite loop that does calibration and enters main control loop as appropriate void Axis::run_state_machine_loop() { - set_min_endstop_enabled(config_.min_endstop.enabled); - set_max_endstop_enabled(config_.max_endstop.enabled); - // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f // TODO: Move this somewhere else // TODO: respect changes of CPR diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 5cc2aab7..bf8cd5bf 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -26,13 +26,6 @@ enum HomingState_t { HOMING_STATE_MOVE_TO_ZERO }; -struct Endstop_t { - uint16_t gpio_num; - bool enabled = false; - int32_t offset = 0; - bool is_active_high = false; -}; - struct AxisConfig_t { bool startup_motor_calibration = false; //config_.min_endstop.enabled) { + if (axis_->min_endstop_.config_.enabled) { set_vel_setpoint(-config_.homing_speed, 0.0f); axis_->homing_state_ = HOMING_STATE_HOMING; } else { diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp new file mode 100644 index 00000000..d46caf0b --- /dev/null +++ b/Firmware/MotorControl/endstop.cpp @@ -0,0 +1,43 @@ +#include + +Endstop::Endstop(EndstopConfig_t &config) + : config_(config) { +} + +static void endstop_cb_wrapper(void* ctx){ + reinterpret_cast(ctx)->endstop_cb(); +} + +void Endstop::endstop_cb(){ + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); + + if(config_.enabled){ + endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + if(config_.is_active_high == false) + endstop_state_ = !endstop_state_; + } else { + endstop_state_ = false; + } +} + +void Endstop::set_endstop_enabled(bool enable){ + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); + if(enable){ + HAL_GPIO_DeInit(gpio_port, gpio_pin); + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = gpio_pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); + + uint32_t pull_up_down = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; + uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, + endstop_cb_wrapper, this); + } + else { + GPIO_unsubscribe(gpio_port, gpio_pin); + } +} \ No newline at end of file diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp new file mode 100644 index 00000000..b0f02bab --- /dev/null +++ b/Firmware/MotorControl/endstop.hpp @@ -0,0 +1,37 @@ +#ifndef __ENDSTOP_HPP +#define __ENDSTOP_HPP + + +struct EndstopConfig_t { + uint16_t gpio_num; + bool enabled = false; + int32_t offset = 0; + bool is_active_high = false; +}; + +class Endstop { + public: + Endstop(EndstopConfig_t& config); + EndstopConfig_t config_; + Axis* axis_ = nullptr; + + bool endstop_state_ = false; + + void set_endstop_enabled(bool enable); + void endstop_cb(); + + auto make_protocol_definitions(){ + return make_protocol_member_list( + make_protocol_object("config", + make_protocol_property("gpio_num", &config_.gpio_num), + make_protocol_property("enabled", &config_.enabled), + make_protocol_property("offset", &config_.offset), + make_protocol_property("is_active_high", &config_.is_active_high) + ) + ); + } + + private: + uint16_t debounce_timer_ = 0; +}; +#endif \ No newline at end of file diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 18b88433..67ea382a 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -14,6 +14,8 @@ SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; +EndstopConfig_t min_endstop_configs[AXIS_COUNT]; +EndstopConfig_t max_endstop_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -26,7 +28,9 @@ typedef Config< SensorlessEstimator::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], - AxisConfig_t[AXIS_COUNT]> ConfigFormat; + AxisConfig_t[AXIS_COUNT], + EndstopConfig_t[AXIS_COUNT], + EndstopConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( @@ -35,7 +39,9 @@ void save_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, - &axis_configs)) { + &axis_configs, + &min_endstop_configs, + &max_endstop_configs)) { //printf("saving configuration failed\r\n"); osDelay(5); } else { user_config_loaded_ = true; @@ -51,7 +57,9 @@ void load_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, - &axis_configs)) { + &axis_configs, + &min_endstop_configs, + &max_endstop_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { @@ -60,6 +68,8 @@ void load_configuration(void) { controller_configs[i] = ControllerConfig_t(); motor_configs[i] = MotorConfig_t(); axis_configs[i] = AxisConfig_t(); + min_endstop_configs[i] = EndstopConfig_t(); + max_endstop_configs[i] = EndstopConfig_t(); } } else { user_config_loaded_ = true; @@ -162,8 +172,10 @@ int odrive_main(void) { Motor *motor = new Motor(hw_configs[i].motor_config, hw_configs[i].gate_driver_config, motor_configs[i]); + Endstop *min_endstop = new Endstop(min_endstop_configs[i]); + Endstop *max_endstop = new Endstop(max_endstop_configs[i]); axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], - *encoder, *sensorless_estimator, *controller, *motor); + *encoder, *sensorless_estimator, *controller, *motor, *min_endstop, *max_endstop); } // Start ADC for temperature measurements and user measurements diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4e4db160..65577760 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -109,6 +109,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include #include #include diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index f242b259..ce0a3158 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -153,6 +153,7 @@ build{ 'MotorControl/axis.cpp', 'MotorControl/motor.cpp', 'MotorControl/encoder.cpp', + 'MotorControl/endstop.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', 'MotorControl/main.cpp', From f7f2452973047fa546effb982c531d2102106234 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 8 Sep 2018 18:57:25 -0400 Subject: [PATCH 15/29] Add endstop debouncing --- Firmware/MotorControl/axis.cpp | 10 ++++++---- Firmware/MotorControl/endstop.cpp | 30 ++++++++++++++++++++++-------- Firmware/MotorControl/endstop.hpp | 13 +++++++++---- 3 files changed, 37 insertions(+), 16 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 984aeb9f..a0e22d21 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -140,6 +140,8 @@ bool Axis::do_updates() { // Sub-components should use set_error which will propegate to this error_ encoder_.update(); sensorless_estimator_.update(); + min_endstop_.update(); + max_endstop_.update(); return check_for_errors(); } @@ -214,20 +216,20 @@ bool Axis::run_closed_loop_control_loop() { // Handle the homing case if (homing_state_ == HOMING_STATE_HOMING) { - if (min_endstop_.endstop_state_) { + if (min_endstop_.getEndstopState()) { encoder_.set_linear_count(min_endstop_.config_.offset); controller_.set_pos_setpoint(0.0f, 0.0f, 0.0f); homing_state_ = HOMING_STATE_MOVE_TO_ZERO; } } else if (homing_state_ == HOMING_STATE_MOVE_TO_ZERO) { - if(!min_endstop_.endstop_state_){ + if(!min_endstop_.getEndstopState()){ homing_state_ = HOMING_STATE_IDLE; } } else { // Check for endstop presses - if (min_endstop_.config_.enabled && min_endstop_.endstop_state_) { + if (min_endstop_.config_.enabled && min_endstop_.getEndstopState()) { return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; - } else if (max_endstop_.config_.enabled && max_endstop_.endstop_state_) { + } else if (max_endstop_.config_.enabled && max_endstop_.getEndstopState()) { return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false; } } diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index d46caf0b..df4f65eb 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -1,4 +1,5 @@ #include +#include Endstop::Endstop(EndstopConfig_t &config) : config_(config) { @@ -8,19 +9,32 @@ static void endstop_cb_wrapper(void* ctx){ reinterpret_cast(ctx)->endstop_cb(); } -void Endstop::endstop_cb(){ - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); - GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); - - if(config_.enabled){ - endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); - if(config_.is_active_high == false) - endstop_state_ = !endstop_state_; +void Endstop::update() { + if (config_.enabled) { + float now = axis_->loop_counter_ * current_meas_period; + if((now - debounce_timer_) >= config_.debounce_ms) { // Debounce timer expired, take the new pin state + endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state + debounce_timer_ = now - config_.debounce_ms; // Ensure timer doesn't have overflow issues + } else { + endstop_state_ = endstop_state_; // Do nothing + } } else { endstop_state_ = false; } } +bool Endstop::getEndstopState() { + return endstop_state_; +} + +void Endstop::endstop_cb() { + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); + + debounce_timer_ = axis_->loop_counter_ * current_meas_period; + pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); +} + void Endstop::set_endstop_enabled(bool enable){ uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index b0f02bab..dcaf0e27 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -7,6 +7,7 @@ struct EndstopConfig_t { bool enabled = false; int32_t offset = 0; bool is_active_high = false; + float debounce_ms = 100; }; class Endstop { @@ -15,10 +16,11 @@ class Endstop { EndstopConfig_t config_; Axis* axis_ = nullptr; - bool endstop_state_ = false; - void set_endstop_enabled(bool enable); void endstop_cb(); + void update(); + + bool getEndstopState(); auto make_protocol_definitions(){ return make_protocol_member_list( @@ -26,12 +28,15 @@ class Endstop { make_protocol_property("gpio_num", &config_.gpio_num), make_protocol_property("enabled", &config_.enabled), make_protocol_property("offset", &config_.offset), - make_protocol_property("is_active_high", &config_.is_active_high) + make_protocol_property("is_active_high", &config_.is_active_high), + make_protocol_property("debounce_ms", &config_.debounce_ms) ) ); } private: - uint16_t debounce_timer_ = 0; + bool endstop_state_ = false; + bool pin_state_ = false; + float debounce_timer_ = 0; }; #endif \ No newline at end of file From 8b7ac1bab2ab5f2ce238bccf41260a17b1ada90b Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 8 Sep 2018 19:03:09 -0400 Subject: [PATCH 16/29] Fix seconds -> ms conversion error --- Firmware/MotorControl/endstop.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index df4f65eb..b9730984 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -12,11 +12,11 @@ static void endstop_cb_wrapper(void* ctx){ void Endstop::update() { if (config_.enabled) { float now = axis_->loop_counter_ * current_meas_period; - if((now - debounce_timer_) >= config_.debounce_ms) { // Debounce timer expired, take the new pin state - endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state - debounce_timer_ = now - config_.debounce_ms; // Ensure timer doesn't have overflow issues + if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001)) { // Debounce timer expired, take the new pin state + endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state + debounce_timer_ = now - (config_.debounce_ms * 0.001); // Ensure timer doesn't have overflow issues } else { - endstop_state_ = endstop_state_; // Do nothing + endstop_state_ = endstop_state_; // Do nothing } } else { endstop_state_ = false; From d169129434c89f68ea3b85554c4efe307682f4c1 Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 21 Sep 2018 18:55:47 -0400 Subject: [PATCH 17/29] Fix float -> double conversion --- Firmware/MotorControl/endstop.cpp | 7 +++---- Firmware/MotorControl/endstop.hpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index b9730984..c4e4a9fa 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -1,5 +1,4 @@ #include -#include Endstop::Endstop(EndstopConfig_t &config) : config_(config) { @@ -12,9 +11,9 @@ static void endstop_cb_wrapper(void* ctx){ void Endstop::update() { if (config_.enabled) { float now = axis_->loop_counter_ * current_meas_period; - if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001)) { // Debounce timer expired, take the new pin state - endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state - debounce_timer_ = now - (config_.debounce_ms * 0.001); // Ensure timer doesn't have overflow issues + if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state + endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state + debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues } else { endstop_state_ = endstop_state_; // Do nothing } diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index dcaf0e27..6a13eb68 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -7,7 +7,7 @@ struct EndstopConfig_t { bool enabled = false; int32_t offset = 0; bool is_active_high = false; - float debounce_ms = 100; + float debounce_ms = 100.0f; }; class Endstop { From 9f9ee3f1a34dff1bf64815d5b9301277c7f375ea Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 21 Sep 2018 18:56:38 -0400 Subject: [PATCH 18/29] Rearrange configs so they match trapTraj --- Firmware/MotorControl/main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 67ea382a..a816d920 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -28,9 +28,9 @@ typedef Config< SensorlessEstimator::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], - AxisConfig_t[AXIS_COUNT], EndstopConfig_t[AXIS_COUNT], - EndstopConfig_t[AXIS_COUNT]> ConfigFormat; + EndstopConfig_t[AXIS_COUNT], + AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( @@ -39,10 +39,10 @@ void save_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, - &axis_configs, &min_endstop_configs, - &max_endstop_configs)) { - //printf("saving configuration failed\r\n"); osDelay(5); + &max_endstop_configs, + &axis_configs)) { + printf("saving configuration failed\r\n"); osDelay(5); } else { user_config_loaded_ = true; } @@ -57,9 +57,9 @@ void load_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, - &axis_configs, &min_endstop_configs, - &max_endstop_configs)) { + &max_endstop_configs, + &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { @@ -67,9 +67,9 @@ void load_configuration(void) { sensorless_configs[i] = SensorlessEstimator::Config_t(); controller_configs[i] = ControllerConfig_t(); motor_configs[i] = MotorConfig_t(); - axis_configs[i] = AxisConfig_t(); min_endstop_configs[i] = EndstopConfig_t(); max_endstop_configs[i] = EndstopConfig_t(); + axis_configs[i] = AxisConfig_t(); } } else { user_config_loaded_ = true; From 01f98f749e957aea40a02d85e902f89d133314d6 Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 21 Sep 2018 19:17:11 -0400 Subject: [PATCH 19/29] Fix issue with saving endstop configs --- Firmware/MotorControl/endstop.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 6a13eb68..8a90d23f 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -13,7 +13,8 @@ struct EndstopConfig_t { class Endstop { public: Endstop(EndstopConfig_t& config); - EndstopConfig_t config_; + + EndstopConfig_t& config_; Axis* axis_ = nullptr; void set_endstop_enabled(bool enable); From 4e317d1fbf14f1b6e49ac24548e3ed9f8ded4729 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 23 Sep 2018 14:21:04 -0400 Subject: [PATCH 20/29] Set endstop state according to config_ --- Firmware/MotorControl/endstop.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index c4e4a9fa..d0c4f981 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -2,6 +2,7 @@ Endstop::Endstop(EndstopConfig_t &config) : config_(config) { + set_endstop_enabled(config_.enabled); } static void endstop_cb_wrapper(void* ctx){ From 197d10f50267e4d6b260fb40d03841f21cb293b6 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 23 Sep 2018 14:21:34 -0400 Subject: [PATCH 21/29] Add endstop_state_ as protocol ro property --- Firmware/MotorControl/endstop.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 8a90d23f..747422a8 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -23,8 +23,11 @@ class Endstop { bool getEndstopState(); + bool endstop_state_ = false; + auto make_protocol_definitions(){ return make_protocol_member_list( + make_protocol_ro_property("endstop_state_", &endstop_state_), make_protocol_object("config", make_protocol_property("gpio_num", &config_.gpio_num), make_protocol_property("enabled", &config_.enabled), @@ -36,7 +39,7 @@ class Endstop { } private: - bool endstop_state_ = false; + bool pin_state_ = false; float debounce_timer_ = 0; }; From d71364068131879c1d1f9f5a632e3c6d094e2155 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 23 Sep 2018 14:22:34 -0400 Subject: [PATCH 22/29] Change debounce logic so less is done in interrupts --- Firmware/MotorControl/endstop.cpp | 7 +++---- Firmware/MotorControl/endstop.hpp | 3 ++- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index d0c4f981..ac0ff9a1 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -10,6 +10,9 @@ static void endstop_cb_wrapper(void* ctx){ } void Endstop::update() { + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); + pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); if (config_.enabled) { float now = axis_->loop_counter_ * current_meas_period; if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state @@ -28,11 +31,7 @@ bool Endstop::getEndstopState() { } void Endstop::endstop_cb() { - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); - GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); - debounce_timer_ = axis_->loop_counter_ * current_meas_period; - pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); } void Endstop::set_endstop_enabled(bool enable){ diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 747422a8..6e9cb46f 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -23,6 +23,7 @@ class Endstop { bool getEndstopState(); + bool endstop_state_ = false; auto make_protocol_definitions(){ @@ -41,6 +42,6 @@ class Endstop { private: bool pin_state_ = false; - float debounce_timer_ = 0; + volatile float debounce_timer_ = 0; }; #endif \ No newline at end of file From 41224a4a4105efce91b97b2fbe553ff898eac947 Mon Sep 17 00:00:00 2001 From: Unknown Date: Thu, 27 Sep 2018 20:45:39 -0400 Subject: [PATCH 23/29] Move Endstop's config struct into the class to unify the style --- Firmware/MotorControl/endstop.cpp | 2 +- Firmware/MotorControl/endstop.hpp | 33 +++++++++++++------------------ Firmware/MotorControl/main.cpp | 12 +++++------ 3 files changed, 21 insertions(+), 26 deletions(-) diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index ac0ff9a1..4443fdd6 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -1,6 +1,6 @@ #include -Endstop::Endstop(EndstopConfig_t &config) +Endstop::Endstop(Endstop::Config_t &config) : config_(config) { set_endstop_enabled(config_.enabled); } diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 6e9cb46f..92ff10d7 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -1,20 +1,19 @@ #ifndef __ENDSTOP_HPP #define __ENDSTOP_HPP - -struct EndstopConfig_t { - uint16_t gpio_num; - bool enabled = false; - int32_t offset = 0; - bool is_active_high = false; - float debounce_ms = 100.0f; -}; - class Endstop { public: - Endstop(EndstopConfig_t& config); - - EndstopConfig_t& config_; + struct Config_t { + uint16_t gpio_num; + bool enabled = false; + int32_t offset = 0; + bool is_active_high = false; + float debounce_ms = 100.0f; + }; + + Endstop(Endstop::Config_t& config); + + Endstop::Config_t& config_; Axis* axis_ = nullptr; void set_endstop_enabled(bool enable); @@ -23,10 +22,9 @@ class Endstop { bool getEndstopState(); - bool endstop_state_ = false; - - auto make_protocol_definitions(){ + + auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_ro_property("endstop_state_", &endstop_state_), make_protocol_object("config", @@ -34,13 +32,10 @@ class Endstop { make_protocol_property("enabled", &config_.enabled), make_protocol_property("offset", &config_.offset), make_protocol_property("is_active_high", &config_.is_active_high), - make_protocol_property("debounce_ms", &config_.debounce_ms) - ) - ); + make_protocol_property("debounce_ms", &config_.debounce_ms))); } private: - bool pin_state_ = false; volatile float debounce_timer_ = 0; }; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index ebfe25a6..39921fea 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -15,8 +15,8 @@ Controller::Config_t controller_configs[AXIS_COUNT]; Motor::Config_t motor_configs[AXIS_COUNT]; Axis::Config_t axis_configs[AXIS_COUNT]; TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT]; -EndstopConfig_t min_endstop_configs[AXIS_COUNT]; -EndstopConfig_t max_endstop_configs[AXIS_COUNT]; +Endstop::Config_t min_endstop_configs[AXIS_COUNT]; +Endstop::Config_t max_endstop_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -30,8 +30,8 @@ typedef Config< Controller::Config_t[AXIS_COUNT], Motor::Config_t[AXIS_COUNT], TrapezoidalTrajectory::Config_t[AXIS_COUNT], - EndstopConfig_t[AXIS_COUNT], - EndstopConfig_t[AXIS_COUNT], + Endstop::Config_t[AXIS_COUNT], + Endstop::Config_t[AXIS_COUNT], Axis::Config_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { @@ -73,8 +73,8 @@ void load_configuration(void) { motor_configs[i] = Motor::Config_t(); trap_configs[i] = TrapezoidalTrajectory::Config_t(); axis_configs[i] = Axis::Config_t(); - min_endstop_configs[i] = EndstopConfig_t(); - max_endstop_configs[i] = EndstopConfig_t(); + min_endstop_configs[i] = Endstop::Config_t(); + max_endstop_configs[i] = Endstop::Config_t(); } } else { user_config_loaded_ = true; From 01b557122ed5b4f8ace81c7ba29a16b882727400 Mon Sep 17 00:00:00 2001 From: Unknown Date: Thu, 6 Dec 2018 23:37:36 -0500 Subject: [PATCH 24/29] Fix missing comma --- Firmware/MotorControl/controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index ca1012c3..d8e71b7c 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -110,7 +110,7 @@ public: make_protocol_property("vel_limit", &config_.vel_limit), make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance), make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate), - make_protocol_property("setpoints_in_cpr", &config_.setpoints_in_cpr) + make_protocol_property("setpoints_in_cpr", &config_.setpoints_in_cpr), make_protocol_property("homing_speed", &config_.homing_speed) ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, From d90793b788e707372d4cc4749739f201f229450c Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 8 Jan 2019 23:22:12 -0500 Subject: [PATCH 25/29] Remove interrupt-based Endstop switching --- Firmware/MotorControl/endstop.cpp | 22 +++++----------------- Firmware/MotorControl/endstop.hpp | 1 - 2 files changed, 5 insertions(+), 18 deletions(-) diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index 4443fdd6..d01c361b 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -5,14 +5,14 @@ Endstop::Endstop(Endstop::Config_t &config) set_endstop_enabled(config_.enabled); } -static void endstop_cb_wrapper(void* ctx){ - reinterpret_cast(ctx)->endstop_cb(); -} - void Endstop::update() { uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); + auto last_pin_state = pin_state_; pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + if(pin_state_ != last_pin_state){ + debounce_timer_ = axis_->loop_counter_ * current_meas_period; + } if (config_.enabled) { float now = axis_->loop_counter_ * current_meas_period; if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state @@ -30,10 +30,6 @@ bool Endstop::getEndstopState() { return endstop_state_; } -void Endstop::endstop_cb() { - debounce_timer_ = axis_->loop_counter_ * current_meas_period; -} - void Endstop::set_endstop_enabled(bool enable){ uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); @@ -42,15 +38,7 @@ void Endstop::set_endstop_enabled(bool enable){ GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Pin = gpio_pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Pull = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP;; HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); - - uint32_t pull_up_down = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; - uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING; - GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode, - endstop_cb_wrapper, this); - } - else { - GPIO_unsubscribe(gpio_port, gpio_pin); } } \ No newline at end of file diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 92ff10d7..9ff3f079 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -17,7 +17,6 @@ class Endstop { Axis* axis_ = nullptr; void set_endstop_enabled(bool enable); - void endstop_cb(); void update(); bool getEndstopState(); From 35d07595ac3dc079b846b9af468519144046307f Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 23 Apr 2019 21:52:11 +0200 Subject: [PATCH 26/29] Fix bad struct and enums --- Firmware/MotorControl/axis.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index a7f9e5b3..0be68574 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -28,8 +28,8 @@ public: ERROR_CONTROLLER_FAILED = 0x200, ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, - ERROR_MIN_ENDSTOP_PRESSED = 0x800, - ERROR_MAX_ENDSTOP_PRESSED = 0x1000 + ERROR_MIN_ENDSTOP_PRESSED = 0x1000, + ERROR_MAX_ENDSTOP_PRESSED = 0x2000 }; enum State_t { @@ -44,7 +44,8 @@ public: AXIS_STATE_CLOSED_LOOP_CONTROL = 8, // Date: Tue, 23 Apr 2019 22:03:25 +0200 Subject: [PATCH 27/29] Fix GPIO Subscribe issues --- Firmware/Board/v3/Inc/gpio.h | 3 +-- Firmware/Board/v3/Src/gpio.c | 5 ++--- Firmware/MotorControl/axis.cpp | 3 +-- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/Firmware/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index c7251132..403271b6 100644 --- a/Firmware/Board/v3/Inc/gpio.h +++ b/Firmware/Board/v3/Inc/gpio.h @@ -72,8 +72,7 @@ void MX_GPIO_Init(void); void SetGPIO12toUART(); bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, - uint32_t pull_up_down, uint32_t interrupt_mode, - void (*callback)(void*), void* ctx); + uint32_t pull_up_down, void (*callback)(void*), void* ctx); void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); void GPIO_set_to_analog(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 2a7133df..571dce7a 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -205,8 +205,7 @@ size_t n_subscriptions = 0; // on a rising edge of the GPIO. // @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, - uint32_t pull_up_down, uint32_t interrupt_mode, - void (*callback)(void*), void* ctx) { + uint32_t pull_up_down, void (*callback)(void*), void* ctx) { // Register handler (or reuse existing registration) // TODO: make thread safe @@ -232,7 +231,7 @@ bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, // Set up GPIO GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Pin = GPIO_pin; - GPIO_InitStruct.Mode = interrupt_mode; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; GPIO_InitStruct.Pull = pull_up_down; HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct); diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 737c0ee3..f668befa 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -121,8 +121,7 @@ void Axis::set_step_dir_active(bool active) { HAL_GPIO_Init(dir_port_, &GPIO_InitStruct); // Subscribe to rising edges of the step GPIO - GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN, - GPIO_MODE_IT_FALLING, step_cb_wrapper, this); + GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN, step_cb_wrapper, this); step_dir_active_ = true; } else { From f09b86b26227e860a7705f62cc0ef5c4cb532c33 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sun, 19 May 2019 22:49:18 +0200 Subject: [PATCH 28/29] Trigger an update to GPIO configuration on property write --- Firmware/MotorControl/endstop.cpp | 38 ++++++++++++++++++------------- Firmware/MotorControl/endstop.hpp | 11 +++++---- 2 files changed, 29 insertions(+), 20 deletions(-) diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index d01c361b..809e52f1 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -1,8 +1,8 @@ #include -Endstop::Endstop(Endstop::Config_t &config) +Endstop::Endstop(Endstop::Config_t& config) : config_(config) { - set_endstop_enabled(config_.enabled); + set_endstop_enabled(config_.enabled); } void Endstop::update() { @@ -10,14 +10,14 @@ void Endstop::update() { GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); auto last_pin_state = pin_state_; pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); - if(pin_state_ != last_pin_state){ + if (pin_state_ != last_pin_state) { debounce_timer_ = axis_->loop_counter_ * current_meas_period; } if (config_.enabled) { float now = axis_->loop_counter_ * current_meas_period; - if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state - endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state - debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues + if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state + endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state + debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues } else { endstop_state_ = endstop_state_; // Do nothing } @@ -30,15 +30,21 @@ bool Endstop::getEndstopState() { return endstop_state_; } -void Endstop::set_endstop_enabled(bool enable){ - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); - GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); - if(enable){ - HAL_GPIO_DeInit(gpio_port, gpio_pin); - GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = gpio_pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP;; - HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); +void Endstop::update_endstop_config(){ + set_endstop_enabled(config_.enabled); +} + +void Endstop::set_endstop_enabled(bool enable) { + if (config_.gpio_num != 0) { + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); + GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); + if (enable) { + HAL_GPIO_DeInit(gpio_port, gpio_pin); + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = gpio_pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; + HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); + } } } \ No newline at end of file diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 9ff3f079..cdca3fdc 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -16,19 +16,22 @@ class Endstop { Endstop::Config_t& config_; Axis* axis_ = nullptr; + void update_endstop_config(); void set_endstop_enabled(bool enable); - void update(); + void update(); bool getEndstopState(); bool endstop_state_ = false; auto make_protocol_definitions() { return make_protocol_member_list( - make_protocol_ro_property("endstop_state_", &endstop_state_), + make_protocol_ro_property("endstop_state", &endstop_state_), make_protocol_object("config", - make_protocol_property("gpio_num", &config_.gpio_num), - make_protocol_property("enabled", &config_.enabled), + make_protocol_property("gpio_num", &config_.gpio_num, + [](void* ctx) { static_cast(ctx)->update_endstop_config(); }, this), + make_protocol_property("enabled", &config_.enabled, + [](void* ctx) { static_cast(ctx)->update_endstop_config(); }, this), make_protocol_property("offset", &config_.offset), make_protocol_property("is_active_high", &config_.is_active_high), make_protocol_property("debounce_ms", &config_.debounce_ms))); From bc805a86c9334ddefd558ff8146a751b4989e6b4 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 25 May 2019 12:28:56 +0200 Subject: [PATCH 29/29] Write instructions for endstops branch --- Firmware/MotorControl/controller.hpp | 2 +- docs/Endstop_configuration.png | Bin 0 -> 20174 bytes docs/endstops.md | 73 +++++++++++++++++++++++++++ 3 files changed, 74 insertions(+), 1 deletion(-) create mode 100644 docs/Endstop_configuration.png create mode 100644 docs/endstops.md diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 6bfeaa81..64adca26 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -32,7 +32,7 @@ public: float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable float vel_ramp_rate = 10000.0f; // [(counts/s) / s] bool setpoints_in_cpr = false; - float homing_speed = 2000.0f; // [counts/s] + float homing_speed = 2000.0f; // [counts/s] }; explicit Controller(Config_t& config); diff --git a/docs/Endstop_configuration.png b/docs/Endstop_configuration.png new file mode 100644 index 0000000000000000000000000000000000000000..56a618fffbea308cc3f1ccd764fca5c686d3a964 GIT binary patch literal 20174 zcmZ^~c_5VE`!|k65=9Ln$zH~i?E5>6tSK?Zn3Ay@%h+X?3S$e|Ny%>PhC#L%TiLSj z8nTAQzJ%wV-k0}6`s zAPS0e1{Z(}6f^HV?^94bOwdxlZ|FU-o>5#`rCVjTpUK4W!qGrvzC_yygTV_3(D*{s zkJ%76L?7oTyF++OG(iFwj6+m=^)!>;rEhoLlCM8MfT@rgm(rKg567@3Wofxg{I9OK zMm(ZYz0iVrb>$_9j++|Hta<^;_v$UD$zyf(g|A`@gJRwzPnztQw0kN`_I4C%h~H`o zP=Ta0o8c41wO;L$9MB!9%p7C=rd*KRsQ^k!x*1X(L=R@hY6AVhVy|E|%Xqk(fWKEa zrry2_x}r+{>0k+Y6VT~XM#VZ#CHnUXSnP8Ma0`&*IRp@-1p`P@AqYs**}|YjcL5Nr zS2M#fWE1miBeUomJPjJCtZiS68_E3{nYXjI=WTLdtOM*r2 zd`Vvtey=qD6ViewR?3!#Z2mg%ug!m2ch6O<>+6wGHsO!fGsm_a_zfimsN%lD9ex<9N z$X|SWgASFV$zmuayL}Ny(=AyQ8W7kD4cB}LDkn}tBfm%%tgI)OM4OF)llVSwzZ;)J zQ-Gg+#aO4}DDAle3Lq-Bl8f1QU7~=;>!Fb}3ctAvDi0&FPd~qfk+h*VZeC!7Z8ICU zj?5F5s}hLxBjr5M^atAF%W7|+Re|@0r0(oRs3Hkq6Y+@b)aN}>hQ^KT@EZ(zNC*}m z2WqHaZ>G)eP0U~o1qGvS-XS=KT!uwh$x>hCJEdX69!P-S=W0Ln3-f`~WLq2bS}74Q z%~Eb>cD`6A>@*53tcAt1Aillug>*a8p1Pl%2(|0}u_pafTC=}Ay|}%jZ>rAoJ@dO$ zKtalZg%ZaaHS}{&Z*95f?7ezci_rLxG496DPa7^PW2r&K)jT}+k|B4P5qvz4U0$yT zgUmKmjyVsuPxd3SYr>_Eh>r$66;*1GWjrHrRNDbZzD6W2*3a$TKYK`sUOi_vs^p!o<&-2d`SDJx6$ms&3a(2JdstVmxFNY-}MalNffdK~Tqf_w?nsQ*3l+ zJfl45N!3X>XLI66AzX0w68%$Li1P3_OT&6|X8e)R>oBda3eZh`mKn*zAm~K1c!K zw4=h>Oq8f_hobK?wz^fYFovag{de;5s7oU6r!Xy&7mxcf6On%iDO~GGlTEh!Jt2ul zDWFexQhP6RK+~NGPPC^VK}N$i<(*}?ydfjLRzY zVU6i)8Wwo~LAAEz?_C8Sb(ybjx4r?*mLY9(M<<-~FF+)hwF|9bpPj@$mN zC!WXV_YlBlu=3ZAWvq@ix?`U2tnLOA3qYJu>ZwtvgV&slLByerDNTNENO{xZTz|M` z{|~MESZ@!owBUYYWmX}};+|Jei@8YDowXL~i&%UHNdHMrz?$hDmGl$|U7wKR>za{# z@d)SENEk^KtyG>RKx~KJxSa7$qBN1+y`|?Lq(DlWJezeJrgRS3NX&%pF#F3!__|lw z`Wz?36(q)uFt04XlZ1Y@^|?BX{1SvaUU^d~R>YouTROIo{VKAcJukbQxa-;U)#ULL z7p|wIJTLX*Q(FlGz3d_NKek6a(9dS#E^s@toIj2 zILhLSt%u<8^n5`=U1GkRxU9P{^s^6baf_zXbCK(Mj98m^nOi(bh+;$b$m?9bKr{K} z?F`4qMuIEe{N?(hG&tTzZZ3uPeCo{BF!2tIPjID3Av}z;EHS3qPu!O&cSl2Aq*|^c zPCJ&N%gVa<3BHVY2~5M=I+Nq$Hf&*JBtlze6eoF3CB;&WCX~QYA}sDtx(HSL1d?Lm z9n2rruy!@Rtgxw>x<&$*f8r*^g1-LPsBgmr9p`~?ZwHQrvdyBa?`@~1c1MbG-91Ss z{Ag+Ztu^%W*sBO-!+uY`40i%MwfcG+0 zNvy9oNvSd%zUTj93D;@KVAhiIcr-)1UZwip)XC*czT}odbMC~>qQa4aYcSFfbZ8;q z@)Y`!dg-WDn68)CG!Of_`qzzU6k++j$tQb_d-(FzxjPm>-}=SNKjRfL>stKvBU}_X zJt#E#Fve`L0i~RAr@@L)#TpRmh3nGbXv8pn(qDg*r7xBx=I%!GgV1cXF&h1qG9pim zf2z=HCiuqb+p=yfZiUOuT{U*Bz%+e!SjlTu= z_0>Yno!}7KR{{pP@nv@JX97u@rf9XX)zCae6^Cy`fmd(8t_RR@Te3piDqcO6{TOmn zNqQk}xcR(j#d*a4%VTZ1feoHju;bqaIZ(2GU{bwMZ>D--yc+{cETb26@Bh^C1_9jt ze|gBiy(yO?E7=a$#CEz^D|%S#y`f6ml?L|O{woHvnIQor-%NkfuqpmK`I=sV*LLnE zwv!~m{R(RVq1J)EPIx=~13ra$t5eN(MbJ_T|G&NI3oA%7KP;AlmF$t<=5g-C#pu20 zvY#`c0_O3}nD5nU@*K8P}iC%fCrn0SBsGE++J5l`Q#Zy;!_i%ZNtz<+CBn zo->Os>rr!9*p-OmdE=Qo<+_MZ*@cC*1^SoQxS1obWmY6gp4G&yJ}A>uJ-UwCx*;XM zynnHF)N36Qr$75{eJ1@dpYd7U1K4*=%o(vQlVj?WpL~Pnjugru&>%^huN`Cd1y>9@ zxg2Bl5;vTbZTmZt5#66XA_*L-3zt$>};`GBlepO0UZXsQ&nDTxA<#%kIfYYE&p6>83)5lf+1P1+n5Lnh5mAHAj?JVz zQoRlDIrooxn`Pk=RgJ!Ig^GwL)+Dyz$1nNBmfX^-OiV8FHzb^Hw#b-DCiOc)Q`iWx z+|=_+r=w>dPsS<2`|L&I*kiSE(mz`}x~}C_5(O&p;L59D4dhc6rT4`>d@iCZy?N@) z3Q5-$tRP*6QkvO?IL%dr&|`1vVR=?9Wj}i{WQNYIxcNv;tL1{C`tZ!D+gYHnt zIQ%#Db*4HG(c?eq%O%Yj_L)DGcLS7>;cdN;m{twQ*ZbQB#p=Daw^N@NJERtAs@9u4 z6^dD|#zuLzB*c!@?wEOCwN?#yhh?DG;(r#drqO{yV&cDkBXu~wjazxvq7jj3#6A3vxda^yca3*zk zxKB$j+W=?aar2_GZ*t7osr_ZE?WO}#XaG`Z{umnB0sThv9bLE@Va5_LOwG%qrDUtS zk1ms)nbTJ>HmMx$Uc%-WRG-;NTm3$q&vgi>S-Und8L$3?V!|1_ygPtdT0o7IBX34% zzVr7x9h+?r*z3Qa7tcHAF_C zfwB3qHg7T|(RMI}5ts=Fr9r`ZW3u>h|pf6uv-Wq%{I0-A~xc|8ZIq1vJ#fiB& zj!F2{gDGUcS#ou~BhUs`er=MGNqP2NJTy|KB5TkMPRjVh-`>@o%Og-U9gStX`AqZs z-Qlc+XlYLw+Zis$y>sCk^P;OYmSUp@M#>2~jcrHT7C@^S}xAc*5NZ@W=9_3{(G-s5|ErOyf0N2Af!i`jN7{(EH>g@lEfN$XrY&Lzt#KI{DWpTgih);tkwf)o34lXGJfL{-5A0(Yr#%gUmMm5XT9 zW*Sd?CzbVmGi5yWjWIqO_`S|Gr6F+Kr>moXI)oYiT^sRWWGAHE{0qkEpwlUd749Tg zB0Du*Tccv{yYhj43Bh#pfua3U`q#4@-;i3j61;zW%pUg(m0!ylZoMju{Z*rC3G*5H z8463t>!)`Ey{ddZ<1Q|xAFE8e_?k#66t|!*x5~IKRvAR@?=XQg+zzjyYTZQ7%*>Hj zU;B{(ON8(4A)Lf$l@!Z zw_yVkdE@98&%vq@mS)g&A7Q2{&5_`u_s_pRxw8cV;-DMj7=9>UQ%#mW7B>QynZ>yG z1D(J56t-;1*GOCSDr%MBi3 zW(ZJxVP-*&j&96i0%O1!7)oM^7<@s8cWN~GimS3(ZyD0cajJCf(ezYzgEbxZXcaPk zu7RsW)1V@flY8Mi)UQcF(9-1pJLU!a0(VI*)9W5X+{UH=+{%|SDP52dgnPS(&4I=K zavXh8Pv7mh7a-i^uji2Tq>{xjA^`VAT?nhW_s*wmwGKKhv+L{(I*$;Jm=jf-TNGFVL4IlQ}ydEcZBKV`90!WVyMoE1>_#C=7yMg(6Dr z?C1J>uX&myQ!vi}{abis^u@1>+I{w8(5n^gH}Q@iYn(n9<9NCiX$u}AnPT%BKzQ-O z!toffE$WhZxHvPDIAl~c)J(G9EECeIK zn1xcuB$=mC&a4_j{L}Z(F(3r3L>LO;gq~#&{w^{0Wigw@=LGt<>=Q;rjOVuw^(N;J z!E5PQJ6}UswePg%m&?8XSU_BxhTmL2)sbO-uep0H(ckQ0KT>yHKIPiUpBq?B&)~v` zCgpE`?b`q?hp-+{EZy&x8EaZ5HV)Twlkvi5*w6384&xc^^MxH~r$y!Zy^`qep$*3g@hUOmEZy zy0dwmVtnbSqS-@ZWc8#4R|V=KJ^8vQ1LI}YyK-21JavME+|%W$*cIvSg2acr7d2## zVJ__bCh>e1Ytf_CgX;40FlRJ|EaJC+hpV@3Yy|`~dsM@iwCXu>*J%NiuH7Xr%n<46 z{pCFVVWKKl^KS0Ubp((leHv;X;z?a6y`o2FfLk>AvkpZ!Zg%-Wwj{%4K?BeKHt0>z zy$8BJ^^6r9r_`=9JWEyHdR4R;SD0P5_+d5l zozBwMZu=kXH4&7r?2j+PPFs8r;~2yI-=n#U_*dF>brrrD6L@&6Ma-q9!@>$T7GBXq z`K}gl$aCDfSAwoJbda(Be%f>w+2ExSXS5f+-s#agq5Kfve?Pvf^?aVvm1Bh;RERZr zIj4jjmTkhh2(~5lzGQl(dh76nj@8;FzRUg7%(}+ZG#%?4d>r!_Z!+LjtlH4+f{^)V z&}ncs6K&2SlcGDcQC1&?uOZ7_0-B7!83lUUMAED3t^|E@Jlei03X9dv;hI{JM)K2u zpLJr^wPM;aCta*K@VkahwYtBL067>CV64?gEoVJ#_-772ikZGft>F2C9V+~3)hUfd z<^5I=sEb>M2VLwy@T^;S7a>}%FMmrNXXHl%bW(f;E~aTY0;F4sL%KZ zRw|!zMr~ti5Zehy6-Pmc6nI$dsD~)no_iV;m}{@HvvF~9#{#^FJc{7-Q+&aFWe$KD zy)D7^Q^kzj0^5ycgP|&UHN>gNB>D>jL+7W1A^=pAJHDzlv{7N|&O~_yI^6N70F;s4 z>MY25nF4_g16@w(=iEBjhniU&dKnmB0bs)%@zMxRj!nTbol#n?`{rS!^sPI;7GB4FtU3&o&qy$z>e z5FIo+8m2VoWKM9ens_R_o1jU?`zjfaM8teH8}|FGKew)bf$l)NXE3-nq1y`Wf3Df-?=(&8M!vmr8j+(ccz#x z(N$NYjEH&jXYI9Iimo9=-|C8|;*Vf^U4QBA6n&5fIL8%T9Wl{_^l$sKNo&5O9crK)W z#dN%g$IVI_dwek|hB<2Mmnb=e0;ib#0~Jqk{-PF-YAUp zbrdJ1G&z-<2=(U#-?YYCyMZxHd@6#RO_S+2ua?bln<%7$rak+h9OH|7Utum1U?+SM z7$RNE%?oYAaKImDgk3XUpaqK#kE3mPvH5E;;63P00j{`06{u=R7t?MQ$W7=J>*ao4 z1d1FJhzEg8h~o<%UnE{xFQof|nI%6i%$!y=YZwlMT&x=;QaEnq-2*pNG47 z=Q^(;gKI(nAQki5cF zjys{HAFsF+CM3uzD!(nX&!`H0*#I&v^@RG>(+M&2BDXiA#TrUyg&c7Q-|&^D;vg;X z?qlh(3GVns(aP@%J?{#wBwgaitv6TZJ;f$W%dK)Mk#~6pTsH%l@YWT3!n?$H@066v zcc75#QTsUPkUd}Rty~9^Xl`+Ss`LXY8L)=ACFE=*BWco9N@u@~%RE)Fq2QOLU5Eq^ z^83-d>=FyeEGJ8}LdfS>#4%q^TSB3I7}tJ8F@xIFuQ$$6T@7~;`MV9&h<%I>M4d-w zvQm_pN$w6Rv<4~egpvo_PyC+#{HEeKXVqoGyh(=3|BBz}-F>KE5nbUo$A|5CnXgs( zy3;cpj`OrlAxP$=RMOH7aAOrxh_`&lR3WY<-f~*(XUrgDeQaVy%TH^j z$MBQZiWPWZ~pIuTji%@sP}_!1sP#gBgOEDQ?DvPN>oLOEZ7glaMd034eMqGuq1S zZUy6V^vxcb`GUeJR!*Ze2twSaoNva6Zj6^gO)tht^2py}P^l6*UAJTniE4{Ey`?_= zd_IzOe?1}V(9yD4WuuvkOXAW|C9KdbzAhPbGHv(M6BTN?fKLOhOMs7ZE7{2YOe64- zi2Hc{(?9nXwiZWx5`Si;&1}B8RYp|C+HPXmOi;xu@CZ3*)d+lY-GdgW6~vr9-}{w* zz67L8W1S|e8rvaon}bbCMW%orJ^)xbr{`fFC7>wWNc&S(m&z>puJ*vP{$J@o#ne5{ zRq{(j+pGmoe?7lc#5sy_7S1T|!Xwoj%!_gB7s4;;xAm4IC7z0p`hp7+{Ql4mBpUZB zOCOd3udh|T=yUXT6u8>xD;8%Wqj^u3RiOj3?oKe+1i%@?Pb&FVHKe0I=k7^edf_@U z!)g2u5M{Lk?v7kisSj!N9e9>*FCuX1?71nnwF4?#PILh0PCri1XtoHT_1|ZG_}QM@ zbBC6|giY?PKw?07^cEc*a~lt!RT)aZbii-I+jsN>h5KBqj` zmw8y9h22`q+rA{hm(* z3%k_41sw}4C4i1C3LKEpL-b&KdE1L?IV@o?suJ3Pc1~0!pRq?D2k}YCTTgEwNWq8# z0k>o6bB2NW{4O1^ypvw43BLlE`=@ZT#|jo@EKhdDO_duQ>m; z=IT_o#54Hgr%fMc>LUX@@b9aB4j`P5msf;TZFqmag$@bXu|O{LY1J_$j^iRxGSqw?ynLjZIDp<8S-uTU`H~ly ze!Grj&%zOW%u-U&sMyE6->X&ko!=xNIKh3koS0rI!eR2*h_)utuP#Gq)8Biir?M%3 zv(Y;cmcO?}kP>*wQd6E~&xZZDdoA;HqsGgM(VRtDN38FV6Jf~-K5~&sId!n^O=?Uz zF%JJ6!|gg@y}YCYb_(kI69EZE*`OOd2xj+wX+(7CH9i3cj8yvF6Q;%sRuLbV{HRuG z#;Jkdz_L8qvML5q#u;50l3>w1r6|22FF80VAiawN%(y zsHT)m#itp$>R-MJM#VrKHhis%*EdURaG;tV+E&$|dK<7)nx)P8TVNAX-lWJh`x+Hm z-;!X9^0Qa^o=1<@fGX-1t6A6R5-Im}3%=`-wAPGNYAqpZ2B>C!a;7%NJjcUfLfV>S zKo^Ldm-Tim=b1Awz4#AshkSSj1cj?AHqz%r$){*;aH0kes*axr(c<}uVknUcF#%L^ zYiQP;`G1Z(T??8p2$F!iI2ETU2wReC4f2tZUj3kmI5e9) zWcqiE+kEP_@QeCiA2Q`>8Sp_mj(3_puxyccjD}@uxHH#rrO134;VP$f>kW#2E7v{E0}83y!hbGI|LvDj0q@AL8%eg3kcTb4?lTt zhon>xjdX+pPha^xwVIrryaW#jm?i!k2U1Co zaqj#5bEEtU%iYpUxKna@qB8D7T+p@0@1o?!Hv-$8~~N#j9i2E}MJLpZ30!pmEy z?QNK*8<)wde$d(ikV?JSidUtXgNJudX#z?7c?9j|hpz*#`$RLRUIr=b#&=(87U5?s z^x%`4YcwX$h+)pB1LGC303!t{T%LM+{ko)_<4@PFzcPF*PVTZLts5YlGiaTJa?vtq zy`f*kNzn)DMjfSDC;<a@iC?2iOl zwCf10eEoIumNLJj6;9UMk0CS&dD}T2Q}_N$Vdpk5Ml_;x6BMFA9ssCHCo2@2?gsi- zLEKDTJXM8wPF@E{{d*w-z|``8pB$M3Tkx+otuWRl-p&`8Ry;Jsgac!rpTZShLNw#f zBQ{~>10UimlK61cz;3mXT=4a}z07+2lkq&TYv{p+_S2b8Kb)Xyx?Ja;x&!#u9%CKu zgLo1kQlAOG68@Pp5) z{xOBU4rPf0B#HnnOvo-=Ely5d*PzS?%##(Z(*9+z!j18;?t+e7JViC!F_`{BbmH=1)oXi9WE`W<90{sWkaE>r}NDoq$ry!`jD)s6Z0 z4h4Rbc7Md1_W^$QF~7 zws8a9RRNIAu8?8jzov&8FnmyIQD9R4nhN06{Jj877O+;VDHagxF~I(ak|+OPCQsOb zFc(^2=Cu^0llU5h4|-0IpQ%KOH)a@7;{l8#c(?|zW3BY!j#!T~7x zIk=1Gv4ie;{Qgw;+QRQQz>}Y$mc!%WLmH93iLmTfGQcih!tns~i>XWwt;|19xDA5- z)3D3;&(pj5U9U~45TTNMP(%a*57=~^`K3jaWb+Zu;6%u*5frtl0&GJNR(8n=YTWQ=E*gO+TSHrL@0kKtHs;?m1~Aoz?%yYY)tREK zMUyA947KzpK>m*0Aq|Y2Odk0)s4ECTHXdcf%8fs%3t!gB96q!ou&jvWsaL}pWjtu} zzgz<#t=?=Ctk$B$14KS7Tim3Fh|784cO0;n+L&=({RLRvnYQz2Iw)U94bfrj*KeL( z)vSL+=(zJF7C4|g^Bn)=O=Toradia4uTKX_v#Fh2;2y0eu5B7!KjKDxxrnf&|34TG zgj%f*WAl;uc(EXzGM?I0(Au$E|9LV_0PGLFO~BKY6taZlwA?=Zdh6~s75|}-Jr%{9 zG7=8V)fSr0U>e5nI^(NP$TO0)GkYF&s{yb1hZq0sUOo^$e8CuNDhAX%L);

^ry8tE+sXl76#RyC|hi4ZbV8jC|`|MNP9_18V*PCLLN)+&E(9c)!5ShGh z5l?#f&XGMVZ4EJN9+OmPX=!K#Mn&y>py36C`u3jU@7UYeWW7tv6Oj>7%mr_b*Jy@R ze{tf6e`{;02BFU0{29c&*2UIwx^3ueyRZ(vWYg#srYx6&Mijd~r-1qdHnNz7!}*{Qf8?sdEWttnIa z3wnq8+1|9Z0~BvfARl10qb&$cTMnw%_8qxlv4}y;4KnRibmYF6T#4p?^IOc)LwaFj zQCD0as-2D}XFqX6tMci-<9P)i(3l>lQo8z<% zaE8^%69M8uYMtc|beucO<=jbF8c}qt2%yQp=HIe~Fr}QT&Yr0@o)NYCvh7#T<7(B7pZitcT5q46g)$9Dq=+o7a``JWyKLmIAmD%!EgO}>|r3! zij9XBLBvQd1^eXRvL*RZvcpDM&90Mw#QE znGA%hkT$%|foTGhly3OZqPoRedUE%=-8`34OweAvXw!MB@qw1Fc5^#a+=SotWbSRZU2nVzk3( zc$ntY&0&Db6xQW|*Ra~YFyn1Btaz~yZo{yr$|Rl-B+HP)pAQgmgK!rwhMg-V=;wG1 zmCLw78gVtSG+N48ob5@GeM3aZ`)O0s#}v^$>W!~zk!O>)mb25@j#e&T&>l9#%xHKV zc0#u6P3l7&H(8kq3JVij`o2&bIKu|kK7rDlU#bXT-R35qp7UqN3j~~IS^*T{WatXN z!d+XHO60)M*m}Xma@kEm^uH9FUz;<)&MpJns6Pg-OLyN5qk((S&jaSOO@sAwQl^bM&80>wCv0bL=CPhX<*FK)JUmV+DAo-TIJ3TUJwv zwDNSzwZgZkZQR@b$7%f1z@gk+u7QBR$`3Q8Nt?OGf&2QV;!32UipKK=wEGp*C9mh$ zPj5TDxxYS!fiHq=CkZ0QvMwqKHa~^Y^y07WPd1atWs#w|35l5Y zcY|W9H=w^bmQPGbztIducKk)oG0XfVhEjM%+s@S3KA7w9TN_GnZ*^AfQ(DfwC$FnZ z1OE|;|55vK@7IeVX(^su_RpggDwGosJHOckPo^t?OlB}AMO*>#?}sqq?s-aHHiXtz zDR#QMpOh6?3XIf_)l_7Ei|3UQkLILl@222L0gH?7tX4lqCa?iFo|6i2(#MDEqO5K~NcR=h4RgRnr0vwg1#ImfTp^4AOGk|_5rVnC%j0F7Z z#e{~U413<1qTX&>RY`UVzgPWx$)fygXn#k7qgSao^cRgia~_-xL(==w zhS9ZU!Z%7REJCYb<;VY|outRA{2u7HxM4*piuE2kd13GPCAg<1<1!G~`kRtAJHB8n zwea6*@31=w>omTk2gg~MK1Pk^Wc0D-UmFKNuubO&TT)9Oy0q&NPLw(yL2&&i%^4itwvTYO%-B{%<584L${de{}WzJ<$WAYPo+a zQ{?}>R)nVKz8!$qaz(54R%B&f^6KU$haKUH^t-&T5CfP5-?%LguZNxF++>`pWPOw< zK#d33Hq`jeSRnlaoaetT6mV@&K1(3ijr#ZV8Q@cy3*!?QwnQu~nCxd$EeOQ%^MT^z zQA=zI-4FRv8%?;#XVK1t%gpJq`G4Ib)X-@79`if}K&g2Nnv90?G5jZVnGF%Kgvr3<-pfUnbgkY@a!6H#!l?fb#n|C~{R8^t+j$pMAn@`tk^kwBZC zbTCVm(`HOYQQ=GpqYZrNI}m{YDaB|9Y7vGKG0hEM_q>{5&u`Ks@#EqxDg><*UsP98 zIKg+bp%kF34xLryN|F;2{_|kZ-+JG-;Q9=AuKN*$QMzhCpsq zGgsY3^ORAE5y%*^-sWiD(!!kBt&yMo^Uoo3@~!}PVR=*e!;9tAjOwp+JF-RBIRD~F zGc{Q&r4F@=r}lm?`VKNvgIxsAD=|`*sBvuRgMAdPd`RAS(|>2-;Y(93)fly)wJB(1 zw$GS6dse~9ZjyuRhR)USybb`hD3S2$C)joyp6SVa+Sz3AkwQy^FS}F=$)jU)B9uEY zUCY`#GnjZ|&Zn+fjtVKlo9)+xn0#&k5wuNp7Uzj?`mPz>oOF8XH{Z;Z08Q?!j+Va} z;AuiRs5L&*4BzF_Gk4%<_3R-t5|~asD9>1=ZmXP#M}I{3f4Esm^olkg<3if;w{+zXL!{#1%<bkUwWx?F~Spnjg(}w@%cGUnP8sgPkn?*&w{ZnCi{D08dbxeV{HriQ}%%pMer{^yl zdSSK$0?Tp|@5F_#%fCj?EU8oRC z_xPZH?#6l>#nL=iC8uc(U3RG)vAN@bK+OwM z8$n=*J>uW!k+fSHv50&o?rQ2XuZ>sLF-(uy%U&5l%UCX2g}-|^~)B)?w~ zL%*e|XQ1)oB!+*#PWWd0N4Rm;*K64A(&?2s`6%vkqTum%D6jM0B6+jCPD^i5u+CHk z(lz&f2aV>Oq0jS!iR|F5=sQxI$zJR&a+=LxUdNi=ljfZ> zu%96jikBhZJu&~Ftz8>|BuBePLAeoRlcXF++|#pqrDaIQm;9^(Kkd98`%&}IV3iiY zg@8<4q4C#kHtFmC0^4SRfjvDJ@b6N+{7MWg{vDKzxEO3lcY(pZdh+ihBx+Uf{_VXn z8icYaob=*OjQdX$D{$e}!OYhs^CU&iW`1%f?BHH8f1x-s$D&vB#%+EwE@U=73-)L7 zFuuL3!DZt9a#E>F#6q#Cp~XLa@k!<2Rj zO-@BjFzi;XW@0=6s*Ow)ZgR404*@c7Kx4@IP6oLEARhpFBEFULfM@}nj0}nW7vKdrmrEya zK@p4y%X0wA0uX%Wcwi*0!XGj%#{eCO314CdX?7&?HTgf$;1LKsDxn~E2vtUD$JnZcfVdWe~3y=-nZB;eC1lMQE#*fcz$oA2 zmo6=3c~VgL#@$;I%>Zl&9rV-7PTU6l_bmw9U#tkpM|(#{i7M`;z`QotrWBlj4=xos z5Kx5MY2FL}66dJ8=rcIL^f_9Qv7z0MHU&6LeKbj+sJ8vmK5hSFTC6PTxX<+&`V(VV zU2(I@5Wwete_a#|*H$dcLK0%Cyy57{!4P}hBAEssts zZwg-zN|Oo^@`xx3YoC{JfyJg@5(Vna$>_)6=SRhDNP~pxodJ-77XdYtVzY}e9}J-3hyEGCJP?^Xd2Sa5 z$9R@cP9*^M5v;2?fPZyB@C%v2!%bEccez3 z0OF}GW$0w z)4Ov%MPmuERZ`~Y#f^LXKmrXTb)r3NTGRsNKX1*LF_r8?L-T@PShuYdyYq8{lnSJ zhoJiB2$_WHOz}*ab8I9K?OmYWMP`-Nf+mX|D}Zy6h~gPcy(|#-jNo|YHpVcXwghcr zdt759`5N@!Yq=(or8uOtoYu9!d(D@^tM~iDtm%LhII#*E$wvVoBn-ndKol@Kbrc1~AK5HL8`k8f^i?DP7I1Zv=p}amgp18@E5SD- zik;omu?3V~{$Od;RVgDc;(Zo8@U_MmmM_sW^V>>7o2b&watug{IR@D22AdTjPSA`7 z4;SNS9-cbjM5HO0WjcGK=%yMe`pz!R=D~@=wHhWp)XHMQC1#8Ys5h#8`qa5Y} z2Ga~G=}ZYbn52Lo4p53}H`FuAJBrG<^+?$QVVbK;Dw+XoKcl1*2Ej(Dm*5lG{xac! zY8g)bzL9_BGaXG)f4vRqKkue7=GAEmTosz?!Z`pJ6`#v(V|)vwmGPC&wKvCgef#5Y zHK3|!$RJwRgOf4Qiw^;Cat#)r$2oBftZm=lz@a3k7=K_MU@4=+x78xDQ${qu%or4*D76rvBldlY!90zc zm)U|qZvs@fSfKCDg%mwpIPo2LPMG%uB9X@R4>hxq=g@24!n#toy=TJ0G!Zi@c1%ig zavQ0!8C5q=?psY>Pl8PRQDGK0FP+<>qZF;6Ljdp+02bN#UJ-PW2)xIsVkd+wj$L-~ ztWB_X=rC+mK~Gb7dqKPoPe~yin9v$Pa9+4LQ#>t03MzPe{aFK>vYqhb9QL^Irxoj( zj;95*HU2>X8T4QtXWb9^g9DGh@Q`{zyBs;iu1Us#t4Xw6AJ-8|-)0S36X{(Yz-yW~ z(m3(EDA}8&>OBd+u4u=<-V-nAWH=q0q9sjJLs?5Z8lidb_~&$PjhLse_2`bXU9!nU zHwO5!4sc^W5)ikJhYDVqaNq;NyilOa_K1bC0?t|aIctl1@@R#*RKfMYg4NW_2WYjy zk)65rp844kb)DJdf5bm8+He~fh}*3^osBv%now<9Y}6b%UQVCIBn7VpOKH((>Q_jQ zzheVfB-vZA;3uB_5@(`ZeQF+06dFAqUWL;IhH&gRS$>m1?=nt(*Hogad9MM}fzFVlOF>~#Fmx@$g>W3wYP= zesOBzQd)XiC0A1GdH^x?1aY~Bq@-wWnpFqsH?W<{Bdwxgt%uI7W zDbJ78)MeI4gYP@LlhLk{GZjER#rRxJor2IhP1_`kl{B8)SxloLxG45ufa}%x)!W|= zghtQLl+^oMu0Hsx_hu~Nhj*b0FRCIJWCcQ3 zTC?)^v^-YG5x%Zp<X-+seJ@IqCOS1c%=h(+}4x-N4VzmxKV>3L2c#II;V(%!wY>lD}~zes8;tu%)JQ z7*%~lvixab*E*K~FX(+Bgf5mXNe)mfNfwnY!JC46dopMQQI(8%JDJS|5Z+{8XL$bI zN8>pf-hl(jAD^{Ei_$|%j;FnpKULMt3MDDh`ip>{QI{Mmt*%$=tbF+Cy^HyNTISN? z)*o7vmD`FE-RkQ5AwPFSlFAvJOXMsa-~M_zw|N+O7yr1=jI&DGOteaIctkR3{Ea*E z*`&JNv7F^=ldWqzIIK4af0L0V%2{Q&l8;rlh;Rzm#ExAPV2OXR*$^ntACN)ezOnbW!4vH&fT#7C5|DHZ@Gn`T+>po{{H(Q4y5>q#uL466xvxQ z{-9=NB=smU}WvO)-+oQaREJ@6Z!s`+u6ck(-*1zddqd7)TTNx zRVG|2bKY3Z38FheVF1~+v`Xtq)Mew*$vgcJcw=<8h~&Gjox#o|t|Nk~NV>zwT8X4J zeqoOnxP0cfF?*folCoG)PR$oO>;V|v#QcrJLd03c{I^cDq6K6QPFFJR^qL;#g8La) zLm|VK0!#SeQ#&%wGWSwSX%Rk^*472M0U#qhkAk^C?Y9S%e7QkDm0$&nN1?p}zdqAc zDgg!4k1*O19OGZEBmnwYRviQi*)P?P5IGhYjH->Yzx%)1IQMWU(>{)iYa3DHI3sCN z6Dv)&1M!YtWJ*m=yN!%7gobs_X)qPls%$)5%OHy-t*yik+h81GL>Y`?^I{wtr*SNc zG&07i_kO6}_r2czXXcN&=eh3txt`~~f4|@N^ZjuX;UR96&^1L|UPVx|_-bD-Q%iS3 zeYm&?9^aa9Wg-^O?6m5a)!KsDbYB%noW>PbCN(yB*+m1oT|^ESg@@!k%FXhtLnx5g zNAUy3tI-BC_X@wac!5LL#h!E1r~)bQ?H55*mV80YH(}NwRbsq;CyyiS|0BVJ_sdsB z9#+J$2o=_&{>nV>{x|4G)Sf%tR&9Pf>BKz;$F7=cCwZsp6Z`i}cusSrkchz05Dje= zgFyhFK5Mg=$ss3l1cBA27+^FJfY@f{nsKYb?xWiKz*U*_z>X1?sm)`hkop~Don!^q z#|r@oIfZiUebpPN<<8Zdd(z{U;Hn2o?50-hKM10EWmSA_5P7<3IO_x*lr$`NLhqI9 z%=8O@b|{3zACX}GFiQ>xr=Pz-#_F;6Hq4j5-mVI->l1ViFzL7pY!R$}lZ6{CZ0aEx zMhEiBhF(UQGsFq_lm7xS4agmI00N<}+H0u*2d>QQcBu41f{u^jUA5Mq^{8keCFz>jRrQoit zW3zesvinl4!PeDm+k~hTP^WQ8u(%`2k}S#z>3*S3r$Ir(-(YQlI7i2AuH&YoC0*un z-I<)lkp*c-Cf@!?9XAclFn%cSswWWr(WGv{p&h!OyIWSM<`){QNBv={hPon+N6)+_ zjQfT%d~#!^%BU-=y%UbJb4?58322FlE~2unkcYA5tE|3+My;t{cd@#h0Kz&hC|6E!Opn@rEm#_(7;3P7E}Dx;XlSH$lEC4rLg>+=#zV zTi1W9!V%|_f&;u_kL9M=)vvdBn57)n99%3*k%#M*aNd8_<-ipD6xjRms^AA+6=ZJU zr|?&Qro?Gq(@M;|6!=A>1ySs^e`++v|CW9$*>Fc#r00>W1P9IMMl+=i$0k}Lt0P}m z`lrV|%BFwgZd4ToE*hBujsM3-}73Cea7t(-d{V(yTdKH} zgKCYKhiYdU$@7IfMDE$_A>>zH$<;RLtPF32c}d<<0WR|~_A)L=h&jgmv6zYCtnu^} z$vL_=g?5iiNJi#6UCydWrLz5X5%UeIjpXpww#Y!M)<(Jv8{1nGQ_diFQd&;qo(vAU z%T@lgD}EM<;oezkhj>cVNylTpx^Bz_+Yf?Nhz%h zX|kpO!e8P~Xa-{k5Aw$bC6RT&0a{%_Gwi(NW0m7t9pQ3<#tU$1|Gm&#&{tDRyJ!o1 zvAQ_4fZEMQC7$!Cz}_(PMdUz6`>3E(i$2E4+0?PaQo_Q|6~^{>M!p(rKs>W+b04V` z`&Q*#)TA6fDbj8$Ux|K(4Z)bL&KJi3uaz{UT=%2CYbk3o|Jrj~w+y+;+cuqcNG-}) zMK*(+2vUvF>KriCaYRMVM?3_byJdNxX|t@J^rTay&7 zz6~&Q&CQTS6>Vr*3OOk17kUd@->SbUepYyj%o<&Tp5}fl+FU%M7leHLtQq2H0;7)h z&i%C)d0|Wb)BHVW>&)EtGyg%4H_CT-f)aELKn3)8X+2RXBq#ak-q7)s$i-=6Zvztn zNt|j$ADF(^p*QkZdnZPr=N;m&0N4bIe~_Lzqg>MaW(rHFJ|Ni^#vtjIxnHtaJWW7*74_)_h*=a*AS75FX@aEl8KewThg@~vDB++EA8=Lz>@8f!1{=`EAl$|={9jlU z;Mi1DG8YNvtAwZ0FYra{FCZ-D9}HiK4HWZ&?>a8LW2I%09Nya9jo=`&lnKZIJBPpN l3Ab)e{k-CJ3F^>CZRNPEM|n=PD{!N_GgkH|iwPb#{tfjN)4l)z literal 0 HcmV?d00001 diff --git a/docs/endstops.md b/docs/endstops.md new file mode 100644 index 00000000..5b94cb97 --- /dev/null +++ b/docs/endstops.md @@ -0,0 +1,73 @@ +# Endstops + +Endstops are used for both "homing" the axis of a machine and for stopping the machine in the event it attempts to go "out of bounds". + +## Configuration Properties + +Each axis has two endstops: a "min" and a "max". Each endstop has the following properties: + +Name | Type | Default +--- | -- | -- +gpio_num | int | 0 +enabled | boolean | False +offset | int | 0 +debounce_ms | float | 100.0 +is_active_high | boolean | False + + +### gpio_num +The GPIO pin number, according to the silkscreen labels on ODrive + +### enabled +Enables/disables detection of the endstop. If disabled, homing and e-stop cannot take place. + +### offset +This is the location along the axis, in counts, that the endstop is positioned at. For example, if you want a position command of `0` to represent a position 100 counts away from the endstop, the offset would be -100 (because the endstop is at position -100) + +### debounce_ms +The debouncing time, in milliseconds, for this endstop. Most switches exhibit some sort of bounce, and this setting will help prevent the switch from triggering repeatedly. It works for both HIGH and LOW transitions, regardless of the setting of `is_active_high`. + +### is_active_high +This is how you configure the endstop to be either "NPN" or "PNP". An "NPN" configuration would be `is_active_high = False` whereas a PNP configuration is `is_active_high = True`. Refer to the following table for more information: + +![Endstop configuration](Endstop_configuration.png) + +3D printer endstops (like those that come with a RAMPS 1.4) are typically configuration **4**. + +### Configuring an endstop + +You can access these configuration properties through odrivetool. For example, if we want to configure a 3D printer-style minimum endstop on GPIO 5 for homing, and you want your motor to pull off the endstop about a quarter turn with a 8192 cpr encoder, you would set: + +``` +..min_endstop.config.gpio_num = 5 +..min_endstop.config.is_active_high = 4 +..min_endstop.config.offset = -2048; +..min_endstop.config.enabled = True + +.save_configuration() +.reboot() +``` + + +## Homing + +Homing is possible once the ODrive has closed loop control over the axis. To trigger homing, we use must first be in AXIS_STATE_CLOSED_LOOP_CONTROL, then we call`..controller.home_axis()` This starts the homing sequence. The homing sequence works as follows: + +1. Verify that the `min_endstop` is `enabled` +2. Drive towards the `min_endstop` in velocity control mode at `controller.config.homing_speed` +3. When the `min_endstop` is pressed, set the current position = `min_endstop.config.offset` +4. Request position control mode, and move to the positon = `0` + +### Homing Speed +Homing speed is configurable through the value +`..controller.config.homing_speed` in counts/second. It has the default value of 2000 counts/second. + +Note the assumption is made that `min_endstop` is in the negative direction, thus the velocity commanded is `-controller.config.homing_speed`, which will drive the axis towards the endstop. If you have an unusual setup and want to change this behaviour, simply use a negative value for `homing_speed`. + +### Homing at startup +It is possible to configure the odrive to enter homing immediately at startup. For safety reasons, we require the user to specifically enable closed loop control at startup, even if homing is requested. Thus, to enable homing at startup, the following must be configured: + +``` +..config.startup_closed_loop_control = True +..config.startup_homing = True +```