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',