From f84ff36c747b07c3044b1025a83ab07e400b4327 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 30 Apr 2020 11:44:14 +0200 Subject: [PATCH 01/19] only enable vbus_voltage clamping if dc_bus_overvoltage_trip_level > nominal_voltage --- Firmware/MotorControl/low_level.cpp | 5 ++++- Firmware/MotorControl/odrive_main.h | 7 ++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 14a59b4d..689bb277 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -594,7 +594,10 @@ void update_brake_current() { // Don't start braking until -Ibus > regen_current_allowed float brake_current = -Ibus_sum - board_config.max_regen_current; float brake_duty = brake_current * std::abs(board_config.brake_resistance) / vbus_voltage; - brake_duty += std::max((vbus_voltage - board_config.nominal_voltage) / (board_config.dc_bus_overvoltage_trip_level / 0.9f - board_config.nominal_voltage), 0.0f); + + if (board_config.nominal_voltage < board_config.dc_bus_overvoltage_trip_level) { + brake_duty += std::max((vbus_voltage - board_config.nominal_voltage) / (board_config.dc_bus_overvoltage_trip_level / 0.9f - board_config.nominal_voltage), 0.0f); + } // Clamp the duty cycle brake_duty = std::clamp(brake_duty, 0.0f, 0.9f); diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index fe14446f..a505b3e1 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -95,7 +95,12 @@ struct BoardConfig_t { // Date: Fri, 1 May 2020 11:36:07 +0200 Subject: [PATCH 02/19] update developer documentation --- docs/configuring-vscode.md | 11 ++++++----- docs/developer-guide.md | 4 ++-- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/docs/configuring-vscode.md b/docs/configuring-vscode.md index 69edf772..7eb70178 100644 --- a/docs/configuring-vscode.md +++ b/docs/configuring-vscode.md @@ -23,12 +23,12 @@ Before doing the VSCode setup, make sure you've installed all of your [prerequis You should now be ready to compile and test the ODrive project. ## Building the Firmware -* Tasks -> Run Build Task +* Terminal -> Run Build Task (Ctrl+Shift+B) A terminal window will open with your native shell. VSCode is configured to run the command `make -j4` in this terminal. ## Flashing the Firmware -* Tasks -> Run Task -> flash +* Terminal -> Run Task -> flash A terminal window will open with your native shell. VSCode is configured to run the command `make flash` in this terminal. @@ -42,12 +42,13 @@ Note: If developing on Windows, you should have `arm-none-eabi-gdb` and `openOCD * Make sure you have the Firmware folder as your active folder * Set `CONFIG_DEBUG=true` in the tup.config file * Flash the board with the newest code (starting debug session doesn't do this) - * Debug -> Start Debugging (or press F5) + * In the _Run_ tab (Ctrl+Shift+D), select "Debug ODrive (Firmware)" + * Press _Start Debugging_ (or press F5) * The processor will reset and halt. * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. - * Run (F5) + * _Continue_ (F5) * Stepping over/in/out, restarting, and changing breakpoints can be done by first pressing the "pause" (F6) button at the top the screen. - * When done debugging, simply stop (Shift+F5) the debugger. It will kill your openOCD process too. + * When done debugging, simply stop (Shift+F5) the debugger. It will kill your openOCD process too. ## Cleaning the Build This sometimes needs to be done if you change branches. diff --git a/docs/developer-guide.md b/docs/developer-guide.md index 6f351070..dc20b208 100644 --- a/docs/developer-guide.md +++ b/docs/developer-guide.md @@ -59,7 +59,7 @@ sudo apt-get install openocd sudo add-apt-repository ppa:jonathonf/tup && sudo apt-get update && sudo apt-get install tup ``` -#### Arch Linux +#### Linux (Arch Linux) ```bash sudo pacman -S arm-none-eabi-gcc arm-none-eabi-binutils sudo pacman -S arm-none-eabi-gdb @@ -128,7 +128,7 @@ If the flashing worked, you can connect to the board using the [odrivetool](gett

## Testing -The script `tools/run_tests.py` runs a sequence of automated tests for several firmware features as well as high power burn-in tests. Some tests only need one ODrive and one motor/encoder pair while other tests need a back-to-back test rig such as [this one](https://cad.onshape.com/documents/026bda35ad5dff4d73c1d37f/w/ae302174f402737e1fdb3783/e/5ca143a6e5e24daf1fe8e434). In any case, to run the tests you need to provide a YAML file that lists the parameters of your test setup. An example can be found at [`tools/test-rig-parallel.yaml`](tools/test-rig-parallel.yaml`). The programmer serial number can be found by running `Firmware/find_programmer.sh` (make sure it has the latest formware from STM). +The script `tools/run_tests.py` runs a sequence of automated tests for several firmware features as well as high power burn-in tests. Some tests only need one ODrive and one motor/encoder pair while other tests need a back-to-back test rig such as [this one](https://cad.onshape.com/documents/026bda35ad5dff4d73c1d37f/w/ae302174f402737e1fdb3783/e/5ca143a6e5e24daf1fe8e434). In any case, to run the tests you need to provide a YAML file that lists the parameters of your test setup. An example can be found at [`tools/test-rig-parallel.yaml`](tools/test-rig-parallel.yaml`). The programmer serial number can be found by running `Firmware/find_programmer.sh` (make sure it has the latest firmware from STM).
The test script commands the ODrive to high currents and high motor speeds so if your ODrive is connected to anything other than a stirdy test-rig (or free spinning motors), it will probably break your machine.
From 6ade7e47edfd2e5be237db2e9f9b5bbe4a0f8368 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 1 May 2020 11:35:33 +0200 Subject: [PATCH 03/19] update testing documentation --- Firmware/.vscode/launch.json | 20 +++++ docs/testing.md | 122 +++++++++++++++++++++++++++--- tools/odrive/tests/test_runner.py | 2 +- 3 files changed, 131 insertions(+), 13 deletions(-) diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index 48fecec2..5107086e 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -30,6 +30,26 @@ "interface/stlink-v2.cfg", "target/stm32f4x_stlink.cfg", ], + "svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd", + "cwd": "${workspaceRoot}" + }, + { + // For the Cortex-Debug extension + // ssh -t odrv -L3333:localhost:3333 bash -c "\"openocd '-f' 'interface/stlink-v2.cfg' '-f' 'target/stm32f4x_stlink.cfg'\"" + "type": "cortex-debug", + "servertype": "external", + "gdbTarget": "localhost:3333", + "preLaunchCommands": [ + "load" + ], + "request": "launch", + "name": "Debug ODrive via external server", + "executable": "${workspaceRoot}/build/ODriveFirmware.elf", + "configFiles": [ + "interface/stlink-v2.cfg", + "target/stm32f4x_stlink.cfg", + ], + "svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd", "cwd": "${workspaceRoot}" }, ] diff --git a/docs/testing.md b/docs/testing.md index 347db2b1..46ba9b69 100644 --- a/docs/testing.md +++ b/docs/testing.md @@ -3,21 +3,119 @@ This section describes how to use the automated testing facilities. You don't have to do this as an end user. -They test the following aspects: - - System functions (communication interfaces, configuration storage) - - Functionality of the motor controller and state machine - - High speed and high load conditions - The testing facility consists of the following components: - * **Test rig:** In the simplest case this can be a single ODrive with a single motor and encoder pair. Can also be multiple ODrives with multiple axes, some of which may be mechanically coupled. + * **Test rig:** In the simplest case this can be a single ODrive optionally with a single motor and encoder pair. Can also be multiple ODrives with multiple axes, some of which may be mechanically coupled. * **Test host:** The PC on which the test script runs. All ODrives must be connected to the test host via USB. * **test-rig.yaml:** Describes your test rig. Make sure all values are correct. Incorrect values may physically break or fry your test setup. - * **run_tests.py:** This is the main script that runs all the tests. + * **test_runner.py:** This is the main script that runs all the tests. + * **..._test.py** The actual tests -## How to run +## The Tests -Example: + - `analog_input_test.py`: Analog Input + - `calibration_test.py`: Motor calibration, encoder offset calibration, encoder direction find, encoder index search + - `can_test.py`: Partial coverage of the commands described in [CAN Protocol](can-protocol) + - `closed_loop_test.py`: Velocity control, position control (TODO: sensorless control) + - `encoder_test.py`: Incremental encoder, hall effect encoder, sin/cos encoder, SPI encoders (AMS, CUI) + - `nvm_test.py`: Configuration storage + - `pwm_input_test.py`: PWM input + - `step_dir_test.py`: Step/dir input + - `uart_ascii_test.py`: Partial coverage of the commands described in [ASCII Protocol](ascii-protocol) + +All tests in a file can be run with e.g.: + + python3 uart_ascii_test.py --test-rig-yaml ../../test-rig-rpi.yaml + +See the following sections for a more detailed test flow description. + +## Our test rig + +Our test rig essentially consists of the following components: + + - an ODrive as the test subject + - a Teensy 4.0 to emulate external hardware such as encoders + - a Motor + Encoder pair for closed loop control tests + - a Raspberry Pi 4.0 as test host + - a CAN hat for the Raspberry Pi for CAN tests + +This document is therefore centered around this test rig layout. +If your test rig differs, you may be able to run some but not all of the tests. + +## How to set up a Raspberry Pi as testing host + + 1. Install Raspbian Lite on a Raspberry Pi 4.0. I used the NOOBS installer for this. + 2. Prepare the installation: + + sudo systemctl enable ssh + sudo systemctl start ssh + # Transfer your public key for passwordless SSH. All subsequent steps can be done via SSH. + sudo apt-get update + sudo apt-get upgrade + + 3. Add the following lines to `/boot/config.txt`: + - `enable_uart=1` + - `dtparam=spi=on` + - `dtoverlay=spi-bcm2835-overlay` + - `dtoverlay=mcp2515-can0,oscillator=12000000,interrupt=25` - Note: These oscillator and interrupt GPIO settings here are for the "RS485 CAN HAT" I have. There appear to be multiple versions, so they may be different from yours. Check the marking on the oscillator and the schematics. + + 4. Remove the following arguments from `/boot/cmdline.txt`: + - `console=serial0,115200` + + 5. Reboot. + + 6. Install the prerequisites: + + sudo apt-get install ipython3 python3-appdirs python3-yaml python3-usb python3-serial python3-can python3-scipy git openocd + # Optionally, to be able to compile the firmware: + sudo apt-get install gcc-arm-none-eabi + + 7. Install Teensyduino and teensy-loader-cli: + + sudo apt-get install libfontconfig libxft2 libusb-dev + + wget https://downloads.arduino.cc/arduino-1.8.12-linuxarm.tar.xz + tar -xf arduino-1.8.12-linuxarm.tar.xz + wget https://www.pjrc.com/teensy/td_151/TeensyduinoInstall.linuxarm + chmod +x TeensyduinoInstall.linuxarm + ./TeensyduinoInstall.linuxarm --dir=arduino-1.8.12 + sudo cp -R arduino-1.8.12 /usr/share/arduino + sudo ln -s /usr/share/arduino/arduino /usr/bin/arduino + + git clone https://github.com/PaulStoffregen/teensy_loader_cli + pushd teensy_loader_cli + sudo cp teensy_loader_cli /usr/bin/ + sudo ln -s /usr/bin/teensy_loader_cli /usr/bin/teensy-loader-cli + popd + + 8. Add the following lines to `/etc/udev/rules.d/49-stlinkv2`: + + SUBSYSTEMS=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374b", MODE:="0666" + SUBSYSTEMS=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3748", MODE:="0666" + + 9. `sudo ../../odrivetool udev-setup` + + 10. `sudo udevadm trigger` + + 11. Run once after every reboot: `sudo ipython3 --pdb test_runner.py -- --setup-host --test-rig-yaml ../../test-rig-rpi.yaml` + +## SSH testing flow + +Here's one possible workflow for developing on the local host and testing on a remote SSH host. + +We assume that the ODrive repo is at `/path/to/ODriveFirmware` and your testing host is configured under the SSH name `odrv`. + +To flash and start remote debugging: + + 1. Start OpenOCD remotely, along with a tunnel to localhost: `ssh -t odrv -L3333:localhost:3333 bash -c "\"openocd '-f' 'interface/stlink-v2.cfg' '-f' 'target/stm32f4x_stlink.cfg'\""` + You can keep this open for multiple debug sessions. Press Ctrl+C to quit. + 2. Compile the firmware + 3. In VSCode, select the run configuration "Debug ODrive via external server" and press Run. In contrast to the other configurations, this will flash the new firmware before dropping you into the debugger. + +To run a test: + + rsync -avh -e ssh /path/to/ODriveFirmware odrv:/opt/odrivetest --exclude="Firmware/build" --exclude="Firmware/.tup" --exclude=".git" --delete + + ssh odrv + > cd /opt/odrivetest/tools/odrive/tests/ + > ipython3 --pdb uart_ascii_test.py -- --test-rig-yaml ../../test-rig-rpi.yaml -``` -./run_tests.py --skip-boring-tests --ignore top-odrive.yellow bottom-odrive.yellow -``` diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index 56b9e663..d1697ec9 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -410,7 +410,7 @@ class TeensyComponent(Component): env['ARDUINO_COMPILE_DESTINATION'] = hexfile run_shell( ['arduino', '--board', 'teensy:avr:teensy40', '--verify', sketchfile], - logger, env = env, timeout = 60) + logger, env = env, timeout = 120) def program(self, hex_file_path: str, logger: Logger): """ From 90f5c1d0b15b4ae9088a21332fedeed01cfc2f03 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 1 May 2020 16:32:09 +0200 Subject: [PATCH 04/19] change velocity clamping in input filter mode as per: https://discordapp.com/channels/369667319280173067/369678934985408524/705227337230188595 --- Firmware/MotorControl/controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 3b450277..f4b7f84b 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -110,7 +110,8 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } void Controller::update_filter_gains() { - input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time + float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz); + input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped } @@ -175,7 +176,7 @@ bool Controller::update(float* current_setpoint_output) { float delta_vel = input_vel_ - vel_setpoint_; // Vel error float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback current_setpoint_ = accel * config_.inertia; // Accel - vel_setpoint_ += std::clamp(current_meas_period * accel, 2.0f * std::abs(delta_vel), -2.0f * std::abs(delta_vel)); // delta vel + vel_setpoint_ += current_meas_period * accel; // delta vel pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos } break; case INPUT_MODE_MIRROR: { From c6bb19145707b236b36713aedd95e2ae5638dbcc Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 1 May 2020 16:01:14 -0700 Subject: [PATCH 05/19] improvements to pos filter analysis script --- analysis/filterpoles.py | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/analysis/filterpoles.py b/analysis/filterpoles.py index abdaee2c..ca57bc8f 100644 --- a/analysis/filterpoles.py +++ b/analysis/filterpoles.py @@ -5,15 +5,20 @@ from scipy.integrate import solve_ivp import matplotlib.pyplot as plt do_mass_spring = True -do_PLL = True +do_PLL = False -bandwidth = 1 +bandwidth = 10 pos_ref = 0 vel_ref = 0 init_pos = 1000 init_vel = 0 +plotend = 1 +plotfrequency = 1000.0 + +fig, ax1 = plt.subplots() + if do_mass_spring: # 2nd order system response with manipulation of velocity only # This is similar to a mass/spring/damper system @@ -36,10 +41,20 @@ if do_mass_spring: Xdot = [pos_dot, vel_dot] return Xdot - sol = solve_ivp(get_Xdot, (0.0, 10.0), [init_pos, init_vel], t_eval=np.linspace(0, 10, 100)) + sol = solve_ivp(get_Xdot, (0.0, plotend), [init_pos, init_vel], t_eval=np.linspace(0, plotend, plotend*plotfrequency)) - plt.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='physical mass pos') - plt.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='physical mass vel') + color = 'tab:red' + ax1.set_xlabel('time (s)') + ax1.set_ylabel('pos', color=color) + ax1.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='physical mass', color=color) + ax1.tick_params(axis='y', labelcolor=color) + + ax2 = ax1.twinx() # instantiate a second axes that shares the same x-axis + + color = 'tab:blue' + ax2.set_ylabel('vel', color=color) # we already handled the x-label with ax1 + ax2.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='physical mass', color=color) + ax2.tick_params(axis='y', labelcolor=color) if do_PLL: @@ -64,7 +79,7 @@ if do_PLL: Xdot = [pos_dot, vel_dot] return Xdot - sol = solve_ivp(get_Xdot, (0.0, 10.0), [init_pos, init_vel], t_eval=np.linspace(0, 10, 100)) + sol = solve_ivp(get_Xdot, (0.0, plotend), [init_pos, init_vel], t_eval=np.linspace(0, plotend, plotend*plotfrequency)) plt.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='PLL pos') plt.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='PLL vel') @@ -72,4 +87,4 @@ if do_PLL: plt.legend() -plt.show(block=False) \ No newline at end of file +plt.show(block=True) \ No newline at end of file From d002a74d246b199e4e6670acf0bb132196dd81b2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 1 May 2020 16:37:20 -0700 Subject: [PATCH 06/19] rename psu current fault levels --- Firmware/MotorControl/axis.cpp | 6 +++--- Firmware/MotorControl/axis.hpp | 2 +- Firmware/MotorControl/odrive_main.h | 4 ++-- Firmware/communication/communication.cpp | 4 ++-- tools/odrive/enums.py | 2 +- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 53b9925d..862c027e 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -174,11 +174,11 @@ bool Axis::do_checks() { } } - if (Ibus_sum > board_config.power_supply_max_current) { + if (Ibus_sum > board_config.dc_max_positive_current) { error_ |= ERROR_DC_BUS_OVER_CURRENT; } - if (Ibus_sum < board_config.power_supply_min_current) { - error_ |= ERROR_DC_BUS_UNDER_CURRENT; + if (Ibus_sum < board_config.dc_max_negative_current) { + error_ |= ERROR_DC_BUS_OVER_REGEN_CURRENT; } // Sub-components should use set_error which will propegate to this error_ diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index c5a87a67..5ccf0c58 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -24,7 +24,7 @@ public: ERROR_MIN_ENDSTOP_PRESSED = 0x1000, ERROR_MAX_ENDSTOP_PRESSED = 0x2000, ERROR_ESTOP_REQUESTED = 0x4000, - ERROR_DC_BUS_UNDER_CURRENT = 0x8000, // too much current pushed into the power supply + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x8000, // too much current pushed into the power supply ERROR_DC_BUS_OVER_CURRENT = 0x10000, // too much current pulled out of the power supply ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000, // the min endstop was not enabled during homing }; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index a505b3e1..e11c2a00 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -101,8 +101,8 @@ struct BoardConfig_t { //!< The closer the voltage approaches dc_bus_overvoltage_trip_level, //!< the more power is sunk into the resistor. //!< This feature is disabled if dc_bus_overvoltage_trip_level <= nominal_voltage. - float power_supply_max_current = INFINITY; // Max current [A] the power supply can source - float power_supply_min_current = -INFINITY; // Max current [A] the power supply can sink + float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source + float dc_max_negative_current = -INFINITY; // Max current [A] the power supply can sink PWMMapping_t pwm_mappings[GPIO_COUNT]; PWMMapping_t analog_mappings[GPIO_COUNT]; }; diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index ea52ca87..d2aa1133 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -167,8 +167,8 @@ static inline auto make_obj_tree() { make_protocol_property("enable_ascii_protocol_on_usb", &board_config.enable_ascii_protocol_on_usb), make_protocol_property("dc_bus_undervoltage_trip_level", &board_config.dc_bus_undervoltage_trip_level), make_protocol_property("dc_bus_overvoltage_trip_level", &board_config.dc_bus_overvoltage_trip_level), - make_protocol_property("power_supply_min_current", &board_config.power_supply_min_current), - make_protocol_property("power_supply_max_current", &board_config.power_supply_max_current), + make_protocol_property("dc_max_negative_current", &board_config.dc_max_negative_current), + make_protocol_property("dc_max_positive_current", &board_config.dc_max_positive_current), #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 make_protocol_object("gpio1_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[0])), make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])), diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 1292e48b..99b21867 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -32,7 +32,7 @@ class errors: ERROR_MIN_ENDSTOP_PRESSED = 0x1000 ERROR_MAX_ENDSTOP_PRESSED = 0x2000 ERROR_ESTOP_REQUESTED = 0x4000 - ERROR_DC_BUS_UNDER_CURRENT = 0x8000 + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x8000 ERROR_DC_BUS_OVER_CURRENT = 0x10000 ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000 From edb6a1611c321418b944aa663f3d5dd52d70a291 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 1 May 2020 17:11:47 -0700 Subject: [PATCH 07/19] changed max brake duty to 95 pct --- Firmware/MotorControl/low_level.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 689bb277..b044b9a0 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -593,16 +593,15 @@ void update_brake_current() { // Don't start braking until -Ibus > regen_current_allowed float brake_current = -Ibus_sum - board_config.max_regen_current; - float brake_duty = brake_current * std::abs(board_config.brake_resistance) / vbus_voltage; + float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage; if (board_config.nominal_voltage < board_config.dc_bus_overvoltage_trip_level) { - brake_duty += std::max((vbus_voltage - board_config.nominal_voltage) / (board_config.dc_bus_overvoltage_trip_level / 0.9f - board_config.nominal_voltage), 0.0f); + brake_duty += std::max((vbus_voltage - board_config.nominal_voltage) / (board_config.dc_bus_overvoltage_trip_level / 0.9f - board_config.nominal_voltage), 0.0f); } - // Clamp the duty cycle - brake_duty = std::clamp(brake_duty, 0.0f, 0.9f); + // Duty limit at 95% to allow bootstrap caps to charge + brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); - // Duty limit at 90% to allow bootstrap caps to charge // If brake_duty is NaN, this expression will also evaluate to false int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; From 1dc420a76d6f90844cbfd4a9f0f6e580a2ffc39c Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 4 May 2020 11:32:23 +0200 Subject: [PATCH 08/19] various changes to update_brake_current: - cleaner implementaiton of the overvoltage ramp with config variables separate from other functions - explicit enable flag for overvoltage ramp (disabled by default) - reinstate brake duty cycle NaN error - add informational flag for brake resistor saturation --- Firmware/MotorControl/low_level.cpp | 29 ++++++++++++++++-------- Firmware/MotorControl/low_level.h | 1 + Firmware/MotorControl/motor.hpp | 3 ++- Firmware/MotorControl/odrive_main.h | 26 ++++++++++++++++----- Firmware/communication/communication.cpp | 5 +++- 5 files changed, 46 insertions(+), 18 deletions(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index b044b9a0..afd1a5fa 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -36,6 +36,7 @@ const float adc_ref_voltage = 3.3f; // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late float vbus_voltage = 12.0f; bool brake_resistor_armed = false; +bool brake_resistor_saturated = false; /* Private constant data -----------------------------------------------------*/ static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC }; static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]); @@ -595,19 +596,27 @@ void update_brake_current() { float brake_current = -Ibus_sum - board_config.max_regen_current; float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage; - if (board_config.nominal_voltage < board_config.dc_bus_overvoltage_trip_level) { - brake_duty += std::max((vbus_voltage - board_config.nominal_voltage) / (board_config.dc_bus_overvoltage_trip_level / 0.9f - board_config.nominal_voltage), 0.0f); + if (board_config.enable_dc_bus_overvoltage_ramp && (board_config.dc_bus_overvoltage_ramp_start < board_config.dc_bus_overvoltage_ramp_end)) { + brake_duty += std::fmax((vbus_voltage - board_config.dc_bus_overvoltage_ramp_start) / (board_config.dc_bus_overvoltage_ramp_end - board_config.dc_bus_overvoltage_ramp_start), 0.0f); } - // Duty limit at 95% to allow bootstrap caps to charge - brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); - - // If brake_duty is NaN, this expression will also evaluate to false - int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - safety_critical_apply_brake_resistor_timings(low_off, high_on); + if (!std::isnan(brake_duty)) { + if (brake_duty >= 0.95f) { + brake_resistor_saturated = true; + } + // Duty limit at 95% to allow bootstrap caps to charge + brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); + + // If brake_duty is NaN, this expression will also evaluate to false + int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + safety_critical_apply_brake_resistor_timings(low_off, high_on); + } else { + // Shuts off all motors AND brake resistor, sets error code on all motors. + low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN); + } } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 503b98e1..43a3ac9b 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -23,6 +23,7 @@ extern const float adc_ref_voltage; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; extern bool brake_resistor_armed; +extern bool brake_resistor_saturated; extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 5d493437..b0f1cef1 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -23,7 +23,8 @@ public: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200, ERROR_CURRENT_SENSE_SATURATION = 0x0400, ERROR_INVERTER_OVER_TEMP = 0x0800, - ERROR_CURRENT_LIMIT_VIOLATION = 0x1000 + ERROR_CURRENT_LIMIT_VIOLATION = 0x1000, + ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000, }; enum MotorType_t { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index e11c2a00..f659a431 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -95,12 +95,26 @@ struct BoardConfig_t { // brake_duty_cycle += 0% + * vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100% + */ + bool enable_dc_bus_overvoltage_ramp = false; + float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`. + //!< Do not set this lower than your usual vbus_voltage, + //!< unless you like fried brake resistors. + float dc_bus_overvoltage_ramp_end = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`. + //!< Must be larger than `dc_bus_overvoltage_ramp_start`, + //!< otherwise the ramp feature is disabled. + float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source float dc_max_negative_current = -INFINITY; // Max current [A] the power supply can sink PWMMapping_t pwm_mappings[GPIO_COUNT]; diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index d2aa1133..582a4617 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -126,6 +126,7 @@ static inline auto make_obj_tree() { make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased), make_protocol_ro_property("user_config_loaded", const_cast(&user_config_loaded_)), make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed), + make_protocol_property("brake_resistor_saturated", &brake_resistor_saturated), make_protocol_object("system_stats", make_protocol_ro_property("uptime", &system_stats_.uptime), make_protocol_ro_property("min_heap_space", &system_stats_.min_heap_space), @@ -159,7 +160,6 @@ static inline auto make_obj_tree() { ), make_protocol_object("config", make_protocol_property("brake_resistance", &board_config.brake_resistance), - make_protocol_property("nominal_voltage", &board_config.nominal_voltage), make_protocol_property("max_regen_current", &board_config.max_regen_current), // TODO: changing this currently requires a reboot - fix this make_protocol_property("enable_uart", &board_config.enable_uart), @@ -167,6 +167,9 @@ static inline auto make_obj_tree() { make_protocol_property("enable_ascii_protocol_on_usb", &board_config.enable_ascii_protocol_on_usb), make_protocol_property("dc_bus_undervoltage_trip_level", &board_config.dc_bus_undervoltage_trip_level), make_protocol_property("dc_bus_overvoltage_trip_level", &board_config.dc_bus_overvoltage_trip_level), + make_protocol_property("enable_dc_bus_overvoltage_ramp", &board_config.enable_dc_bus_overvoltage_ramp), + make_protocol_property("dc_bus_overvoltage_ramp_start", &board_config.dc_bus_overvoltage_ramp_start), + make_protocol_property("dc_bus_overvoltage_ramp_end", &board_config.dc_bus_overvoltage_ramp_end), make_protocol_property("dc_max_negative_current", &board_config.dc_max_negative_current), make_protocol_property("dc_max_positive_current", &board_config.dc_max_positive_current), #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 From 23f513e99e43e20068179d37c811b86925747e3a Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 4 May 2020 16:26:52 +0200 Subject: [PATCH 09/19] update changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 47e41b83..8d4d0077 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,7 +24,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Added support for Flylint VSCode Extension for static code analysis * Using an STM32F405 .svd file allows CortexDebug to view registers during debugging * Added scripts for building via docker. -* Brake resistor logic now attempts to clamp voltage according to `odrv.config.nominal_voltage` +* Brake resistor logic now attempts to clamp voltage according to `odrv.config.dc_bus_overvoltage_ramp_start` and `odrv.config.dc_bus_overvoltage_ramp_end` ### Changed * Changed ratiometric `motor.config.current_lim_tolerance` to absolute `motor.config.current_lim_margin` From 50ecdef8608bb334ba7f2d2232449e29c7ab2b8e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 5 May 2020 18:16:06 +0200 Subject: [PATCH 10/19] move DC overcurrent/undercurrent checks to update_brake_current() The checks now take into account the brake current. Expose ibus on fibre. --- Firmware/MotorControl/axis.cpp | 15 -------- Firmware/MotorControl/axis.hpp | 2 -- Firmware/MotorControl/low_level.cpp | 44 ++++++++++++++++-------- Firmware/MotorControl/low_level.h | 1 + Firmware/MotorControl/motor.hpp | 2 ++ Firmware/MotorControl/odrive_main.h | 9 +++-- Firmware/communication/communication.cpp | 1 + tools/odrive/enums.py | 5 +-- 8 files changed, 43 insertions(+), 36 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 862c027e..b68ef01b 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -166,21 +166,6 @@ bool Axis::do_checks() { if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level)) error_ |= ERROR_DC_BUS_OVER_VOLTAGE; - // This is the same math that's used in update_brake_current(). Should we calculate IBus globally? - float Ibus_sum = 0.0f; - for (size_t i = 0; i < AXIS_COUNT; ++i) { - if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) { - Ibus_sum += axes[i]->motor_.current_control_.Ibus; - } - } - - if (Ibus_sum > board_config.dc_max_positive_current) { - error_ |= ERROR_DC_BUS_OVER_CURRENT; - } - if (Ibus_sum < board_config.dc_max_negative_current) { - error_ |= ERROR_DC_BUS_OVER_REGEN_CURRENT; - } - // Sub-components should use set_error which will propegate to this error_ motor_.do_checks(); // encoder_.do_checks(); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 5ccf0c58..5b489a93 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -24,8 +24,6 @@ public: ERROR_MIN_ENDSTOP_PRESSED = 0x1000, ERROR_MAX_ENDSTOP_PRESSED = 0x2000, ERROR_ESTOP_REQUESTED = 0x4000, - ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x8000, // too much current pushed into the power supply - ERROR_DC_BUS_OVER_CURRENT = 0x10000, // too much current pulled out of the power supply ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000, // the min endstop was not enabled during homing }; diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index afd1a5fa..454fa339 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -35,6 +35,7 @@ const float adc_ref_voltage = 3.3f; // This value is updated by the DC-bus reading ADC. // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late float vbus_voltage = 12.0f; +float ibus_ = 0.0f; // exposed for monitoring only bool brake_resistor_armed = false; bool brake_resistor_saturated = false; /* Private constant data -----------------------------------------------------*/ @@ -600,23 +601,38 @@ void update_brake_current() { brake_duty += std::fmax((vbus_voltage - board_config.dc_bus_overvoltage_ramp_start) / (board_config.dc_bus_overvoltage_ramp_end - board_config.dc_bus_overvoltage_ramp_start), 0.0f); } - if (!std::isnan(brake_duty)) { - if (brake_duty >= 0.95f) { - brake_resistor_saturated = true; - } - - // Duty limit at 95% to allow bootstrap caps to charge - brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); - - // If brake_duty is NaN, this expression will also evaluate to false - int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - safety_critical_apply_brake_resistor_timings(low_off, high_on); - } else { + if (std::isnan(brake_duty)) { // Shuts off all motors AND brake resistor, sets error code on all motors. low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN); + return; } + + if (brake_duty >= 0.95f) { + brake_resistor_saturated = true; + } + + // Duty limit at 95% to allow bootstrap caps to charge + brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); + + // Special handling to avoid the case 0.0/0.0 == NaN. + Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / board_config.brake_resistance) : 0.0f; + + ibus_ = Ibus_sum; + + if (Ibus_sum > board_config.dc_max_positive_current) { + low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT); + return; + } + if (Ibus_sum < board_config.dc_max_negative_current) { + low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT); + return; + } + + // If brake_duty is NaN, this expression will also evaluate to false + int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + safety_critical_apply_brake_resistor_timings(low_off, high_on); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 43a3ac9b..f0b72ecb 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -22,6 +22,7 @@ extern const float adc_full_scale; extern const float adc_ref_voltage; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; +extern float ibus_; extern bool brake_resistor_armed; extern bool brake_resistor_saturated; extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index b0f1cef1..aba285cc 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -25,6 +25,8 @@ public: ERROR_INVERTER_OVER_TEMP = 0x0800, ERROR_CURRENT_LIMIT_VIOLATION = 0x1000, ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000, + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x4000, // too much current pushed into the power supply + ERROR_DC_BUS_OVER_CURRENT = 0x8000, // too much current pulled out of the power supply }; enum MotorType_t { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index f659a431..ade4416c 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -101,11 +101,14 @@ struct BoardConfig_t { * the ODrive will sink more power than usual into the the brake resistor * in an attempt to bring the voltage down again. * - * This setting is active even if all motors are disarmed. - * * The brake duty cycle is increased by the following amount: * vbus_voltage == dc_bus_overvoltage_ramp_start => brake_duty_cycle += 0% * vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100% + * + * Remarks: + * - This setting is active even if all motors are disarmed. + * - brake_resistance must be non-zero, otherwise this will immediately + * result in overcurrent */ bool enable_dc_bus_overvoltage_ramp = false; float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`. @@ -116,7 +119,7 @@ struct BoardConfig_t { //!< otherwise the ramp feature is disabled. float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source - float dc_max_negative_current = -INFINITY; // Max current [A] the power supply can sink + float dc_max_negative_current = -0.000001f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable. PWMMapping_t pwm_mappings[GPIO_COUNT]; PWMMapping_t analog_mappings[GPIO_COUNT]; }; diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 582a4617..fb033bdd 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -116,6 +116,7 @@ public: static inline auto make_obj_tree() { return make_protocol_member_list( make_protocol_ro_property("vbus_voltage", &vbus_voltage), + make_protocol_ro_property("ibus", &ibus_), make_protocol_ro_property("serial_number", &serial_number), make_protocol_ro_property("hw_version_major", &hw_version_major), make_protocol_ro_property("hw_version_minor", &hw_version_minor), diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 99b21867..1198272e 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -32,8 +32,6 @@ class errors: ERROR_MIN_ENDSTOP_PRESSED = 0x1000 ERROR_MAX_ENDSTOP_PRESSED = 0x2000 ERROR_ESTOP_REQUESTED = 0x4000 - ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x8000 - ERROR_DC_BUS_OVER_CURRENT = 0x10000 ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000 class motor: @@ -50,6 +48,9 @@ class errors: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200 ERROR_CURRENT_SENSE_SATURATION = 0x0400 ERROR_CURRENT_LIMIT_VIOLATION = 0x1000 + ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000 + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x4000 + ERROR_DC_BUS_OVER_CURRENT = 0x8000 class encoder: ERROR_NONE = 0 From afe83430fee9f5f3ca1a1d220660080818bfcbb0 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 5 May 2020 18:16:45 +0200 Subject: [PATCH 11/19] add test for over-regen-current check --- tools/odrive/tests/closed_loop_test.py | 226 ++++++++++++++++--------- 1 file changed, 145 insertions(+), 81 deletions(-) diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index 2b9cff42..dd26b6a5 100644 --- a/tools/odrive/tests/closed_loop_test.py +++ b/tools/odrive/tests/closed_loop_test.py @@ -10,8 +10,9 @@ from test_runner import * from odrive.enums import * -class TestClosedLoopControl(): +class TestClosedLoopControlBase(): """ + Base class for close loop control tests. """ def get_test_cases(self, testrig: TestRig): @@ -27,10 +28,7 @@ class TestClosedLoopControl(): if encoder.impl in testrig.get_connected_components(motor): yield (odrive.axes[num], motor, encoder) - def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): - axis = axis_ctx.handle - time.sleep(1.0) # wait for PLLs to stabilize - + def prepare(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): # Make sure there are no funny configurations active logger.debug('Setting up clean configuration...') axis_ctx.parent.erase_config_and_reboot() @@ -58,96 +56,162 @@ class TestClosedLoopControl(): test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_no_error(axis_ctx) - nominal_rps = 1.0 - nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps - logger.debug(f'Testing closed loop velocity control at {nominal_rps} rounds/s...') - axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL - axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH - axis_ctx.handle.controller.input_vel = 0 - - request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) - axis_ctx.handle.controller.input_vel = nominal_vel - - data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=5.0) - - test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) - test_assert_no_error(axis_ctx) - request_state(axis_ctx, AXIS_STATE_IDLE) - - # encoder.vel_estimate - slope, offset, fitted_curve = fit_line(data[:,(0,1)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.02) - test_assert_eq(offset, nominal_vel, accuracy = 0.05) - test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.3, inlier_range = nominal_vel * 0.5, max_outliers = len(data[:,0]) * 0.1) - - # encoder.pos_estimate - slope, offset, fitted_curve = fit_line(data[:,(0,2)]) - test_assert_eq(slope, nominal_vel, accuracy = 0.01) - test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + # Return a context that can be used in a with-statement. + class safe_terminator(): + def __enter__(self): + pass + def __exit__(self, exc_type, exc_val, exc_tb): + logger.debug('clearing config...') + axis_ctx.parent.erase_config_and_reboot() + return safe_terminator() - logger.debug(f'Testing closed loop position control...') +class TestClosedLoopControl(TestClosedLoopControlBase): + """ + Tests position and velocity control + """ + + def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): + with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger): + nominal_rps = 1.0 + nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps + logger.debug(f'Testing closed loop velocity control at {nominal_rps} rounds/s...') + + axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL + axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH + axis_ctx.handle.controller.input_vel = 0 + + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + axis_ctx.handle.controller.input_vel = nominal_vel + + data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=5.0) + + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + test_assert_no_error(axis_ctx) + request_state(axis_ctx, AXIS_STATE_IDLE) + + # encoder.vel_estimate + slope, offset, fitted_curve = fit_line(data[:,(0,1)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.02) + test_assert_eq(offset, nominal_vel, accuracy = 0.05) + test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.3, inlier_range = nominal_vel * 0.5, max_outliers = len(data[:,0]) * 0.1) + + # encoder.pos_estimate + slope, offset, fitted_curve = fit_line(data[:,(0,2)]) + test_assert_eq(slope, nominal_vel, accuracy = 0.01) + test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + + logger.debug(f'Testing closed loop position control...') + + axis_ctx.handle.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL + axis_ctx.handle.controller.input_pos = 0 + axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 5.0 # max 5 rps + axis_ctx.handle.encoder.set_linear_count(0) + + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Test small position changes + axis_ctx.handle.controller.input_pos = 5000 + time.sleep(0.3) + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, 5000, range=2000) # large range needed because of cogging torque + axis_ctx.handle.controller.input_pos = -5000 + time.sleep(0.3) + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, -5000, range=2000) + + axis_ctx.handle.controller.input_pos = 0 + time.sleep(0.3) + + nominal_vel = float(enc_ctx.yaml['cpr']) * 5.0 + axis_ctx.handle.controller.input_pos = nominal_vel * 2.0 # 10 turns (takes 2 seconds) + + # Test large position change with bounded velocity + data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=4.0) + + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + test_assert_no_error(axis_ctx) + request_state(axis_ctx, AXIS_STATE_IDLE) + + data_motion = data[data[:,0] < 1.9] + data_still = data[data[:,0] > 2.1] + + # encoder.vel_estimate + slope, offset, fitted_curve = fit_line(data_motion[:,(0,1)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) + test_assert_eq(offset, nominal_vel, accuracy = 0.05) + test_curve_fit(data_motion[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + # encoder.pos_estimate + slope, offset, fitted_curve = fit_line(data_motion[:,(0,2)]) + test_assert_eq(slope, nominal_vel, accuracy = 0.01) + test_curve_fit(data_motion[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + # encoder.vel_estimate + slope, offset, fitted_curve = fit_line(data_still[:,(0,1)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) + test_assert_eq(offset, 0.0, range = nominal_vel * 0.05) + test_curve_fit(data_still[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + # encoder.pos_estimate + slope, offset, fitted_curve = fit_line(data_still[:,(0,2)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) + test_assert_eq(offset, nominal_vel*2, range = nominal_vel * 0.02) + test_curve_fit(data_still[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.01, max_outliers = len(data[:,0]) * 0.01) + + +class TestRegenProtection(TestClosedLoopControlBase): + """ + Tries to brake with a disabled brake resistor. + This should result in a low level error disabling all power outputs. + """ + + def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): + with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger): + nominal_rps = 6.0 + nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps - axis_ctx.handle.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL - axis_ctx.handle.controller.input_pos = 0 - axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 5.0 # max 5 rps - axis_ctx.handle.encoder.set_linear_count(0) + # Accept a bit of noise on Ibus + axis_ctx.parent.handle.config.dc_max_negative_current = -0.2 - request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + logger.debug(f'Brake control test from {nominal_rps} rounds/s...') + + axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 10.0 # max 10 rps + axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL + axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH - # Test small position changes - axis_ctx.handle.controller.input_pos = 5000 - time.sleep(0.3) - test_assert_no_error(axis_ctx) - test_assert_eq(axis_ctx.handle.encoder.pos_estimate, 5000, range=2000) # large range needed because of cogging torque - axis_ctx.handle.controller.input_pos = -5000 - time.sleep(0.3) - test_assert_no_error(axis_ctx) - test_assert_eq(axis_ctx.handle.encoder.pos_estimate, -5000, range=2000) - - axis_ctx.handle.controller.input_pos = 0 - time.sleep(0.3) + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) - nominal_vel = float(enc_ctx.yaml['cpr']) * 5.0 - axis_ctx.handle.controller.input_pos = nominal_vel * 2.0 # 10 turns (takes 2 seconds) - - # Test large position change with bounded velocity - data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=4.0) - - test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) - test_assert_no_error(axis_ctx) - request_state(axis_ctx, AXIS_STATE_IDLE) + # accelerate... + axis_ctx.handle.controller.input_vel = nominal_vel + time.sleep(1.0) + test_assert_no_error(axis_ctx) - data_motion = data[data[:,0] < 1.9] - data_still = data[data[:,0] > 2.1] + # ... and brake + axis_ctx.handle.controller.input_vel = 0 + time.sleep(1.0) + test_assert_no_error(axis_ctx) - # encoder.vel_estimate - slope, offset, fitted_curve = fit_line(data_motion[:,(0,1)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) - test_assert_eq(offset, nominal_vel, accuracy = 0.05) - test_curve_fit(data_motion[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + # once more, but this time without brake resistor + axis_ctx.parent.handle.config.brake_resistance = 0 - # encoder.pos_estimate - slope, offset, fitted_curve = fit_line(data_motion[:,(0,2)]) - test_assert_eq(slope, nominal_vel, accuracy = 0.01) - test_curve_fit(data_motion[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + # accelerate... + axis_ctx.handle.controller.input_vel = nominal_vel + time.sleep(1.0) + test_assert_no_error(axis_ctx) - # encoder.vel_estimate - slope, offset, fitted_curve = fit_line(data_still[:,(0,1)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) - test_assert_eq(offset, 0.0, range = nominal_vel * 0.05) - test_curve_fit(data_still[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) - - # encoder.pos_estimate - slope, offset, fitted_curve = fit_line(data_still[:,(0,2)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) - test_assert_eq(offset, nominal_vel*2, range = nominal_vel * 0.02) - test_curve_fit(data_still[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.01, max_outliers = len(data[:,0]) * 0.01) + # ... and brake + axis_ctx.handle.controller.input_vel = 0 # this should fail almost instantaneously + time.sleep(0.1) + test_assert_eq(axis_ctx.handle.error, errors.axis.ERROR_MOTOR_DISARMED | errors.axis.ERROR_BRAKE_RESISTOR_DISARMED) + test_assert_eq(axis_ctx.handle.motor.error, errors.motor.ERROR_DC_BUS_OVER_REGEN_CURRENT) if __name__ == '__main__': test_runner.run([ - TestClosedLoopControl() + TestClosedLoopControl(), + TestRegenProtection(), ]) From 7290e55525fb8e241c0f9dbcdcfa31ca5af832f5 Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 5 May 2020 20:30:03 -0400 Subject: [PATCH 12/19] Convert Axes to a std::array --- Firmware/MotorControl/main.cpp | 2 +- Firmware/MotorControl/odrive_main.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 71b6425e..a596c8b2 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -23,7 +23,7 @@ bool user_config_loaded_; SystemStats_t system_stats_; -Axis *axes[AXIS_COUNT]; +std::array axes; ODriveCAN *odCAN = nullptr; typedef Config< diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index ade4416c..121ff024 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -132,7 +132,7 @@ class Motor; class ODriveCAN; constexpr size_t AXIS_COUNT = 2; -extern Axis *axes[AXIS_COUNT]; +extern std::array axes; extern ODriveCAN *odCAN; // if you use the oscilloscope feature you can bump up this value From bf6d47f9505dcdd73dc03b776356e50cd7e5f0b0 Mon Sep 17 00:00:00 2001 From: Unknown Date: Tue, 5 May 2020 21:37:47 -0400 Subject: [PATCH 13/19] Add some missing reference & symbols --- Firmware/MotorControl/encoder.cpp | 2 +- Firmware/MotorControl/encoder.hpp | 2 +- Firmware/MotorControl/main.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index dcb1bbf9..72986105 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -3,7 +3,7 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config, Motor::Config_t motor_config) : + Config_t& config, const Motor::Config_t& motor_config) : hw_config_(hw_config), config_(config) { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 872bf810..28830c38 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -56,7 +56,7 @@ public: }; Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config, Motor::Config_t motor_config); + Config_t& config, const Motor::Config_t& motor_config); void setup(); void set_error(Error_t error); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index a596c8b2..f49d02d6 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -242,7 +242,7 @@ int odrive_main(void) { axes[i]->setup(); } - for(auto axis : axes){ + for(auto& axis : axes){ axis->encoder_.setup(); } From eb944018e338cb8a8d7b06513ef561f2fb8a3f06 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 5 May 2020 18:27:56 +0200 Subject: [PATCH 14/19] update documentation --- CHANGELOG.md | 2 +- docs/testing.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8d4d0077..e0543398 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,7 +19,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Gain scheduling for anti-hunt when close to 0 position error * Velocity Limiting in Current Control mode according to `vel_limit` and `vel_gain` * Regen current limiting according to `max_regen_limit`, in Amps -* DC Bus hard current limiting according to `power_supply_min_current` and `power_supply_max_current` +* DC Bus hard current limiting according to `dc_max_negative_current` and `dc_max_positive_current` * Unit Testing with Doctest has been started for select algorithms, see [Firmware/Tests/test_runner.cpp](Firmware/Tests/test_runner.cpp) * Added support for Flylint VSCode Extension for static code analysis * Using an STM32F405 .svd file allows CortexDebug to view registers during debugging diff --git a/docs/testing.md b/docs/testing.md index 46ba9b69..771d4e44 100644 --- a/docs/testing.md +++ b/docs/testing.md @@ -15,7 +15,7 @@ The testing facility consists of the following components: - `analog_input_test.py`: Analog Input - `calibration_test.py`: Motor calibration, encoder offset calibration, encoder direction find, encoder index search - `can_test.py`: Partial coverage of the commands described in [CAN Protocol](can-protocol) - - `closed_loop_test.py`: Velocity control, position control (TODO: sensorless control) + - `closed_loop_test.py`: Velocity control, position control (TODO: sensorless control), brake regen current protection - `encoder_test.py`: Incremental encoder, hall effect encoder, sin/cos encoder, SPI encoders (AMS, CUI) - `nvm_test.py`: Configuration storage - `pwm_input_test.py`: PWM input From 35d1a24314c5434771704716e93a4e514392e285 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 5 May 2020 19:38:40 +0200 Subject: [PATCH 15/19] improve encoder tests --- tools/odrive/tests/encoder_test.py | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/tools/odrive/tests/encoder_test.py b/tools/odrive/tests/encoder_test.py index b9af8ed6..59eac451 100644 --- a/tools/odrive/tests/encoder_test.py +++ b/tools/odrive/tests/encoder_test.py @@ -43,22 +43,22 @@ class TestEncoderBase(): # encoder.shadow_count slope, offset, fitted_curve = fit_line(data[:,(0,1)]) test_assert_eq(slope, true_cps, accuracy=0.005) - test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.count_in_cpr slope, offset, fitted_curve = fit_sawtooth(data[:,(0,2)], true_cpr if reverse else 0, 0 if reverse else true_cpr) test_assert_eq(slope, true_cps, accuracy=0.005) - test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.phase slope, offset, fitted_curve = fit_sawtooth(data[:,(0,3)], pi if reverse else -pi, -pi if reverse else pi, sigma=5) test_assert_eq(slope / 7, 2*pi*true_rps, accuracy=0.05) - test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.pos_estimate slope, offset, fitted_curve = fit_line(data[:,(0,4)]) test_assert_eq(slope, true_cps, accuracy=0.005) - test_curve_fit(data[:,(0,4)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,4)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.pos_cpr slope, offset, fitted_curve = fit_sawtooth(data[:,(0,5)], true_cpr if reverse else 0, 0 if reverse else true_cpr) @@ -120,7 +120,7 @@ class TestIncrementalEncoder(TestEncoderBase): yield (encoder, valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, logger: Logger): true_cps = 8192*-0.5 # counts per second generated by the virtual encoder code = teensy_incremental_encoder_emulation_code.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num)) @@ -180,7 +180,7 @@ class TestSinCosEncoder(TestEncoderBase): yield (odrive.encoders[0], valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger): code = teensy_sin_cos_encoder_emulation_code.replace("{enc_sin}", str(teensy_gpio_sin.num)).replace("{enc_cos}", str(teensy_gpio_cos.num)) teensy.compile_and_program(code) @@ -247,7 +247,7 @@ class TestHallEffectEncoder(TestEncoderBase): yield (encoder, valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, teensy_gpio_c: TeensyGpio, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, teensy_gpio_c: TeensyGpio, logger: Logger): true_cpr = 90 true_rps = -1.0 @@ -432,7 +432,7 @@ class TestSpiEncoder(TestEncoderBase): yield (encoder, 7, valid_combinations) - def run_test(self, enc: EncoderComponent, odrive_ncs_gpio: int, teensy: TeensyComponent, teensy_gpio_sck: TeensyGpio, teensy_gpio_miso: TeensyGpio, teensy_gpio_mosi: TeensyGpio, teensy_gpio_ncs: TeensyGpio, teensy_gpio_reset: TeensyGpio, reset_gpio: LinuxGpioComponent, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, odrive_ncs_gpio: int, teensy: TeensyComponent, teensy_gpio_sck: TeensyGpio, teensy_gpio_miso: TeensyGpio, teensy_gpio_mosi: TeensyGpio, teensy_gpio_ncs: TeensyGpio, teensy_gpio_reset: TeensyGpio, reset_gpio: LinuxGpioComponent, logger: Logger): true_cpr = 16384 true_rps = 1.0 @@ -452,6 +452,11 @@ class TestSpiEncoder(TestEncoderBase): enc.handle.config.mode = self.mode enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio enc.handle.config.cpr = true_cpr + # Also put the other encoder into SPI mode to make it more interesting + other_enc = enc.parent.encoders[1 - enc.num] + other_enc.handle.config.mode = self.mode + other_enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio + other_enc.handle.config.cpr = true_cpr enc.parent.save_config_and_reboot() time.sleep(1.0) @@ -499,8 +504,8 @@ class TestSpiEncoder(TestEncoderBase): if __name__ == '__main__': test_runner.run([ - #TestIncrementalEncoder(), - #TestSinCosEncoder(), + TestIncrementalEncoder(), + TestSinCosEncoder(), TestHallEffectEncoder(), TestSpiEncoder(ENCODER_MODE_SPI_ABS_AMS), TestSpiEncoder(ENCODER_MODE_SPI_ABS_CUI), From 8ab2e0530e56bed9db8494e35b9764703d49ac34 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 5 May 2020 20:01:39 +0200 Subject: [PATCH 16/19] add helper to pass through encoder --- tools/odrive/tests/not_a_test.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 tools/odrive/tests/not_a_test.py diff --git a/tools/odrive/tests/not_a_test.py b/tools/odrive/tests/not_a_test.py new file mode 100644 index 00000000..7be3e995 --- /dev/null +++ b/tools/odrive/tests/not_a_test.py @@ -0,0 +1,30 @@ + +import test_runner + +from fibre.utils import Logger +from test_runner import * + +class EncoderPassthrough(): + """ + Does nothing except passing encoder0 through. + """ + + def get_test_cases(self, testrig: TestRig): + for odrive in testrig.get_components(ODriveComponent): + for num in range(1): + encoders = testrig.get_connected_components({ + 'a': (odrive.encoders[num].a, False), + 'b': (odrive.encoders[num].b, False), + 'z': (odrive.encoders[num].z, False) + }, EncoderComponent) + motors = testrig.get_connected_components(odrive.axes[num], MotorComponent) + + for motor, encoder in itertools.product(motors, encoders): + if encoder.impl in testrig.get_connected_components(motor): + yield (odrive.axes[num], motor, encoder) + + def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): + logger.debug(f'Encoder {axis_ctx.num} was passed through') + +if __name__ == '__main__': + test_runner.run(EncoderPassthrough()) From 484c5ab9dad2ff5429d7233f10e907395efb5813 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 6 May 2020 11:23:46 +0200 Subject: [PATCH 17/19] clarify comments --- Firmware/MotorControl/low_level.cpp | 1 - Firmware/MotorControl/odrive_main.h | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 454fa339..8236d39e 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -628,7 +628,6 @@ void update_brake_current() { return; } - // If brake_duty is NaN, this expression will also evaluate to false int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; if (low_off < 0) low_off = 0; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 121ff024..2dbb82a6 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -106,9 +106,9 @@ struct BoardConfig_t { * vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100% * * Remarks: - * - This setting is active even if all motors are disarmed. - * - brake_resistance must be non-zero, otherwise this will immediately - * result in overcurrent + * - This setting is active even when all motors are disarmed. + * - brake_resistance must be non-zero, otherwise this will result in an + * overcurrent fault as soon as vbus_voltage exceeds dc_bus_overvoltage_ramp_start. */ bool enable_dc_bus_overvoltage_ramp = false; float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`. From 9155edfb1795657ea39741c1962ebc754c15548e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 6 May 2020 19:19:26 +0200 Subject: [PATCH 18/19] Fix a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint. If certain inputs were passed to trajectory planner, an expression inside the trajectory planner which is an argument to sqrtf() could become negative due to finite floating point accuracy. This led to Vr_ == NaN and then Tf_ == 0, causing the trajectory to jump to the final setpoint instantaneously. To the user, this manifested as a sudden increase in velocity (limited by controller.config.vel_limit) and/or an overcurrent fault. This bug was likely to show up when constantly sending trajectory setpoints while moving in the negative direction. See also: https://discourse.odriverobotics.com/t/move-to-pos-not-works-well/4626 --- CHANGELOG.md | 1 + Firmware/MotorControl/trapTraj.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e0543398..f671d0f7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -38,6 +38,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Fix IPython `RuntimeWarning` that would occur every time `odrivetool` was started. * Reboot on `erase_configuration()`. This avoids unexpected behavior of a subsequent `save_configuration()` call, since the configuration is only erased from NVM, not from RAM. * Change `motor.get_inverter_temp()` to use a property which was already being sampled at `motor.inverter_temp` +* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint # Releases ## [0.4.11] - 2019-07-25 diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 2067c35e..dd7f64a2 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -44,7 +44,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, // Are we displacing enough to reach cruising speed? if (s*dX < s*dXmin) { // Short move (triangle profile) - Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_)); + Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f)); Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_); Td_ = std::max(0.0f, -Vr_ / Dr_); Tv_ = 0.0f; From 33e00f767db58786f3f0b88794c5e4997f9b8a06 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 6 May 2020 19:23:31 +0200 Subject: [PATCH 19/19] add test for trapezoidal trajectory --- Firmware/Tests/test_trap_traj.cpp | 235 ++++++++++++++++++++++++++++++ 1 file changed, 235 insertions(+) create mode 100644 Firmware/Tests/test_trap_traj.cpp diff --git a/Firmware/Tests/test_trap_traj.cpp b/Firmware/Tests/test_trap_traj.cpp new file mode 100644 index 00000000..b5d30014 --- /dev/null +++ b/Firmware/Tests/test_trap_traj.cpp @@ -0,0 +1,235 @@ + +#include +#include +#include +#include +#include + +#include "MotorControl/utils.hpp" + +// TODO: This is currently a copy-paste of the real code due to non-trivial +// include dependencies. Should include real code. + +class TrapezoidalTrajectory { +public: + struct Step_t { + float Y; + float Yd; + float Ydd; + }; + + explicit TrapezoidalTrajectory(); + bool planTrapezoidal(float Xf, float Xi, float Vi, + float Vmax, float Amax, float Dmax); + Step_t eval(float t); + + float Xi_; + float Xf_; + float Vi_; + + float Ar_; + float Vr_; + float Dr_; + + float Ta_; + float Tv_; + float Td_; + float Tf_; + + float yAccel_; + + float t_; +}; + + + +// A sign function where input 0 has positive sign (not 0) +float sign_hard(float val) { + return (std::signbit(val)) ? -1.0f : 1.0f; +} + +// Symbol Description +// Ta, Tv and Td Duration of the stages of the AL profile +// Xi and Vi Adapted initial conditions for the AL profile +// Xf Position set-point +// s Direction (sign) of the trajectory +// Vmax, Amax, Dmax and jmax Kinematic bounds +// Ar, Dr and Vr Reached values of acceleration and velocity + +TrapezoidalTrajectory::TrapezoidalTrajectory() {} + +bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, + float Vmax, float Amax, float Dmax) { + float dX = Xf - Xi; // Distance to travel + float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance + float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement + float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any) + Ar_ = s * Amax; // Maximum Acceleration (signed) + Dr_ = -s * Dmax; // Maximum Deceleration (signed) + Vr_ = s * Vmax; // Maximum Velocity (signed) + + // If we start with a speed faster than cruising, then we need to decel instead of accel + // aka "double deceleration move" in the paper + if ((s * Vi) > (s * Vr_)) { + Ar_ = -s * Amax; + } + + // Time to accel/decel to/from Vr (cruise speed) + Ta_ = (Vr_ - Vi) / Ar_; + Td_ = -Vr_ / Dr_; + + // Integral of velocity ramps over the full accel and decel times to get + // minimum displacement required to reach cuising speed + float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_; + + // Are we displacing enough to reach cruising speed? + if (s*dX < s*dXmin) { + // Short move (triangle profile) + Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f)); + //Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_)); + Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_); + Td_ = std::max(0.0f, -Vr_ / Dr_); + Tv_ = 0.0f; + } else { + // Long move (trapezoidal profile) + Tv_ = (dX - dXmin) / Vr_; + } + + // Fill in the rest of the values used at evaluation-time + Tf_ = Ta_ + Tv_ + Td_; + Xi_ = Xi; + Xf_ = Xf; + Vi_ = Vi; + yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase + + return true; +} + +TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) { + Step_t trajStep; + if (t < 0.0f) { // Initial Condition + trajStep.Y = Xi_; + trajStep.Yd = Vi_; + trajStep.Ydd = 0.0f; + } else if (t < Ta_) { // Accelerating + trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t); + trajStep.Yd = Vi_ + Ar_*t; + trajStep.Ydd = Ar_; + } else if (t < Ta_ + Tv_) { // Coasting + trajStep.Y = yAccel_ + Vr_*(t - Ta_); + trajStep.Yd = Vr_; + trajStep.Ydd = 0.0f; + } else if (t < Tf_) { // Deceleration + float td = t - Tf_; + trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td); + trajStep.Yd = Dr_*td; + trajStep.Ydd = Dr_; + } else if (t >= Tf_) { // Final Condition + trajStep.Y = Xf_; + trajStep.Yd = 0.0f; + trajStep.Ydd = 0.0f; + } else { + // TODO: report error here + } + + return trajStep; +} + +static_assert(sizeof(float) * CHAR_BIT == 32); + + +void run_trajectory_test(float goal, float position, float velocity, float Vmax, float Amax, float Dmax) { + float dt = 0.000125f; + int replan_interval = 10; // must be > 2 (see note below) + float t = 0.0f; + float Vmax_test = std::max(Vmax, std::abs(velocity)); + + TrapezoidalTrajectory traj{}; + + int replan_counter = 0; + + do { + if (replan_counter <= 0) { + CHECK(traj.planTrapezoidal(goal, position, velocity, Vmax, Amax, Dmax)); + t = 0.0f; + replan_counter = replan_interval; + } else { + replan_counter--; + } + + TrapezoidalTrajectory::Step_t step = traj.eval(t); + t += dt; + + //std::cerr << "vel: " << step.Yd << ", pos: " << step.Y << "\n"; + + // Check if acceleration within bounds + if (velocity >= 0.0f) { + CHECK(step.Ydd <= Amax); + CHECK(step.Ydd >= -Dmax); + CHECK((step.Yd - velocity) / dt <= Amax * 1.002f); + CHECK((step.Yd - velocity) / dt >= -Dmax * 1.002f); + } else { + CHECK(step.Ydd <= Dmax); + CHECK(step.Ydd >= -Amax); + CHECK((step.Yd - velocity) / dt <= Dmax * 1.002f); + CHECK((step.Yd - velocity) / dt >= -Amax * 1.002f); + } + + // Check if velocity within bounds + CHECK(step.Yd >= -Vmax_test); + CHECK(step.Yd <= Vmax_test); + CHECK((step.Y - position) / dt >= -Vmax_test * 1.002f); + CHECK((step.Y - position) / dt <= Vmax_test * 1.002f); + velocity = step.Yd; + + // Check if position is making progress + // TODO: the trajectory planner currently needs three "warm-up" iterations + // until its position makes progress. This should probably be revisited. + // TODO: this is disabled currently because there are legitimate trajectories + // where the position first moves in the wrong direction. + //if ((replan_counter < replan_interval - 2) && (t <= traj.Tf_)) { + // CHECK(std::abs(step.Y - goal) < std::abs(position - goal)); + //} + position = step.Y; + + } while (t <= traj.Tf_); + + CHECK(position >= goal - 1.0f); + CHECK(position <= goal + 1.0f); + CHECK(velocity >= -Dmax * dt); + CHECK(velocity <= Dmax * dt); +} + + +TEST_SUITE("Trajectory Planner") { + // these form a triangle trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 > 16384 + TEST_CASE("neg-dir-triangle") { + run_trajectory_test(-8192.0f, 8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-triangle") { + run_trajectory_test(8192.0f, -8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + + // these form a trapezoid trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 < 16384 + TEST_CASE("neg-dir-trapezoid") { + run_trajectory_test(-25000.0f, 25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-trapezoid") { + run_trajectory_test(25000.0f, -25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + + // for the following tests note that v^2/(2*a) = 27712^2 / (2*22288) = 17227 > 16384 + TEST_CASE("neg-dir-not-enough-braking-distance") { + run_trajectory_test(-8192.0f, 8192.0f, -27712.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-not-enough-braking-distance") { + run_trajectory_test(8192.0f, -8192.0f, 27712.0f, 27712.0f, 22288.0f, 22288.0f); + } + + TEST_CASE("neg-dir-over-speed") { + run_trajectory_test(-8192.0f, 8192.0f, -40000.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-over-speed") { + run_trajectory_test(8192.0f, -8192.0f, 40000.0f, 27712.0f, 22288.0f, 22288.0f); + } +}