diff --git a/CHANGELOG.md b/CHANGELOG.md index 736558ba..aad81ce5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,7 @@ # Unreleased Features Please add a note of your changes below this heading if you make a Pull Request. +### Added +* [Mechanical brake support](docs/mechanical-brakes.md) ### Changed @@ -63,6 +65,9 @@ Please add a note of your changes below this heading if you make a Pull Request. * `axis.motor.thermal_current_lim` has been removed. Instead a new property is available `axis.motor.effective_current_lim` which contains the effective current limit including any thermal limits. * `axis.motor.get_inverter_temp()`, `axis.motor.inverter_temp_limit_lower` and `axis.motor.inverter_temp_limit_upper` have been moved to seperate fet thermistor object under `axis.fet_thermistor`. `get_inverter_temp()` function has been renamed to `temp` and is now a read-only property. * `axis.config.counts_per_step` is now `axis.config.turns_per_step` +* Outputs of `axis.sensorless_estimator` are now in turns/s instead of electrical rad/s +* Fixed bug of high current during lockin-ramp caused by `motor::update()` expecting a torque command instead of current +* Fixed bug where commanded velocity was extremely high just after sensorless ramp when using `input_mode` INPUT_MODE_VEL_RAMP caused by `vel_setpoint` and `axis.config.sensorless_ramp.vel` being in different units # Releases diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index 04d3e4bf..2f0c7a10 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -102,6 +102,7 @@ Encoder encoders[AXIS_COUNT] = { // TODO: this has no hardware dependency and should be allocated depending on config Endstop endstops[2 * AXIS_COUNT]; +MechanicalBrake mechanical_brakes[AXIS_COUNT]; SensorlessEstimator sensorless_estimators[AXIS_COUNT]; Controller controllers[AXIS_COUNT]; @@ -122,6 +123,7 @@ std::array axes{{ motors[0], // motor trap[0], // trap endstops[0], endstops[1], // min_endstop, max_endstop + mechanical_brakes[0], // mechanical brake }, { 1, // axis_num @@ -141,6 +143,7 @@ std::array axes{{ motors[1], // motor trap[1], // trap endstops[2], endstops[3], // min_endstop, max_endstop + mechanical_brakes[1], // mechanical brake }, }}; diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 2b8c062b..99aa7b80 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -19,7 +19,8 @@ Axis::Axis(int axis_num, Motor& motor, TrapezoidalTrajectory& trap, Endstop& min_endstop, - Endstop& max_endstop) + Endstop& max_endstop, + MechanicalBrake& mechanical_brake) : axis_num_(axis_num), default_step_gpio_pin_(default_step_gpio_pin), default_dir_gpio_pin_(default_dir_gpio_pin), @@ -33,6 +34,7 @@ Axis::Axis(int axis_num, trap_traj_(trap), min_endstop_(min_endstop), max_endstop_(max_endstop), + mechanical_brake_(mechanical_brake), current_limiters_(make_array( static_cast(&fet_thermistor), static_cast(&motor_thermistor))), @@ -49,6 +51,7 @@ Axis::Axis(int axis_num, trap_traj_.axis_ = this; min_endstop_.axis_ = this; max_endstop_.axis_ = this; + mechanical_brake_.axis_ = this; } Axis::LockinConfig_t Axis::default_calibration() { @@ -432,6 +435,7 @@ bool Axis::run_homing() { } bool Axis::run_idle_loop() { + mechanical_brake_.engage(); set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on); while (requested_state_ == AXIS_STATE_UNDEFINED) { motor_.setup(); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 1dce2112..5d60705d 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -10,6 +10,7 @@ class Axis; #include "open_loop_controller.hpp" #include "trapTraj.hpp" #include "endstop.hpp" +#include "mechanical_brake.hpp" #include "low_level.h" #include "utils.hpp" #include "task_timer.hpp" @@ -105,7 +106,8 @@ public: Motor& motor, TrapezoidalTrajectory& trap, Endstop& min_endstop, - Endstop& max_endstop); + Endstop& max_endstop, + MechanicalBrake& mechanical_brake); bool apply_config(); void clear_config(); @@ -160,6 +162,7 @@ public: TrapezoidalTrajectory& trap_traj_; Endstop& min_endstop_; Endstop& max_endstop_; + MechanicalBrake& mechanical_brake_; TaskTimes task_times_; // List of current_limiters and thermistors to diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index a6890d2b..e439a104 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -6,9 +6,10 @@ #include "utils.hpp" #include + class Encoder : public ODriveIntf::EncoderIntf { public: - const uint32_t MODE_FLAG_ABS = 0x100; + static constexpr uint32_t MODE_FLAG_ABS = 0x100; struct Config_t { Mode mode = MODE_INCREMENTAL; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index d11d8aec..6a9d8a48 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -45,6 +45,7 @@ static bool config_read_all() { config_manager.read(&axes[i].trap_traj_.config_) && config_manager.read(&axes[i].min_endstop_.config_) && config_manager.read(&axes[i].max_endstop_.config_) && + config_manager.read(&axes[i].mechanical_brake_.config_) && config_manager.read(&motors[i].config_) && config_manager.read(&fet_thermistors[i].config_) && config_manager.read(&axes[i].motor_thermistor_.config_) && @@ -64,6 +65,7 @@ static bool config_write_all() { config_manager.write(&axes[i].trap_traj_.config_) && config_manager.write(&axes[i].min_endstop_.config_) && config_manager.write(&axes[i].max_endstop_.config_) && + config_manager.write(&axes[i].mechanical_brake_.config_) && config_manager.write(&motors[i].config_) && config_manager.write(&fet_thermistors[i].config_) && config_manager.write(&axes[i].motor_thermistor_.config_) && @@ -83,6 +85,7 @@ static void config_clear_all() { axes[i].trap_traj_.config_ = {}; axes[i].min_endstop_.config_ = {}; axes[i].max_endstop_.config_ = {}; + axes[i].mechanical_brake_.config_ = {}; motors[i].config_ = {}; fet_thermistors[i].config_ = {}; axes[i].motor_thermistor_.config_ = {}; @@ -385,6 +388,13 @@ static void rtos_main(void*) { // must happen after communication is initialized pwm0_input.init(); + // Set up the CS pins for absolute encoders (TODO: move to GPIO init switch statement) + for(auto& axis : axes){ + if(axis.encoder_.config_.mode & Encoder::MODE_FLAG_ABS){ + axis.encoder_.abs_spi_cs_pin_init(); + } + } + // Try to initialized gate drivers for fault-free startup. // If this does not succeed, a fault will be raised and the idle loop will // periodically attempt to reinit the gate driver. @@ -526,6 +536,7 @@ extern "C" int main(void) { if (mode == ODriveIntf::GPIO_MODE_DIGITAL || mode == ODriveIntf::GPIO_MODE_DIGITAL_PULL_UP || mode == ODriveIntf::GPIO_MODE_DIGITAL_PULL_DOWN || + mode == ODriveIntf::GPIO_MODE_MECH_BRAKE || mode == ODriveIntf::GPIO_MODE_ANALOG_IN) { GPIO_InitStruct.Alternate = 0; } else { @@ -622,6 +633,11 @@ extern "C" int main(void) { GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; } break; + case ODriveIntf::GPIO_MODE_MECH_BRAKE: { + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + } break; default: { odrv.misconfigured_ = true; continue; diff --git a/Firmware/MotorControl/mechanical_brake.cpp b/Firmware/MotorControl/mechanical_brake.cpp new file mode 100644 index 00000000..c9d3602f --- /dev/null +++ b/Firmware/MotorControl/mechanical_brake.cpp @@ -0,0 +1,13 @@ +#include + +void MechanicalBrake::engage() { + if (odrv.config_.gpio_modes[config_.gpio_num] == ODriveIntf::GPIO_MODE_MECH_BRAKE){ + get_gpio(config_.gpio_num).write(config_.is_active_low ? 0 : 1); + } +} + +void MechanicalBrake::release() { + if (odrv.config_.gpio_modes[config_.gpio_num] == ODriveIntf::GPIO_MODE_MECH_BRAKE){ + get_gpio(config_.gpio_num).write(config_.is_active_low ? 1 : 0); + } +} diff --git a/Firmware/MotorControl/mechanical_brake.hpp b/Firmware/MotorControl/mechanical_brake.hpp new file mode 100644 index 00000000..26362ce5 --- /dev/null +++ b/Firmware/MotorControl/mechanical_brake.hpp @@ -0,0 +1,25 @@ +#ifndef __MECHANICAL_BRAKE_HPP +#define __MECHANICAL_BRAKE_HPP + +#include + +class MechanicalBrake : public ODriveIntf::MechanicalBrakeIntf { + public: + struct Config_t { + uint16_t gpio_num = 0; + bool is_active_low = true; + + // custom setters + MechanicalBrake* parent = nullptr; + void set_gpio_num(uint16_t value) { gpio_num = value; } + }; + + MechanicalBrake() {} + + MechanicalBrake::Config_t config_; + Axis* axis_ = nullptr; + + void release(); + void engage(); +}; +#endif // __MECHANICAL_BRAKE_HPP \ No newline at end of file diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index a5ac87d0..109a7f00 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -168,6 +168,8 @@ Motor::Motor(TIM_HandleTypeDef* timer, * @returns: True on success, false otherwise */ bool Motor::arm(PhaseControlLaw<3>* control_law) { + axis_->mechanical_brake_.release(); + CRITICAL_SECTION() { control_law_ = control_law; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index ca74b553..ee17e125 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -140,6 +140,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include #include #include #include diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 2bbe6e8d..70819875 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -82,8 +82,8 @@ bool SensorlessEstimator::update() { pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase); // update PLL velocity phase_vel_ += current_meas_period * pll_ki * delta_phase; - - vel_estimate_ = phase_vel_ / (2 * M_PI); + // convert to mechanical turns/s for controller usage. + vel_estimate_ = phase_vel_ / (std::max((float)axis_->motor_.config_.pole_pairs, 1.0f) * 2.0f * M_PI); return true; }; diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index e8faf520..c6c69d8c 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -190,6 +190,7 @@ sources = { 'MotorControl/encoder.cpp', 'MotorControl/endstop.cpp', 'MotorControl/async_estimator.cpp', + 'MotorControl/mechanical_brake.cpp', 'MotorControl/controller.cpp', 'MotorControl/foc.cpp', 'MotorControl/open_loop_controller.cpp', diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 3d782135..4d2f27ce 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -468,6 +468,7 @@ interfaces: trap_traj: TrapezoidalTrajectory min_endstop: Endstop max_endstop: Endstop + mechanical_brake: MechanicalBrake task_times: c_is_class: False attributes: @@ -990,7 +991,6 @@ interfaces: accel_limit: float32 decel_limit: float32 - ODrive.Endstop: c_is_class: True attributes: @@ -1004,6 +1004,22 @@ interfaces: is_active_high: bool debounce_ms: {type: uint32, c_setter: set_debounce_ms} + ODrive.MechanicalBrake: + c_is_class: True + attributes: + config: + c_is_class: False + attributes: + gpio_num: {type: uint16, c_setter: set_gpio_num} + is_active_low: bool + functions: + engage: + doc: | + This function engages the mechanical brake if one is present and enabled. + release: + doc: | + This function releases the mecahncal brake if one is present and enabled. + ODrive.TaskTimer: c_is_class: True attributes: @@ -1039,6 +1055,7 @@ valuetypes: Enc0: {doc: The pin is used by quadrature encoder 0.} Enc1: {doc: The pin is used by quadrature encoder 1.} Enc2: {doc: This mode is not supported on ODrive v3.x.} + MechBrake: {doc: This is to support external mechanical brakes.} ODrive.Can.Protocol: values: {Simple: } diff --git a/docs/_data/index.yaml b/docs/_data/index.yaml index 0ff47762..273029b6 100644 --- a/docs/_data/index.yaml +++ b/docs/_data/index.yaml @@ -17,6 +17,8 @@ sections: url: /encoders - title: Homing & Endstops url: /endstops + - title: Mechanical Brakes + url: /mechanical-brakes - title: Thermistors url: /thermistors - title: Control & Tuning diff --git a/docs/commands.md b/docs/commands.md index f19d5f33..c5104359 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -81,15 +81,15 @@ All variables that are part of a `[...].config` object can be saved to non-volat ## Setting up sensorless The ODrive can run without encoder/hall feedback, but there is a minimum speed, usually around a few hunderd RPM. -To give an example, suppose you have a motor with 7 pole pairs, and you want to spin it at 3000 RPM. Then you would set the `input_vel` to `3000 * 2*pi/60 * 7 = 2199 rad/s electrical`. - -Below are some suggested starting parameters that you can use. Note that you _must_ set the `pm_flux_linkage` correctly for sensorless mode to work. +Below are some suggested starting parameters that you can use. Note that you _must_ set the `pm_flux_linkage` correctly for sensorless mode to work. Motor calibration and setup must also be completed before sensorless mode will work. ``` odrv0.axis0.controller.config.vel_gain = 0.01 odrv0.axis0.controller.config.vel_integrator_gain = 0.05 odrv0.axis0.controller.config.control_mode = 2 -odrv0.axis0.controller.input_vel = 400 +odrv0.axis0.controller.input_vel = 10 +odrv0.axis0.controller.config.vel_limit = +odrv0.axis0.motor.config.current_lim = 2 * odrv0.axis0.config.sensorless_ramp.current odrv0.axis0.sensorless_estimator.config.pm_flux_linkage = 5.51328895422 / ( * ) odrv0.axis0.config.enable_sensorless_mode = True ``` diff --git a/docs/interfaces.md b/docs/interfaces.md index 28a1b3dc..0b9cb6c3 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -18,25 +18,25 @@ The ODrive can be controlled over various ports and protocols. If you're comfort ## Pinout -| # | Label | `GPIO_MODE_DIGITAL` | `GPIO_MODE_ANALOG_IN` | `GPIO_MODE_UART0` | `GPIO_MODE_PWM0` | `GPIO_MODE_CAN0` | `GPIO_MODE_I2C0` | `GPIO_MODE_ENC0` | `GPIO_MODE_ENC1` | -|----|---------------|------------------------|-----------------------|-------------------|------------------|------------------|------------------|------------------|------------------| -| 0 | _not a pin_ | | | | | | | | | -| 1 | GPIO1 (+) | general purpose | analog input | **UART0.TX** | PWM0.0 | | | | | -| 2 | GPIO2 (+) | general purpose | analog input | **UART0.RX** | PWM0.1 | | | | | -| 3 | GPIO3 | general purpose | **analog input** | | PWM0.2 | | | | | -| 4 | GPIO4 | general purpose | **analog input** | | PWM0.3 | | | | | -| 5 | GPIO5 | general purpose | **analog input** (*) | | | | | | | -| 6 | GPIO6 (*) (+) | **general purpose** | | | | | | | | -| 7 | GPIO7 (*) (+) | **general purpose** | | | | | | | | -| 8 | GPIO8 (*) (+) | **general purpose** | | | | | | | | -| 9 | M0.A | general purpose | | | | | | **ENC0.A** | | -| 10 | M0.B | general purpose | | | | | | **ENC0.B** | | -| 11 | M0.Z | **general purpose** | | | | | | | | -| 12 | M1.A | general purpose | | | | | I2C.SCL | | **ENC1.A** | -| 13 | M1.B | general purpose | | | | | I2C.SDA | | **ENC1.B** | -| 14 | M1.Z | **general purpose** | | | | | | | | -| 15 | _not exposed_ | general purpose | | | | **CAN0.RX** | I2C.SCL | | | -| 16 | _not exposed_ | general purpose | | | | **CAN0.TX** | I2C.SDA | | | +| # | Label | `GPIO_MODE_DIGITAL` | `GPIO_MODE_ANALOG_IN` | `GPIO_MODE_UART0` | `GPIO_MODE_PWM0` | `GPIO_MODE_CAN0` | `GPIO_MODE_I2C0` | `GPIO_MODE_ENC0` | `GPIO_MODE_ENC1` | `GPIO_MODE_MECH_BRAKE` | +|----|---------------|------------------------|-----------------------|-------------------|------------------|------------------|------------------|------------------|------------------|------------------------| +| 0 | _not a pin_ | | | | | | | | | | +| 1 | GPIO1 (+) | general purpose | analog input | **UART0.TX** | PWM0.0 | | | | | mechanical brake | +| 2 | GPIO2 (+) | general purpose | analog input | **UART0.RX** | PWM0.1 | | | | | mechanical brake | +| 3 | GPIO3 | general purpose | **analog input** | | PWM0.2 | | | | | mechanical brake | +| 4 | GPIO4 | general purpose | **analog input** | | PWM0.3 | | | | | mechanical brake | +| 5 | GPIO5 | general purpose | **analog input** (*) | | | | | | | mechanical brake | +| 6 | GPIO6 (*) (+) | **general purpose** | | | | | | | | mechanical brake | +| 7 | GPIO7 (*) (+) | **general purpose** | | | | | | | | mechanical brake | +| 8 | GPIO8 (*) (+) | **general purpose** | | | | | | | | mechanical brake | +| 9 | M0.A | general purpose | | | | | | **ENC0.A** | | | +| 10 | M0.B | general purpose | | | | | | **ENC0.B** | | | +| 11 | M0.Z | **general purpose** | | | | | | | | | +| 12 | M1.A | general purpose | | | | | I2C.SCL | | **ENC1.A** | | +| 13 | M1.B | general purpose | | | | | I2C.SDA | | **ENC1.B** | | +| 14 | M1.Z | **general purpose** | | | | | | | | | +| 15 | _not exposed_ | general purpose | | | | **CAN0.RX** | I2C.SCL | | | | +| 16 | _not exposed_ | general purpose | | | | **CAN0.TX** | I2C.SDA | | | | (*) ODrive v3.5 and later
diff --git a/docs/mechanical-brakes.md b/docs/mechanical-brakes.md new file mode 100644 index 00000000..ab2164e1 --- /dev/null +++ b/docs/mechanical-brakes.md @@ -0,0 +1,61 @@ +# Mechanical Brake + +Some systems employ mechanical brakes on motors as a safety feature. These brakes can also be engaged as a power-saving function if the motor is not moving, but still under load. + +ODrive supports the use of its GPIO pins to connect to external brake drive electronics. + +When the ODrive engages the drive electronics, the brake will be disabled. When the drive enters a fault or idle state, the brake will be re-engaged. + +--- + +## Mechanical Brake Configuration +Each axis supports one mechanical brake. The following properties are accessible through `odrivetool`: + +Name | Type | Default +--- | -- | -- +gpio_num | int | 0 +is_active_low | boolean | true + +### gpio_num +The GPIO pin number, according to the silkscreen labels on ODrive. Set with these commands: +``` +..mechanical_brake.config.gpio_num = <1, 2, 3, 4, 5, 6, 7, 8> +``` +After GPIO pin number is changed, you'll need to run `.save_configuration()` and `.reboot()` for changes to take effect. + +### is_active_low +Most safety braking systems are active low, e.g. when the power is off, the brake is on. If the system uses brake drive electronics which use active high logic, flip this bit then reconsider the safety implications of your design... + +### Enabling +The configuration of the mechanical brake will enable the brake functionality. There's no need to specifically 'enable' this feature. + + +### Example + +Let's say we're hacking away on an old ABB robotic arm. We've wired a 24V brake drive circuit triggered by GPIO5. When GPIO5 is driven, it will release the brakes on the axis we're moving. + +We need to notify the axis of the GPIO number we've attached our brake to, and configure the ODrive pin mode to `GPIO_MODE_MECH_BRAKE`: +``` +..mechanical_brake.config.gpio_num = 5 +.config.gpio5_mode = GPIO_MODE_MECH_BRAKE +``` + +Pin configurations only take effect after a save/reboot so don't forget to run: +``` +.save_configuration() +.reboot() +``` + +### Testing The Mechanical Brakes +Depending on your system this could be a dangerous experiment. Ensure that you have taken all necessary precautions to confirm if the wrong brake were inadvertently released it would not lead to injury or damage to equipment. + +``` +..mechanical_brake.release() +``` +Note: If a brake is configured, it will be automatically engaged/disengaged during the next state machine step. + +After you're satisfied with the testing, you can re-enable the brake using the command + +``` +..mechanical_brake.engage() +``` diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 6445a530..c2a6e329 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -18,6 +18,7 @@ GPIO_MODE_PWM0 = 10 GPIO_MODE_ENC0 = 11 GPIO_MODE_ENC1 = 12 GPIO_MODE_ENC2 = 13 +GPIO_MODE_MECH_BRAKE = 14 # ODrive.Can.Protocol PROTOCOL_SIMPLE = 0 diff --git a/tools/odrive/tests/run_all_tests.sh b/tools/odrive/tests/run_all_tests.sh new file mode 100755 index 00000000..ab04a99c --- /dev/null +++ b/tools/odrive/tests/run_all_tests.sh @@ -0,0 +1,30 @@ +#!/usr/bin/env bash +set -euo pipefail + +declare -a tests=('analog_input_test.py' + 'calibration_test.py' + 'can_test.py' + 'closed_loop_test.py' + 'encoder_test.py' + 'fibre_test.py' + 'integration_test.py' + 'nvm_test.py' + 'pwm_input_test.py' + 'step_dir_test.py' + 'uart_ascii_test.py' + ) +summary="" + +for test in "${tests[@]}"; do + (ipython3 "$test" -- --test-rig-yaml ../../test-rig-rpi.yaml || true) | tee /tmp/odrivetest.log + if grep "All tests passed!" /tmp/odrivetest.log; then + summary="$summary - $test: passed"$'\n' + else + summary="$summary - $test: failed"$'\n' + fi + + echo "########################" + echo "Current status:" + echo -n "$summary" + echo "########################" +done