diff --git a/CHANGELOG.md b/CHANGELOG.md index 980d4e54..bad1b4c9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -87,6 +87,8 @@ Please add a note of your changes below this heading if you make a Pull Request. ## [0.4.3] - 2018-08-30 ### 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/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index 7c7a4e63..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, - 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 4276e697..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, - 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 diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 9980133a..6589532e 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -13,28 +13,35 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, SensorlessEstimator& sensorless_estimator, Controller& controller, Motor& motor, - TrapezoidalTrajectory& trap) : - hw_config_(hw_config), + TrapezoidalTrajectory& trap, + Endstop& min_endstop, + Endstop& max_endstop) + : hw_config_(hw_config), config_(config), encoder_(encoder), sensorless_estimator_(sensorless_estimator), controller_(controller), motor_(motor), - trap_(trap) { + trap_(trap), + min_endstop_(min_endstop), + max_endstop_(max_endstop) +{ encoder_.axis_ = this; sensorless_estimator_.axis_ = this; controller_.axis_ = this; motor_.axis_ = this; trap_.axis_ = this; - decode_step_dir_pins(); update_watchdog_settings(); + min_endstop_.axis_ = this; + max_endstop_.axis_ = this; } static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } + // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. void Axis::setup() { @@ -119,8 +126,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, - step_cb_wrapper, this); + GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN, step_cb_wrapper, this); step_dir_active_ = true; } else { @@ -158,6 +164,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(); bool ret = check_for_errors(); odCAN->send_heartbeat(this); return ret; @@ -275,6 +283,26 @@ bool Axis::run_closed_loop_control_loop() { float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ + + // Handle the homing case + if (homing_state_ == HOMING_STATE_HOMING) { + 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_.getEndstopState()){ + homing_state_ = HOMING_STATE_IDLE; + } + } else { + // Check for endstop presses + if (min_endstop_.config_.enabled && min_endstop_.getEndstopState()) { + return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; + } else if (max_endstop_.config_.enabled && max_endstop_.getEndstopState()) { + return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false; + } + } return true; }); set_step_dir_active(false); @@ -318,11 +346,18 @@ 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){ + 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; 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) @@ -365,6 +400,10 @@ void Axis::run_state_machine_loop() { status = encoder_.run_direction_find(); } break; + case AXIS_STATE_HOMING: + status = controller_.home_axis(); + break; + case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: { if (!motor_.is_calibrated_) goto invalid_state_label; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 7971f475..12ccea3b 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,6 +5,13 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif + +enum HomingState_t { + HOMING_STATE_IDLE, + HOMING_STATE_HOMING, + HOMING_STATE_MOVE_TO_ZERO +}; + class Axis { public: enum Error_t { @@ -21,6 +28,8 @@ public: ERROR_CONTROLLER_FAILED = 0x200, ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, + ERROR_MIN_ENDSTOP_PRESSED = 0x1000, + ERROR_MAX_ENDSTOP_PRESSED = 0x2000 ERROR_ESTOP_REQUESTED = 0x1000 }; @@ -36,6 +45,7 @@ public: AXIS_STATE_CLOSED_LOOP_CONTROL = 8, //min_endstop_.config_.enabled) { + set_vel_setpoint(-config_.homing_speed, 0.0f); + axis_->homing_state_ = HOMING_STATE_HOMING; + } else { + return false; + } + 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 020f34d0..64adca26 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -32,6 +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] }; explicit Controller(Config_t& config); @@ -45,6 +46,8 @@ public: // Trajectory-Planned control void move_to_pos(float goal_point); void move_incremental(float displacement, bool from_goal_point); + + bool home_axis(); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); @@ -110,17 +113,19 @@ 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, "pos_setpoint", "vel_feed_forward", "current_feed_forward"), make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, "vel_setpoint", "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, - "current_setpoint"), + "current_setpoint"), make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"), - make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), + make_protocol_function("home_axis", *this, &Controller::home_axis) ); } }; diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp new file mode 100644 index 00000000..809e52f1 --- /dev/null +++ b/Firmware/MotorControl/endstop.cpp @@ -0,0 +1,50 @@ +#include + +Endstop::Endstop(Endstop::Config_t& config) + : config_(config) { + set_endstop_enabled(config_.enabled); +} + +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 + 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 + } + } else { + endstop_state_ = false; + } +} + +bool Endstop::getEndstopState() { + return endstop_state_; +} + +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 new file mode 100644 index 00000000..cdca3fdc --- /dev/null +++ b/Firmware/MotorControl/endstop.hpp @@ -0,0 +1,44 @@ +#ifndef __ENDSTOP_HPP +#define __ENDSTOP_HPP + +class Endstop { + public: + 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 update_endstop_config(); + void set_endstop_enabled(bool enable); + + 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_object("config", + 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))); + } + + private: + bool pin_state_ = false; + volatile float debounce_timer_ = 0; +}; +#endif \ No newline at end of file diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 88e3262b..3f39b1b6 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -17,6 +17,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]; +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 }; @@ -32,6 +34,8 @@ typedef Config< Controller::Config_t[AXIS_COUNT], Motor::Config_t[AXIS_COUNT], TrapezoidalTrajectory::Config_t[AXIS_COUNT], + Endstop::Config_t[AXIS_COUNT], + Endstop::Config_t[AXIS_COUNT], Axis::Config_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { @@ -43,8 +47,10 @@ void save_configuration(void) { &controller_configs, &motor_configs, &trap_configs, + &min_endstop_configs, + &max_endstop_configs, &axis_configs)) { - //printf("saving configuration failed\r\n"); osDelay(5); + printf("saving configuration failed\r\n"); osDelay(5); } else { user_config_loaded_ = true; } @@ -61,6 +67,8 @@ extern "C" int load_configuration(void) { &controller_configs, &motor_configs, &trap_configs, + &min_endstop_configs, + &max_endstop_configs, &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); @@ -75,6 +83,8 @@ extern "C" int load_configuration(void) { // Default step/dir pins are different, so we need to explicitly load them Axis::load_default_step_dir_pin_config(hw_configs[i].axis_config, &axis_configs[i]); Axis::load_default_can_id(i, axis_configs[i]); + min_endstop_configs[i] = Endstop::Config_t(); + max_endstop_configs[i] = Endstop::Config_t(); } } else { user_config_loaded_ = true; @@ -180,8 +190,10 @@ int odrive_main(void) { hw_configs[i].gate_driver_config, motor_configs[i]); TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_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, *trap); + *encoder, *sensorless_estimator, *controller, *motor, *trap, *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 60b4818b..9b9658c9 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -122,6 +122,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 027db034..65d10538 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -165,6 +165,7 @@ build{ 'MotorControl/axis.cpp', 'MotorControl/motor.cpp', 'MotorControl/encoder.cpp', + 'MotorControl/endstop.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', 'MotorControl/trapTraj.cpp', diff --git a/docs/Endstop_configuration.png b/docs/Endstop_configuration.png new file mode 100644 index 00000000..56a618ff Binary files /dev/null and b/docs/Endstop_configuration.png differ 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 +``` diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index d4425a21..616662b6 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -12,6 +12,7 @@ AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 AXIS_STATE_CLOSED_LOOP_CONTROL = 8 AXIS_STATE_LOCKIN_SPIN = 9 AXIS_STATE_ENCODER_DIR_FIND = 10 +AXIS_STATE_HOMING = 9 class errors: class axis: