diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 064cb733..99f62b44 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -218,7 +218,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s if (vel_des < -vel_lim) vel_des = -vel_lim; // Check for overspeed fault (done in this module (controller) for cohesion with vel_lim) - if (config_.vel_limit_tolerance > 0.0f) { // 0.0f to disable + if (config_.vel_limit_tolerance > 0.0f) { // 0.0f to disable if (fabsf(vel_estimate) > config_.vel_limit_tolerance * vel_lim) { set_error(ERROR_OVERSPEED); return false; @@ -255,6 +255,26 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s Iq = -Ilim; } + // Velocity limiting in current mode + if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL && config_.vel_limit > 0.0f && config_.vel_gain > 0.0f) { + float Imax = (config_.vel_limit - fabsf(vel_estimate)) * config_.vel_gain; + if (Iq > 0 && Iq > Imax) { + limited = true; + if (Imax > 0) { + Iq = Imax; + } else { + Iq = 0; + } + } else if (Iq < 0 && Iq < -Imax) { + limited = true; + if (Imax > 0) { + Iq = -Imax; + } else { + Iq = 0; + } + } + } + // Velocity integrator (behaviour dependent on limiting) if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) { // reset integral if not in use diff --git a/Firmware/Tests/test_other.cpp b/Firmware/Tests/test_other.cpp new file mode 100644 index 00000000..d42680b5 --- /dev/null +++ b/Firmware/Tests/test_other.cpp @@ -0,0 +1 @@ +#include \ No newline at end of file diff --git a/Firmware/Tests/test_runner.cpp b/Firmware/Tests/test_runner.cpp index 9be49eae..4b8521ca 100644 --- a/Firmware/Tests/test_runner.cpp +++ b/Firmware/Tests/test_runner.cpp @@ -229,4 +229,37 @@ TEST_SUITE("delta_enc"){ CHECK(getDelta(550, 450, cpr) == 100); CHECK(getDelta(450, 550, cpr) == -100); } +} + +TEST_SUITE("velLimiter") { + // Velocity limiting in current mode + auto limitVel(float vel_limit, float vel_estimate, float vel_gain, float Iq) { + float Imax = (vel_limit - fabsf(vel_estimate)) * vel_gain; + // bool limited = false; + if (Iq > 0 && Iq > Imax) { + // limited = true; + if (Imax > 0) { + Iq = Imax; + } else { + Iq = 0; + } + } else if (Iq < 0 && Iq < -Imax) { + // limited = true; + if (Imax > 0) { + Iq = -Imax; + } else { + Iq = 0; + } + } + return Iq; + } + + TEST_CASE("limit Vel"){ + CHECK(limitVel(0, 0, 0, 0) == 0.0f); + CHECK(limitVel(1000.0f, 1.0f, 0.0f, 0.0f) == 0.0f); + CHECK(limitVel(1000.0f, 500.0f, 1.0f, 1.0f) == 1.0f); + CHECK(limitVel(1000.0f, 500.0f, 1.0f, -20.0f) == -20.0f); + CHECK(limitVel(1000.0f, 999.0f, 1.0f, 2.0f) == 1.0f); + CHECK(limitVel(1000.0f, 999.0f, 1.0f, -5.0f) == -5.0f); + } } \ No newline at end of file diff --git a/docs/getting-started.md b/docs/getting-started.md index f1a3fc70..e9b71369 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -125,21 +125,16 @@ Try step 5 again ### Linux -1. [Install Python 3](https://www.python.org/downloads/). -2. Install the ODrive tools by opening a terminal and typing `pip install odrive` Enter -3. Set up USB permissions -```bash - echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d[0-9][0-9]", MODE="0666"' | sudo tee /etc/udev/rules.d/91-odrive.rules - sudo udevadm control --reload-rules - sudo udevadm trigger -``` +1. [Install Python 3](https://www.python.org/downloads/). (for example, on Ubuntu, `sudo apt install python3 python3-pip`) +2. Install the ODrive tools by opening a terminal and typing `sudo pip3 install odrive` Enter +3. (needed on Ubuntu, maybe other distros too) Add odrivetool into the path, by adding `~/.local/bin/` into `~/.bash_profile`, for example by running `nano ~/.bashrc`, scrolling to the bottom, pasting `PATH=$PATH:~/.local/bin/`, and then saving and closing, and close and reopen the terminal window. ## Firmware **ODrive v3.5 and later**
Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue. **ODrive v3.4 and earlier**
-Your board does **not** come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue. +Your board does **not** come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the ST Link procedure before you continue. ## Start `odrivetool` To launch the main interactive ODrive tool, type `odrivetool` Enter. Connect your ODrive and wait for the tool to find it. Now you can, for instance type `odrv0.vbus_voltage` Enter to inpect the boards main supply voltage. @@ -194,7 +189,7 @@ This is the resistance of the brake resistor. If you are not using it, you may s `odrv0.axis0.motor.config.pole_pairs` This is the number of **magnet poles** in the rotor, **divided by two**. To find this, you can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._ -If you can't see them, try sliding a magnet around the rotor, and counting how many times it stops. This will be the number of **pole pairs**. If you use a magnetic piece of metal instead of a magnet, you will get the number of **magnet poles**. +If you can't see them, try sliding a loose magnet in your hand around the rotor, and counting how many times it stops. This will be the number of **pole pairs**. If you use a magnetic piece of metal instead of a magnet, you will get the number of **magnet poles**. `odrv0.axis0.motor.config.motor_type` This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and gimbal motors (`MOTOR_TYPE_GIMBAL`).