Compare commits
1
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Icedsoda
|
0d713b9a33 |
No files matched your search
@@ -1,3 +0,0 @@
|
||||
.png filter=lfs diff=lfs merge=lfs -text
|
||||
.jpg filter=lfs diff=lfs merge=lfs -text
|
||||
.jpeg filter=lfs diff=lfs merge=lfs -text
|
||||
@@ -1,29 +0,0 @@
|
||||
This is a machine control software project.
|
||||
|
||||
## Repository structure
|
||||
|
||||
### Frontend
|
||||
|
||||
`/electron`: Frontend code for the control software built with React and Electron.
|
||||
|
||||
### Backend
|
||||
|
||||
`/server`: Implements machine setup & loop. Exposes a REST & Socket.io API for the electron frontend.
|
||||
`/ethercat-hal`: Hardware abstraction layer for Beckhoff (and possibly other EtherCat) devices and protocols.
|
||||
`/ethercat-hal-derive`: Macros for ethercat-hal
|
||||
`/control-core`: Generic logic for the server.
|
||||
|
||||
### HMI Panel Operating System
|
||||
|
||||
`/nixos`: Custom Linux with realtime kernel & preconfigured for touchscreens.
|
||||
|
||||
### Other
|
||||
|
||||
/docs: Documentation for the project.
|
||||
|
||||
## Key Guidelines
|
||||
|
||||
1. Follow Rust & Typescript best practices and idiomatic patterns
|
||||
2. Maintain existing code structure and organization
|
||||
3. Write unit tests for new functionality where applicable.
|
||||
4. Document public APIs and complex logic in code. Suggest changes to the `docs/` folder when appropriate
|
||||
+11
-14
@@ -1,19 +1,16 @@
|
||||
# To get started with Dependabot version updates, you'll need to specify which
|
||||
# package ecosystems to update and where the package manifests are located.
|
||||
# Please see the documentation for all configuration options:
|
||||
# https://docs.github.com/code-security/dependabot/dependabot-version-updates/configuration-options-for-the-dependabot.yml-file
|
||||
|
||||
version: 2
|
||||
updates:
|
||||
- package-ecosystem: "npm"
|
||||
directory: "/electron"
|
||||
- package-ecosystem: "npm" # See documentation for possible values
|
||||
directory: "/electron" # Location of package manifests
|
||||
schedule:
|
||||
interval: "monthly"
|
||||
ignore:
|
||||
- dependency-name: "*"
|
||||
update-types:
|
||||
- "version-update:semver-minor"
|
||||
interval: "weekly"
|
||||
|
||||
- package-ecosystem: "cargo"
|
||||
directory: "/"
|
||||
- package-ecosystem: "cargo" # See documentation for possible values
|
||||
directory: "/" # Location of package manifests
|
||||
schedule:
|
||||
interval: "monthly"
|
||||
ignore:
|
||||
- dependency-name: "*"
|
||||
update-types:
|
||||
- "version-update:semver-minor"
|
||||
interval: "weekly"
|
||||
@@ -36,17 +36,11 @@ jobs:
|
||||
- name: Install
|
||||
run: npm i
|
||||
|
||||
- name: Build
|
||||
run: tsc
|
||||
|
||||
- name: Format
|
||||
run: npm run format
|
||||
|
||||
- name: Lint
|
||||
run: npm run lint
|
||||
|
||||
- name: Test
|
||||
run: npm run test
|
||||
|
||||
- name: Package
|
||||
- name: Build
|
||||
run: npm run package
|
||||
@@ -21,8 +21,6 @@ jobs:
|
||||
- uses: actions/checkout@v4
|
||||
- name: Set Rust version
|
||||
run: rustup override set ${{ env.RUST_VERSION }}
|
||||
- name: Install Formatter
|
||||
run: rustup component add rustfmt
|
||||
- name: Cache Rust dependencies
|
||||
uses: actions/cache@v3
|
||||
with:
|
||||
@@ -37,12 +35,7 @@ jobs:
|
||||
${{ runner.os }}-${{ env.RUST_VERSION }}-cargo-
|
||||
|
||||
- name: Build
|
||||
env:
|
||||
RUSTFLAGS: "-D warnings"
|
||||
run: cargo build --verbose
|
||||
|
||||
- name: Format
|
||||
run: cargo fmt --check --verbose
|
||||
|
||||
- name: Run tests
|
||||
run: cargo test --verbose
|
||||
@@ -1,49 +0,0 @@
|
||||
# Canonical identities on the left, aliases on the right.
|
||||
# Format options:
|
||||
# Canon Name <canon@example.com> <alias@example.com>
|
||||
# Canon Name <canon@example.com> Alias Name <alias@example.com>
|
||||
|
||||
# Icedsoda -> Philipp
|
||||
Philipp <29167146+1cedsoda@users.noreply.github.com> <29167146+1cedsoda@users.noreply.github.com>
|
||||
|
||||
# kraemr / Robin Krämer -> Robin
|
||||
Robin <93094049+kraemr@users.noreply.github.com> <93094049+kraemr@users.noreply.github.com>
|
||||
Robin <93094049+kraemr@users.noreply.github.com> Robin Krämer <93094049+kraemr@users.noreply.github.com>
|
||||
|
||||
# Oshgnacknak -> Josh
|
||||
Josh <osh@oshgnacknak.de> <osh@oshgnacknak.de>
|
||||
|
||||
# Milan
|
||||
Milan <61654753+MilanVDB@users.noreply.github.com>
|
||||
|
||||
# dependabot stays as bot
|
||||
Dependabot <49699333+dependabot[bot]@users.noreply.github.com> <49699333+dependabot[bot]@users.noreply.github.com>
|
||||
|
||||
# Medformatik / Florian -> Florian
|
||||
Florian <dev@fwcr.de> <dev@fwcr.de>
|
||||
Florian <dev@fwcr.de> Medformatik <dev@fwcr.de>
|
||||
|
||||
# Anatomik5 -> Alisher (merge noreply + gmail under Alisher)
|
||||
Alisher <115714693+Anatomik5@users.noreply.github.com> <115714693+Anatomik5@users.noreply.github.com>
|
||||
Alisher <115714693+Anatomik5@users.noreply.github.com> Anatomik5 <darmenovalisher@gmail.com>
|
||||
Alisher <115714693+Anatomik5@users.noreply.github.com> Darmenov Alisher <115714693+Anatomik5@users.noreply.github.com>
|
||||
|
||||
# snacj -> Melvin
|
||||
Melvin <0xsnacj@proton.me> <0xsnacj@proton.me>
|
||||
Melvin <0xsnacj@proton.me> snacj <bbmelvin@gmx.de>
|
||||
Melvin <0xsnacj@proton.me> Melvin Spieß <0xsnacj@proton.me>
|
||||
|
||||
# Janhegerq -> Jan
|
||||
Jan <219994445+Janhegerq@users.noreply.github.com> <219994445+Janhegerq@users.noreply.github.com>
|
||||
Jan <219994445+Janhegerq@users.noreply.github.com> <19994445+Janhegerq@users.noreply.github.com>
|
||||
|
||||
# Christian / TheBest6337 -> christian
|
||||
Christian <christian.zschoegner@outlook.de> <christian.zschoegner@outlook.de>
|
||||
Christian <christian.zschoegner@outlook.de> TheBest6337 <christian.zschoegner@outlook.de>
|
||||
Christian <christian.zschoegner@outlook.de> Christian <80114847+TheBest6337@users.noreply.github.com>
|
||||
|
||||
# nixos-install.sh
|
||||
nixos-install.sh <nixos-install.sh@localhost> <nixos-install.sh@localhost>
|
||||
|
||||
# copilot-swe-agent bot
|
||||
Copilot <198982749+Copilot@users.noreply.github.com> <198982749+Copilot@users.noreply.github.com>
|
||||
Vendored
+2
-3
@@ -1,6 +1,5 @@
|
||||
{
|
||||
"typescript.inlayHints.enumMemberValues.enabled": false,
|
||||
"git.detectSubmodules": true,
|
||||
"git.detectSubmodulesLimit": 10,
|
||||
"rust-analyzer.check.command": "clippy",
|
||||
"git.detectSubmodules": true,
|
||||
"git.detectSubmodulesLimit": 10,
|
||||
}
|
||||
+1
-202
@@ -1,202 +1 @@
|
||||
# `2.9.1`
|
||||
_10.10.2025_
|
||||
|
||||
## Winder V2
|
||||
- [#796](https://github.com/qitechgmbh/control/pull/796) Winding in adaptive mode, while puller is in reverse mode is now possible
|
||||
|
||||
## General
|
||||
- [#780](https://github.com/qitechgmbh/control/pull/780) Updates are faster now
|
||||
- [#746](https://github.com/qitechgmbh/control/pull/746) Improved scrollbar styling for manuals
|
||||
|
||||
# `2.9.0`
|
||||
_06.10.2025_
|
||||
|
||||
## Winder V2
|
||||
- [#778](https://github.com/qitechgmbh/control/pull/778) Increased Winder Traverse Step size limit
|
||||
|
||||
## General
|
||||
- [#725](https://github.com/qitechgmbh/control/pull/725) Improved troubleshooting and update page
|
||||
- [#625](https://github.com/qitechgmbh/control/pull/625) Improved stability of Machine-Connections
|
||||
|
||||
|
||||
# `2.8.0`
|
||||
_30.09.2025_
|
||||
|
||||
|
||||
## Laser V1
|
||||
- [#741](https://github.com/qitechgmbh/control/pull/741) Added roundness measurement to two-axis Laser
|
||||
|
||||
|
||||
## Winder V2
|
||||
- [#745](https://github.com/qitechgmbh/control/pull/745), [#756](https://github.com/qitechgmbh/control/pull/756) improved Winder Frontend
|
||||
|
||||
## General
|
||||
- [#447](https://github.com/qitechgmbh/control/pull/447) added AquaPath Machine
|
||||
|
||||
|
||||
|
||||
|
||||
# `2.7.0`
|
||||
|
||||
_17.09.2025_
|
||||
|
||||
## Extruder V2
|
||||
|
||||
- [#695](https://github.com/qitechgmbh/control/pull/695) Pressure and Rpm Regulation properly switch again
|
||||
|
||||
## Winder V2
|
||||
|
||||
- [#727](https://github.com/qitechgmbh/control/pull/727) Removed estimated diameter
|
||||
|
||||
## Laser V1
|
||||
|
||||
- [#731](https://github.com/qitechgmbh/control/pull/731) Added Laser with two-axis measurements
|
||||
|
||||
## General
|
||||
|
||||
- [#679](https://github.com/qitechgmbh/control/pull/679),[#677](https://github.com/qitechgmbh/control/pull/677),[#698](https://github.com/qitechgmbh/control/pull/698), [#684](https://github.com/qitechgmbh/control/pull/684),[#706](https://github.com/qitechgmbh/control/pull/706) Greatly improved overall system latency and stability
|
||||
|
||||
- [#720](https://github.com/qitechgmbh/control/pull/720), [#686](https://github.com/qitechgmbh/control/pull/686) Improved User Interface
|
||||
|
||||
# `2.6.0`
|
||||
|
||||
_03.09.2025_
|
||||
|
||||
## Extruder V2
|
||||
|
||||
- [#658](https://github.com/qitechgmbh/control/pull/658) Added manual
|
||||
- [#639](https://github.com/qitechgmbh/control/pull/639) Swapped order of screw speed regulation buttons
|
||||
|
||||
## Laser V1
|
||||
|
||||
- [#642](https://github.com/qitechgmbh/control/pull/642) Improved stability of laser connection
|
||||
|
||||
## General
|
||||
|
||||
- [#657](https://github.com/qitechgmbh/control/pull/657) Time frame for graphs is now remembered
|
||||
- [#641](https://github.com/qitechgmbh/control/pull/641) Removed duplicate scrollbar from manuals
|
||||
|
||||
# `2.5.0`
|
||||
|
||||
_27.08.2025_
|
||||
|
||||
## Extruder V2
|
||||
|
||||
- [#595](https://github.com/qitechgmbh/control/pull/600) Simplified power graphs
|
||||
- [#593](https://github.com/qitechgmbh/control/pull/599) Added identifiers for temperature sensors in temperature graph
|
||||
|
||||
## Winder V2
|
||||
|
||||
- [#598](https://github.com/qitechgmbh/control/pull/601) Images now load properly in the manual
|
||||
|
||||
## General
|
||||
|
||||
- [#540](https://github.com/qitechgmbh/control/pull/592) Improved readability of the presets
|
||||
- [#591](https://github.com/qitechgmbh/control/pull/626) Significantly improved user interface performance and stability
|
||||
|
||||
# `2.4.0`
|
||||
|
||||
_14.08.2025_
|
||||
|
||||
## Winder V2
|
||||
|
||||
- [#550](https://github.com/qitechgmbh/control/pull/551) Can now reset wound distance while winding
|
||||
|
||||
## General
|
||||
|
||||
- [#568](https://github.com/qitechgmbh/control/pull/569),[#572](https://github.com/qitechgmbh/control/pull/572), [#574](https://github.com/qitechgmbh/control/pull/587) Improved EtherCAT performance
|
||||
- [#588](https://github.com/qitechgmbh/control/pull/589) Improved frontend performance
|
||||
|
||||
# `2.3.0`
|
||||
|
||||
_06.08.2025_
|
||||
|
||||
## Extruder V2
|
||||
|
||||
- [#520](https://github.com/qitechgmbh/control/pull/523) added warning message incase inverter is overloaded
|
||||
- [#501](https://github.com/qitechgmbh/control/pull/508) raised pressure limit of pressured PID mode for extruder
|
||||
- [#519](https://github.com/qitechgmbh/control/pull/549) added total power consumption monitor and graphs for voltage,current and wattage of the motor
|
||||
|
||||
## Winder V2
|
||||
|
||||
- [#64](https://github.com/qitechgmbh/control/pull/504) added winder handbook
|
||||
|
||||
## Laser V1
|
||||
|
||||
- [#497](https://github.com/qitechgmbh/control/pull/498) improved diameter visualisation
|
||||
|
||||
## General
|
||||
|
||||
- [#62](https://github.com/qitechgmbh/control/pull/514),[#524](https://github.com/qitechgmbh/control/pull/528) added machine presets to load configurations
|
||||
- [#526](https://github.com/qitechgmbh/control/pull/541) added import and export of presets
|
||||
|
||||
# `2.2.0`
|
||||
|
||||
_15.07.2025_
|
||||
|
||||
## Extruder V2
|
||||
|
||||
- [#455](https://github.com/qitechgmbh/control/pull/458), Greatly improved stability and precision of pressure-based regulation for the motor
|
||||
- [#467](https://github.com/qitechgmbh/control/pull/468), [#448](https://github.com/qitechgmbh/control/pull/449), [#485](https://github.com/qitechgmbh/control/pull/486) Improved the Responsiveness for motor control and monitoring
|
||||
- [#138](https://github.com/qitechgmbh/control/pull/495), Added a Warning in case the Pressure sensor is disconnected or incorrectly wired
|
||||
- [#169](https://github.com/qitechgmbh/control/pull/492), Added a Confirmation Dialog when extruder has not reached 90% of target temperature
|
||||
|
||||
## Winder V2
|
||||
|
||||
- [#471](https://github.com/qitechgmbh/control/pull/480), Added a live value to track the pulled distance and the option of switching to a different Mode after a given distance.
|
||||
|
||||
## General
|
||||
|
||||
- [#121](https://github.com/qitechgmbh/control/pull/121), Use Linux realtime threads for EtherCAT and control loop (PREEMPT_RT)
|
||||
- [#138](https://github.com/qitechgmbh/control/pull/495), Updated Quick Start Guide
|
||||
|
||||
# `2.1.0`
|
||||
|
||||
_30.06.2025_
|
||||
|
||||
## Extruder V2
|
||||
|
||||
- Intial Stable Release of **Extruder V2**
|
||||
|
||||
## Winder V2
|
||||
|
||||
- [#420](https://github.com/qitechgmbh/control/pull/429), [#401](https://github.com/qitechgmbh/control/pull/401) Add _Adaptive_ spool speed algorithm, the old algorithm is now called _Minmax_ ans tillavailable.
|
||||
- [#427](https://github.com/qitechgmbh/control/pull/427) Add _Estimated Spool Diameter_ graph
|
||||
- [#419](https://github.com/qitechgmbh/control/pull/419) Fix Issue where traverse won't work at low spool speed
|
||||
|
||||
## Laser V1
|
||||
|
||||
- Initial Stable Release of **Laser V1**
|
||||
|
||||
## HMI Panel
|
||||
|
||||
- [#339](https://github.com/qitechgmbh/control/pull/338) Don't go into sleep Mmde
|
||||
- [#410](https://github.com/qitechgmbh/control/pull/410) Ship with Wireshark
|
||||
- [#424](https://github.com/qitechgmbh/control/pull/424) Updates can be canceled
|
||||
|
||||
## General
|
||||
|
||||
- [#269](https://github.com/qitechgmbh/control/pull/269), [#370](https://github.com/qitechgmbh/control/pull/370) Improved performace of live values
|
||||
- [#358](https://github.com/qitechgmbh/control/pull/358), [#343](https://github.com/qitechgmbh/control/pull/343), [#324](https://github.com/qitechgmbh/control/pull/324), [#390](https://github.com/qitechgmbh/control/pull/390), [#299](https://github.com/qitechgmbh/control/pull/299) Improved functionality for graphs
|
||||
- [#405](https://github.com/qitechgmbh/control/pull/405) Remove weird color behaviour on tocuhscreens
|
||||
- [#369](https://github.com/qitechgmbh/control/pull/369) More failsafe EtherCAT loop
|
||||
|
||||
# `2.0.1`
|
||||
|
||||
_10.06.2025_
|
||||
|
||||
# `2.0.0`
|
||||
|
||||
_10.06.2025_
|
||||
|
||||
## General
|
||||
|
||||
- Initial stable release of QiTech Control.
|
||||
|
||||
## Winder V2
|
||||
|
||||
- Initial stable release of **Winder V2**
|
||||
|
||||
## Features
|
||||
|
||||
- Stabilized **Winder V2**
|
||||
# Unreleased
|
||||
Generated
+391
-281
File diff suppressed because it is too large.
Load diff
-17
@@ -5,23 +5,6 @@ members = [
|
||||
"ethercat-hal-derive",
|
||||
"control-core",
|
||||
"ethercat-eeprom-dump",
|
||||
"control-core-derive",
|
||||
]
|
||||
exclude = []
|
||||
resolver = "2"
|
||||
|
||||
[workspace.lints.clippy]
|
||||
# Individual lints (priority 0)
|
||||
|
||||
# don't lint poissbilities for let chains because they were introduces after Rust 1.86
|
||||
collapsible_if = "allow"
|
||||
|
||||
# Lint groups (priority -1)
|
||||
correctness = { level = "warn", priority = -1 }
|
||||
suspicious = { level = "warn", priority = -1 }
|
||||
complexity = { level = "warn", priority = -1 }
|
||||
perf = { level = "warn", priority = -1 }
|
||||
style = { level = "warn", priority = -1 }
|
||||
pendantic = { level = "warn", priority = -1 }
|
||||
cargo = { level = "warn", priority = -1 }
|
||||
nursery = { level = "warn", priority = -1 }
|
||||
@@ -4,12 +4,6 @@
|
||||
|
||||
QiTech Control is an industrial control panel software for the next generation of QiTech recycling machines built on top of Beckhoff Automation hardware.
|
||||
|
||||
[](https://www.youtube.com/watch?v=KI3YeBwfV-s)
|
||||
*Click here to watch a video demo of our software.*
|
||||
|
||||
[](https://youtu.be/55egCAkQgyM)
|
||||
*Click here to watch a full explainer Video of our Software.*
|
||||
|
||||
# Repo Structure
|
||||
|
||||
Frontend
|
||||
@@ -35,7 +29,7 @@ Other
|
||||
|
||||
## Backend
|
||||
|
||||
To interface with Beckhoff and other EtherCAT devices we need an EtherCAT master stoftware. Possibilities are [PySOEM](https://github.com/bnjmnp/pysoem) (Python), [SOEM](https://github.com/OpenEtherCATsociety/SOEM) (C) and [Ethercrab](https://github.com/ethercrab-rs/ethercrab) (Rust). For realtime operation only C and Rust are suitable. We chose Rust because of safety and confidence in the written code.
|
||||
To interface with Beckhoff and other EtherCAT devices we need an EtherCAY master stoftware. Possibilities are [PySOEM](https://github.com/bnjmnp/pysoem) (Python), [SOEM](https://github.com/OpenEtherCATsociety/SOEM) (C) and [Ethercrab](https://github.com/ethercrab-rs/ethercrab) (Rust). For realtime operation only C and Rust are suitable. We chose Rust because of safety and confidence in the written code.
|
||||
|
||||
[SocketIO](https://socket.io/) was chosen for performant event driven communication from the backend to the server. But we still use REST with [axum](https://docs.rs/axum/latest/axum/) for the communication thet benefits from the request/response model.
|
||||
|
||||
@@ -47,8 +41,6 @@ We could combine the code of the frontend and backend using [Doxius](https://dio
|
||||
|
||||
# Dev Setup
|
||||
|
||||
[Developer Documentation](./docs/developer-docs/)
|
||||
|
||||
## Backend
|
||||
|
||||
- Rust stable 1.86^ toolchain (install via [rustup](https://rustup.rs/))
|
||||
@@ -67,12 +59,13 @@ We could combine the code of the frontend and backend using [Doxius](https://dio
|
||||
| Machine Type | Version | Release Date | Description | Change to Previous Version | Vendor ID | Machine ID | Implemented | Docs |
|
||||
| ------------ | ------- | ------------ | --------------------------- | ------------------------------------------------------ | -------------------------- | ---------- | ----------- | ------------------------------- |
|
||||
| Winder | V1 | ??? | Winding Filaments & Similar | - | 1 (Qitech Industries GmbH) | 1 | Reserved | - |
|
||||
| Winder | V2 | 2025 | Winding Filaments & Similar | Reengineered Traverse | 1 (Qitech Industries GmbH) | 2 | Yes | [](./docs/machines/winder-1.md) |
|
||||
| Winder | V2 | 2025 | Winding Filaments & Similar | Reengineered Traverse | 1 (Qitech Industries GmbH) | 2 | In Progress | [](./docs/machines/winder-1.md) |
|
||||
| Extruder | V1 | ??? | Single Screw Extruder | - | 1 (Qitech Industries GmbH) | 3 | Reserved | - |
|
||||
| Extruder | V2 | 2025 | Single Screw Extruder | PT100 Thermometers, Optional Additional Heating Zone 4 | 1 (Qitech Industries GmbH) | 4 | Yes | |
|
||||
| Waterway | V1 | 2025 | Filament Water Cooling | - | 1 (Qitech Industries GmbH) | 5 | In Progress | |
|
||||
| Laser | V1 | ??? | Diameter Measuring Laser | - | 1 (Qitech Industries GmbH) | 6 | Yes | |
|
||||
| Mock | - | ??? | Mock Machine for Testing | - | 1 (Qitech Industries GmbH) | 7 | Yes | - |
|
||||
| Extruder | V2 | 2025 | Single Screw Extruder | PT100 Thermometers, Optional Additional Heating Zone 4 | 1 (Qitech Industries GmbH) | 4 | Not Yet | |
|
||||
| Waterway | V1 | 2025 | Filament Water Cooling | - | 1 (Qitech Industries GmbH) | 5 | Not Yet | |
|
||||
| DRE | V1 | ??? | Diameter Measuring Laser | - | 1 (Qitech Industries GmbH) | 6 | Not Yet | |
|
||||
| Mock | - | ??? | Mock Machine for Testing | - | 1 (Qitech Industries GmbH) | 7 | Not Yet | - |
|
||||
|
||||
|
||||
# More Docs
|
||||
|
||||
@@ -97,9 +90,8 @@ We could combine the code of the frontend and backend using [Doxius](https://dio
|
||||
- Write Device Identification
|
||||
|
||||
- Server
|
||||
|
||||
- [x] [Threading](./docs/control-loop.md)
|
||||
- [x] [Logging](./docs/logging.md)
|
||||
- [X] [Threading](./docs/control-loop.md)
|
||||
- [X] [Logging](./docs/logging.md)
|
||||
- [ ] Control Loop Setup
|
||||
- Control Loop Thread
|
||||
- [ ] realtime
|
||||
|
||||
+5
-23
@@ -1,31 +1,13 @@
|
||||
#!/bin/bash
|
||||
|
||||
set -e
|
||||
|
||||
FEATURE=""
|
||||
|
||||
# Check if "mock-machine" was passed as an argument
|
||||
if [[ "$1" == "mock-machine" ]]; then
|
||||
echo "Building Debug Code with mock-machine feature"
|
||||
FEATURE="--features mock-machine"
|
||||
fi
|
||||
|
||||
if [[ "$1" == "release" ]]; then
|
||||
if [ $1 == "release" ]; then
|
||||
echo "building Release Code"
|
||||
cargo build --release
|
||||
else
|
||||
echo "Building Debug Code"
|
||||
cargo build $FEATURE
|
||||
echo "building Debug Code"
|
||||
cargo build
|
||||
fi
|
||||
|
||||
# Only setcap if not building with mock-machine
|
||||
if [ "$1" != "mock-machine" ]; then
|
||||
echo "Setting capabilities for server executable"
|
||||
sudo setcap 'cap_net_raw,cap_sys_nice=eip' ./target/debug/server
|
||||
|
||||
echo "Setting permissions for serial ports"
|
||||
# sudo chown "root:$(groups | cut -f1 -d' ')" /dev/ttyUSB*
|
||||
fi
|
||||
|
||||
# set cap for server executable
|
||||
sudo setcap cap_net_raw=eip ./target/debug/server
|
||||
# run
|
||||
./target/debug/server
|
||||
@@ -1,43 +0,0 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Nix store uses sqlite ... SO HAHAHAHAHH NO CONCURRENT ACCESS AHAHAHHAHAHAHHAHAHAHAH
|
||||
time nix build .#packages.x86_64-linux.server --impure --extra-experimental-features nix-command --extra-experimental-features flakes --out-link /tmp/server-result
|
||||
serverstore=$(readlink -f /tmp/server-result)
|
||||
|
||||
time nix build .#packages.x86_64-linux.electron --impure --option sandbox false --extra-experimental-features nix-command --extra-experimental-features flakes --out-link /tmp/electron-result
|
||||
electronstore=$(readlink -f /tmp/electron-result)
|
||||
|
||||
|
||||
nix copy --to "file:///opt/dev-cache" \
|
||||
$(readlink -f /tmp/server-result) \
|
||||
$(readlink -f /tmp/electron-result) --extra-experimental-features nix-command
|
||||
|
||||
nix store sign \
|
||||
--key-file /opt/dev-cache/cache-private-key.pem \
|
||||
--store "file:///opt/dev-cache" \
|
||||
--extra-experimental-features nix-command \
|
||||
"$serverstore" "$electronstore"
|
||||
|
||||
|
||||
nix path-info "$serverstore" --extra-experimental-features nix-command --extra-experimental-features flakes
|
||||
|
||||
|
||||
# nix verify --store "file:///opt/dev-cache" \
|
||||
# --trusted-public-keys "$(cat /opt/dev-cache/cache-public-key.pem)" --extra-experimental-features nix-command --extra-experimental-features flakes \
|
||||
# "$FIRST_PATH" || echo "Verification failed!"
|
||||
|
||||
|
||||
#time nix build .#packages.x86_64-linux.electron --extra-experimental-features nix-command --extra-experimental-features flakes --out-link /tmp/electron-result
|
||||
#electronstore=$(readlink -f /tmp/electron-result)
|
||||
|
||||
#nix-store --generate-binary-cache-key cache.key cache.pem --extra-experimental-features nix-command
|
||||
#nix store sign --key-file cache.key "$serverstore" --extra-experimental-features nix-command
|
||||
#nix path-info --json "$serverstore" --extra-experimental-features nix-command --extra-experimental-features flakes
|
||||
#export
|
||||
#nix-store --export /nix/store/<hash>-server > nix-cache/server.nar
|
||||
#nix-store --export /nix/store/<hash>-electron > nix-cache/electron.nar
|
||||
|
||||
# Any http server
|
||||
# cd ~/nix-cache
|
||||
# python3 -m http.server 8080
|
||||
#
|
||||
@@ -1,15 +0,0 @@
|
||||
[package]
|
||||
name = "control_core_derive"
|
||||
version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[lib]
|
||||
proc-macro = true
|
||||
|
||||
[dependencies]
|
||||
proc-macro2 = "1.0"
|
||||
quote = "1.0"
|
||||
syn = { version = "2.0", features = ["full"] }
|
||||
File diff suppressed because it is too large.
Load diff
@@ -1,65 +0,0 @@
|
||||
use proc_macro::TokenStream;
|
||||
use proc_macro2::TokenStream as TokenStream2;
|
||||
use quote::quote;
|
||||
use syn::{DeriveInput, Error};
|
||||
|
||||
#[proc_macro_derive(BuildEvent)]
|
||||
pub fn build_event_derive(item: TokenStream) -> TokenStream {
|
||||
build_event_derive2(item.into()).unwrap().into()
|
||||
}
|
||||
|
||||
fn build_event_derive2(item: TokenStream2) -> Result<TokenStream2, Error> {
|
||||
// Parse the input tokens into a syntax tree
|
||||
let ast: DeriveInput = syn::parse2(item)?;
|
||||
|
||||
let ident = &ast.ident;
|
||||
let (impl_generics, ty_generics, where_clause) = ast.generics.split_for_impl();
|
||||
|
||||
// Generate the BuildEvent impl
|
||||
let expanded = quote! {
|
||||
impl #impl_generics control_core::socketio::event::BuildEvent for #ident #ty_generics #where_clause {
|
||||
#[doc = "Implemented by the BuildEvent derive macro"]
|
||||
fn build(&self) -> control_core::socketio::event::Event<Self> {
|
||||
|
||||
control_core::socketio::event::Event::new(stringify!(#ident), self.clone())
|
||||
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
Ok(expanded)
|
||||
}
|
||||
|
||||
#[proc_macro_derive(Machine)]
|
||||
pub fn machine_derive(item: TokenStream) -> TokenStream {
|
||||
machine_derive2(item.into()).unwrap().into()
|
||||
}
|
||||
|
||||
fn machine_derive2(item: TokenStream2) -> Result<TokenStream2, Error> {
|
||||
// Parse the input tokens into a syntax tree
|
||||
let ast: DeriveInput = syn::parse2(item)?;
|
||||
|
||||
let ident = &ast.ident;
|
||||
|
||||
let (impl_generics, ty_generics, where_clause) = ast.generics.split_for_impl();
|
||||
|
||||
let expanded = quote! {
|
||||
impl #impl_generics control_core::machines::Machine for #ident #ty_generics #where_clause {
|
||||
fn get_machine_identification_unique(&self) -> control_core::machines::identification::MachineIdentificationUnique {
|
||||
self.machine_identification_unique.clone()
|
||||
}
|
||||
}
|
||||
|
||||
impl #impl_generics control_core::machines::AnyGetters for #ident #ty_generics #where_clause {
|
||||
fn as_any(&self) -> &dyn std::any::Any {
|
||||
self
|
||||
}
|
||||
|
||||
fn as_any_mut(&mut self) -> &mut dyn std::any::Any {
|
||||
self
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
Ok(expanded)
|
||||
}
|
||||
+7
-31
@@ -3,45 +3,21 @@ name = "control_core"
|
||||
version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
anyhow = "1.0.100"
|
||||
|
||||
axum = "0.8.6"
|
||||
|
||||
anyhow = "1.0.98"
|
||||
axum = "0.8.4"
|
||||
ethercat_hal = { path = "../ethercat-hal" }
|
||||
ethercrab = "0.6"
|
||||
interfaces = "0.0.9"
|
||||
serde = "1.0.219"
|
||||
serde_json = "1.0.143"
|
||||
serialport = "4.7.3"
|
||||
serde_json = "1.0.140"
|
||||
serialport = "4.7.1"
|
||||
smol = "2.0.2"
|
||||
socketioxide = "0.17.2"
|
||||
uom = { version = "0.36.0", default-features = false, features = ["f64"] }
|
||||
socketioxide = "0.17.1"
|
||||
uom = "0.36.0"
|
||||
bitvec = { version = "1.0.1", features = ["alloc"] }
|
||||
crc = "3.3.0"
|
||||
crc = "3.2.1"
|
||||
serial = "0.4.0"
|
||||
tracing = "0.1.41"
|
||||
erased-serde = "0.4.8"
|
||||
core_affinity = "0.8.3"
|
||||
control_core_derive = { version = "0.1.0", path = "../control-core-derive" }
|
||||
tokio = { version = "1.45.1", features = ["sync"], optional = true }
|
||||
|
||||
|
||||
[target.'cfg(unix)'.dependencies]
|
||||
libc = "0.2.176"
|
||||
|
||||
[dev-dependencies]
|
||||
approx = "0.5.1"
|
||||
textplots = "0.8"
|
||||
tokio = { version = "1.45.1", features = ["rt-multi-thread", "macros", "time"] }
|
||||
|
||||
|
||||
[features]
|
||||
default = []
|
||||
video-streaming = ["dep:tokio"]
|
||||
|
||||
[package.metadata.cargo-machete]
|
||||
ignored = ["axum","bitvec","control_core_derive"]
|
||||
@@ -0,0 +1,91 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::analog_output::AnalogOutput;
|
||||
use std::{
|
||||
f64::consts::PI,
|
||||
future::Future,
|
||||
pin::Pin,
|
||||
time::{Duration, Instant},
|
||||
};
|
||||
use uom::si::{angle::radian, f64::Angle, ratio::ratio};
|
||||
pub type AnalogFunction = Box<dyn Fn(Duration) -> f64 + Send + Sync>;
|
||||
|
||||
/// Can module analog output with a function
|
||||
pub struct AnalogFunctionGenerator {
|
||||
output: AnalogOutput,
|
||||
function: AnalogFunction,
|
||||
start_ts: Instant,
|
||||
}
|
||||
|
||||
impl AnalogFunctionGenerator {
|
||||
pub fn new(output: AnalogOutput, function: AnalogFunction) -> Self {
|
||||
Self {
|
||||
output,
|
||||
function,
|
||||
start_ts: Instant::now(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for AnalogFunctionGenerator {
|
||||
fn act(&mut self, now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let diff = now - self.start_ts;
|
||||
let value = (self.function)(diff);
|
||||
(self.output.write)((value as f32).into()).await;
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
pub fn analog_sine(amplitude: f64, normal: f64, period: Duration) -> AnalogFunction {
|
||||
Box::new(move |dt: Duration| {
|
||||
// Convert to seconds as f64 for calculation
|
||||
let dt_secs = dt.as_secs_f64();
|
||||
let period_secs = period.as_secs_f64();
|
||||
let phase_secs = dt_secs % period_secs;
|
||||
let angle = Angle::new::<radian>(2.0 * PI * phase_secs / period_secs);
|
||||
let value = angle.sin().get::<ratio>();
|
||||
value * (amplitude / 2.0) + normal + (amplitude / 2.0)
|
||||
})
|
||||
}
|
||||
|
||||
pub fn analog_multiply<const N: usize>(functions: [AnalogFunction; N]) -> AnalogFunction {
|
||||
Box::new(move |dt: Duration| functions.iter().fold(1.0, |acc, func| acc * func(dt)))
|
||||
}
|
||||
|
||||
pub fn analog_sawtooth(amplitude: f64, normal: f64, period: Duration) -> AnalogFunction {
|
||||
Box::new(move |dt: Duration| {
|
||||
let dt_secs = dt.as_secs_f64();
|
||||
let period_secs = period.as_secs_f64();
|
||||
let phase_secs = dt_secs % period_secs;
|
||||
let value = phase_secs / period_secs;
|
||||
value * amplitude + normal
|
||||
})
|
||||
}
|
||||
|
||||
pub fn analog_square(amplitude: f64, normal: f64, period: Duration) -> AnalogFunction {
|
||||
Box::new(move |dt: Duration| {
|
||||
let dt_secs = dt.as_secs_f64();
|
||||
let period_secs = period.as_secs_f64();
|
||||
let phase_secs = dt_secs % period_secs;
|
||||
let value = if phase_secs < period_secs / 2.0 {
|
||||
1.0
|
||||
} else {
|
||||
-1.0
|
||||
};
|
||||
value * amplitude + normal
|
||||
})
|
||||
}
|
||||
|
||||
pub fn analog_triangle(amplitude: f64, normal: f64, period: Duration) -> AnalogFunction {
|
||||
Box::new(move |dt: Duration| {
|
||||
let dt_secs = dt.as_secs_f64();
|
||||
let period_secs = period.as_secs_f64();
|
||||
let phase_secs = dt_secs % period_secs;
|
||||
let value = if phase_secs < period_secs / 2.0 {
|
||||
phase_secs / period_secs * 2.0
|
||||
} else {
|
||||
2.0 - phase_secs / period_secs * 2.0
|
||||
};
|
||||
value * amplitude + normal
|
||||
})
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::analog_input::{AnalogInput, physical::AnalogInputValue};
|
||||
use std::{future::Future, pin::Pin, time::Instant};
|
||||
|
||||
/// Log the state of a analog input
|
||||
#[derive(Debug)]
|
||||
pub struct AnalogInputGetter {
|
||||
input: AnalogInput,
|
||||
normalized: Option<f32>,
|
||||
}
|
||||
|
||||
impl AnalogInputGetter {
|
||||
pub fn new(input: AnalogInput) -> Self {
|
||||
Self {
|
||||
input,
|
||||
normalized: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Value from -1.0 to 1.0
|
||||
pub fn get_normalized(&self) -> Option<f32> {
|
||||
self.normalized
|
||||
}
|
||||
|
||||
pub fn get_physical(&self) -> Option<AnalogInputValue> {
|
||||
match self.get_normalized() {
|
||||
Some(normalized) => Some(self.input.range.normalized_to_physical(normalized)),
|
||||
None => return None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for AnalogInputGetter {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let state = (self.input.state)().await;
|
||||
self.normalized = Some(state.input.normalized);
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::digital_input::DigitalInput;
|
||||
use std::{future::Future, pin::Pin, time::Instant};
|
||||
|
||||
/// Log the state of a digital input
|
||||
#[derive(Debug)]
|
||||
pub struct DigitalInputGetter {
|
||||
input: DigitalInput,
|
||||
value: bool,
|
||||
}
|
||||
|
||||
impl DigitalInputGetter {
|
||||
pub fn new(input: DigitalInput) -> Self {
|
||||
Self {
|
||||
input,
|
||||
value: false,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the current value of the digital input
|
||||
pub fn value(&self) -> bool {
|
||||
self.value
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for DigitalInputGetter {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let state = (self.input.state)()
|
||||
.await
|
||||
.expect("Failed to get digital input state");
|
||||
self.value = state.input.value;
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::digital_input::DigitalInput;
|
||||
use std::{future::Future, pin::Pin, time::Instant};
|
||||
|
||||
/// Log the state of a digital input
|
||||
pub struct DigitalInputLogger {
|
||||
input: DigitalInput,
|
||||
}
|
||||
|
||||
impl DigitalInputLogger {
|
||||
pub fn new(input: DigitalInput) -> Self {
|
||||
Self { input }
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for DigitalInputLogger {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let state = (self.input.state)()
|
||||
.await
|
||||
.expect("Failed to get digital input state");
|
||||
println!("DigitalInputLogger: {}", state.input.value);
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::digital_output::DigitalOutput;
|
||||
use std::{
|
||||
future::Future,
|
||||
pin::Pin,
|
||||
time::{Duration, Instant},
|
||||
};
|
||||
|
||||
/// Set a digital output high and low with a given interval
|
||||
#[derive(Debug)]
|
||||
pub struct DigitalOutputBlinker {
|
||||
last_toggle: Instant,
|
||||
output: DigitalOutput,
|
||||
interval: Duration,
|
||||
enabled: bool,
|
||||
}
|
||||
|
||||
impl DigitalOutputBlinker {
|
||||
pub fn new(output: DigitalOutput, interval: Duration) -> Self {
|
||||
Self {
|
||||
last_toggle: Instant::now(),
|
||||
output,
|
||||
interval,
|
||||
enabled: true,
|
||||
}
|
||||
}
|
||||
pub fn set_interval(&mut self, interval: Duration) {
|
||||
self.interval = interval;
|
||||
}
|
||||
|
||||
pub fn set_enabled(&mut self, enabled: bool) {
|
||||
self.enabled = enabled;
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for DigitalOutputBlinker {
|
||||
fn act(&mut self, now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
{
|
||||
let state = (self.output.state)().await;
|
||||
if now - self.last_toggle > self.interval {
|
||||
match state.output.into() {
|
||||
true => (self.output.write)(false.into()).await,
|
||||
false => (self.output.write)(true.into()).await,
|
||||
}
|
||||
self.last_toggle = now;
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::digital_output::DigitalOutput;
|
||||
use std::{
|
||||
future::Future,
|
||||
pin::Pin,
|
||||
time::{Duration, Instant},
|
||||
};
|
||||
|
||||
/// Set a series of digital outputs high in sequence with a given interval
|
||||
pub struct DigitalOutputBlinkers {
|
||||
last_toggle: Instant,
|
||||
outputs: Vec<Option<DigitalOutput>>,
|
||||
index: usize,
|
||||
interval: Duration,
|
||||
enabled: bool,
|
||||
amount: usize,
|
||||
}
|
||||
|
||||
impl DigitalOutputBlinkers {
|
||||
pub fn new(outputs: Vec<Option<DigitalOutput>>, interval: Duration, amount: usize) -> Self {
|
||||
Self {
|
||||
last_toggle: Instant::now(),
|
||||
outputs,
|
||||
interval,
|
||||
enabled: true,
|
||||
index: 0,
|
||||
amount,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn set_interval(&mut self, interval: Duration) {
|
||||
self.interval = interval;
|
||||
}
|
||||
|
||||
pub fn set_enabled(&mut self, enabled: bool) {
|
||||
self.enabled = enabled;
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for DigitalOutputBlinkers {
|
||||
fn act(&mut self, now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
if now - self.last_toggle > self.interval {
|
||||
if let Some(output) = &self.outputs[self.index] {
|
||||
(output.write)(true.into()).await;
|
||||
}
|
||||
let index_end =
|
||||
(self.index + self.outputs.len() - self.amount) % self.outputs.len();
|
||||
if let Some(output) = &self.outputs[index_end] {
|
||||
(output.write)(false.into()).await;
|
||||
}
|
||||
self.last_toggle = now;
|
||||
self.index = (self.index + 1) % self.outputs.len();
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::digital_output::DigitalOutput;
|
||||
use std::{future::Future, pin::Pin, time::Instant};
|
||||
|
||||
/// Set a digital output high and low with a given interval
|
||||
#[derive(Debug)]
|
||||
pub struct DigitalOutputSetter {
|
||||
output: DigitalOutput,
|
||||
enabled: bool,
|
||||
}
|
||||
|
||||
impl DigitalOutputSetter {
|
||||
pub fn new(output: DigitalOutput) -> Self {
|
||||
Self {
|
||||
output,
|
||||
enabled: false,
|
||||
}
|
||||
}
|
||||
pub fn set(&mut self, enabled: bool) {
|
||||
self.enabled = enabled;
|
||||
}
|
||||
pub fn get(&self) -> bool {
|
||||
self.enabled
|
||||
}
|
||||
pub fn toggle(&mut self) {
|
||||
self.enabled = !self.enabled;
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for DigitalOutputSetter {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
{
|
||||
(self.output.write)(self.enabled.into()).await;
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,525 @@
|
||||
use super::Actor;
|
||||
use crate::modbus::{
|
||||
ModbusFunctionCode, ModbusRequest, ModbusResponse, calculate_modbus_rtu_timeout,
|
||||
};
|
||||
use ethercat_hal::io::serial_interface::{SerialEncoding, SerialInterface};
|
||||
use std::{
|
||||
collections::VecDeque,
|
||||
pin::Pin,
|
||||
time::{Duration, Instant},
|
||||
};
|
||||
use uom::{
|
||||
ConstZero,
|
||||
si::{f64::Frequency, frequency::centihertz},
|
||||
};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum State {
|
||||
/// WaitingForResponse is set after sending a request through the serial_interface
|
||||
WaitingForResponse,
|
||||
/// After Sending a Resuest we need to wait atleast one ethercat cycle
|
||||
/// After one Cycle we check if el6021 status has transmit accepted toggled
|
||||
/// Then we can set state = ReadyToSend
|
||||
WaitingForRequestAccept,
|
||||
/// After Receiving a Response we need to wait atleast one ethercat cycle
|
||||
/// After one Cycle we check if el6021 status has received accepted toggled
|
||||
WaitingForReceiveAccept,
|
||||
/// ReadyToSend is set after receiving the response from the serial_interface
|
||||
ReadyToSend,
|
||||
/// Initial State
|
||||
Uninitialized,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum OperationMode {
|
||||
PU,
|
||||
EXT,
|
||||
NET,
|
||||
}
|
||||
|
||||
impl MitsubishiInverterRS485Actor {
|
||||
pub fn convert_hz_float_to_word(&mut self, value: Frequency) -> u16 {
|
||||
let scaled = value.get::<centihertz>(); // Convert Hz to 0.01 Hz units
|
||||
scaled.round() as u16
|
||||
}
|
||||
|
||||
pub fn set_frequency_target(&mut self, frequency: Frequency) {
|
||||
let mut request: MitsubishiModbusRequest =
|
||||
MitsubishiControlRequests::WriteRunningFrequency.into();
|
||||
let result: u16 = self.convert_hz_float_to_word(frequency); // convert hz float to short
|
||||
request.request.data[2] = result.to_le_bytes()[1];
|
||||
request.request.data[3] = result.to_le_bytes()[0];
|
||||
self.add_request(request);
|
||||
}
|
||||
|
||||
pub fn read_running_frequency() {
|
||||
// Check if the current element pushed to front is freq event
|
||||
}
|
||||
|
||||
pub fn switch_operation_mode() {}
|
||||
}
|
||||
|
||||
/// Specifies all System environmet Variables
|
||||
/// Register addresses are calculated as follows: Register-value 40002 -> address: 40002-40001 -> address:0x1
|
||||
enum MitsubishiSystemRegister {
|
||||
/// Register 40002
|
||||
InverterReset,
|
||||
/// Register 40003
|
||||
ParameterClear,
|
||||
/// Register 40004
|
||||
AllParameterClear,
|
||||
/// Register 40006
|
||||
ParamClearNonCommunication,
|
||||
/// Register 40007
|
||||
AllParameterClearNonCommunication,
|
||||
/// Register 40009
|
||||
InverterStatusAndControl,
|
||||
/// Register 40010
|
||||
OperationModeAndSetting,
|
||||
/// Register 40014
|
||||
RunningFrequencyRAM,
|
||||
/// Register 40015
|
||||
RunningFrequencyEEPROM,
|
||||
/// Register 40201
|
||||
MotorFrequency,
|
||||
}
|
||||
|
||||
impl MitsubishiControlRequests {
|
||||
fn get_system_register(value: MitsubishiSystemRegister) -> u16 {
|
||||
match value {
|
||||
MitsubishiSystemRegister::InverterReset => 0x1,
|
||||
MitsubishiSystemRegister::ParameterClear => 0x2,
|
||||
MitsubishiSystemRegister::AllParameterClear => 0x3,
|
||||
MitsubishiSystemRegister::ParamClearNonCommunication => 0x5,
|
||||
MitsubishiSystemRegister::AllParameterClearNonCommunication => 0x6,
|
||||
MitsubishiSystemRegister::InverterStatusAndControl => 0x8,
|
||||
MitsubishiSystemRegister::OperationModeAndSetting => 0x9,
|
||||
MitsubishiSystemRegister::RunningFrequencyRAM => 0x0d,
|
||||
MitsubishiSystemRegister::RunningFrequencyEEPROM => 0x0e,
|
||||
MitsubishiSystemRegister::MotorFrequency => 0x00C8,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<MitsubishiControlRequests> for MitsubishiModbusRequest {
|
||||
fn from(request: MitsubishiControlRequests) -> Self {
|
||||
match request {
|
||||
MitsubishiControlRequests::WriteRunningFrequency => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::RunningFrequencyRAM,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::PresetHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0x0, 0x0],
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::WriteFrequency,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::ReadInverterStatus => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::InverterStatusAndControl,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::ReadHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0x00, 0x01], // Read 1 register
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::InverterStatus,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::StopMotor => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::InverterStatusAndControl,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::PresetHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0x00, 0x01], // Value 1 to stop
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::InverterControl,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::StartForwardRotation => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::InverterStatusAndControl,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::PresetHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0, 0b00000010], // Value 2 for forward rotation
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::InverterControl,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::StartReverseRotation => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::InverterStatusAndControl,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::PresetHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0, 0b00000100], // Value 4 for reverse rotation
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::InverterControl,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::ReadRunningFrequency => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::RunningFrequencyRAM,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::ReadHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0x00, 0x01], // Read 1 register
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::ReadFrequency,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::ReadMotorFrequency => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::MotorFrequency,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::ReadHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0x0, 0x1],
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::ReadMotorFrequency,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::ResetInverter => {
|
||||
let reg: u16 = MitsubishiControlRequests::get_system_register(
|
||||
MitsubishiSystemRegister::InverterReset,
|
||||
);
|
||||
let reg_bytes = reg.to_be_bytes();
|
||||
MitsubishiModbusRequest {
|
||||
request: ModbusRequest {
|
||||
slave_id: 1,
|
||||
function_code: ModbusFunctionCode::PresetHoldingRegister,
|
||||
data: vec![reg_bytes[0], reg_bytes[1], 0x0, 0b00000001],
|
||||
},
|
||||
request_type: RequestType::OperationCommand,
|
||||
expected_response_type: ResponseType::NoResponse,
|
||||
}
|
||||
}
|
||||
MitsubishiControlRequests::ClearAllParameters => todo!(),
|
||||
MitsubishiControlRequests::ClearNonCommunicationParameter => todo!(),
|
||||
MitsubishiControlRequests::ClearNonCommunicationParameters => todo!(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// These Requests Serve as Templates for controling the inverter
|
||||
#[derive(Debug)]
|
||||
pub enum MitsubishiControlRequests {
|
||||
/// Register 40002, Reset/Restart the Inverter
|
||||
ResetInverter,
|
||||
/// Register 40004, Clear ALL parameters
|
||||
ClearAllParameters,
|
||||
/// Register 40006, Clear a non communication parameter
|
||||
ClearNonCommunicationParameter,
|
||||
/// Register 40007, Clear all Non Communication related Parameters
|
||||
ClearNonCommunicationParameters,
|
||||
/// Register 40009, Read Inverter Status
|
||||
ReadInverterStatus,
|
||||
/// Register 40009, Stops the Motor
|
||||
StopMotor,
|
||||
/// Register 40009, Starts the Motor in Forward Rotation
|
||||
StartForwardRotation,
|
||||
/// Register 40009, Starts the Motor in Reverse Rotation
|
||||
StartReverseRotation,
|
||||
/// Register 40014, Read the current frequency the motor runs at (RAM)
|
||||
ReadRunningFrequency,
|
||||
/// Register 40014, Write the frequency
|
||||
WriteRunningFrequency,
|
||||
/// Read Register 40201, This contains the actual output frequency
|
||||
ReadMotorFrequency,
|
||||
}
|
||||
|
||||
// We need to know from the request queue which events are of what operation type, so that the correct timeout can be used
|
||||
#[derive(Debug)]
|
||||
pub struct MitsubishiModbusRequest {
|
||||
request: ModbusRequest,
|
||||
request_type: RequestType,
|
||||
expected_response_type: ResponseType,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum RotationDirection {
|
||||
Forward,
|
||||
Backwards,
|
||||
Stopped,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum ResponseType {
|
||||
NoResponse,
|
||||
ReadFrequency,
|
||||
ReadMotorFrequency, // Motor Frequency is the actual frequency, that the motor is running at right now
|
||||
ReadInverterStatus,
|
||||
WriteFrequency,
|
||||
InverterStatus,
|
||||
InverterControl,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct MitsubishiInverterRS485Actor {
|
||||
// Communication
|
||||
pub serial_interface: SerialInterface,
|
||||
pub baudrate: Option<u32>,
|
||||
pub encoding: Option<SerialEncoding>,
|
||||
pub request_queue: VecDeque<MitsubishiModbusRequest>,
|
||||
pub response_queue: VecDeque<ModbusResponse>,
|
||||
|
||||
// State
|
||||
pub last_ts: Instant,
|
||||
pub last_message_size: usize,
|
||||
pub last_request_type: RequestType,
|
||||
pub state: State,
|
||||
pub next_response_type: ResponseType,
|
||||
pub frequency: Frequency,
|
||||
}
|
||||
|
||||
impl MitsubishiInverterRS485Actor {
|
||||
pub fn new(serial_interface: SerialInterface) -> Self {
|
||||
Self {
|
||||
serial_interface,
|
||||
last_ts: Instant::now(),
|
||||
state: State::Uninitialized,
|
||||
request_queue: VecDeque::new(),
|
||||
response_queue: VecDeque::new(),
|
||||
next_response_type: ResponseType::ReadMotorFrequency,
|
||||
last_request_type: RequestType::OperationCommand,
|
||||
last_message_size: 0,
|
||||
baudrate: None,
|
||||
encoding: None,
|
||||
frequency: Frequency::ZERO,
|
||||
}
|
||||
}
|
||||
|
||||
/// This would get called by the api to add a new request to the inverter
|
||||
pub fn add_request(&mut self, request: MitsubishiModbusRequest) {
|
||||
self.request_queue.push_front(request);
|
||||
}
|
||||
|
||||
/// This is used by the Api to pop off the Response of our Request
|
||||
pub fn get_response(&mut self) -> Option<ModbusResponse> {
|
||||
if self.response_queue.len() == 0 {
|
||||
return None;
|
||||
}
|
||||
self.response_queue.pop_back()
|
||||
}
|
||||
|
||||
/// This is used internally to read the receive buffer of the el6021
|
||||
fn read_modbus_response(
|
||||
&mut self,
|
||||
) -> Pin<Box<dyn Future<Output = Result<ModbusResponse, anyhow::Error>> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
if !(self.serial_interface.has_message)().await {
|
||||
// return;
|
||||
}
|
||||
|
||||
let res: Option<Vec<u8>> = (self.serial_interface.read_message)().await;
|
||||
let raw_response = match res {
|
||||
Some(res) => res,
|
||||
None => {
|
||||
vec![]
|
||||
}
|
||||
};
|
||||
|
||||
let response: Result<ModbusResponse, _> =
|
||||
ModbusResponse::try_from(raw_response.clone());
|
||||
|
||||
match response {
|
||||
Ok(result) => {
|
||||
self.last_message_size = result.clone().data.len() + 4;
|
||||
self.state = State::ReadyToSend;
|
||||
Ok(result)
|
||||
}
|
||||
Err(_) => {
|
||||
tracing::error!("Error Parsing ModbusResponse!");
|
||||
self.state = State::ReadyToSend;
|
||||
Err(anyhow::anyhow!("error"))
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
/// This is used internally to fill the write buffer of the el6021
|
||||
fn send_modbus_request(&mut self) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
if self.request_queue.len() == 0 {
|
||||
return;
|
||||
};
|
||||
let request: MitsubishiModbusRequest = self.request_queue.pop_back().unwrap();
|
||||
self.next_response_type = request.expected_response_type;
|
||||
self.last_request_type = self.last_request_type;
|
||||
let modbus_request: Vec<u8> = request.request.into();
|
||||
let res = (self.serial_interface.write_message)(modbus_request.clone()).await;
|
||||
match res {
|
||||
Ok(_) => (),
|
||||
Err(_) => tracing::error!("ERROR: serial_interface.write_message has failed"),
|
||||
}
|
||||
self.state = State::WaitingForRequestAccept;
|
||||
self.last_message_size = modbus_request.len();
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub enum RequestType {
|
||||
/// Monitoring, Operation (start,stop etc) command, frequency setting (RAM), less than 12 milliseconds timeout for Response
|
||||
OperationCommand,
|
||||
/// Parameter Read/Write and Frequency (EEPROM), Less than 30 milliseconds timeout for Response
|
||||
ReadWrite,
|
||||
/// Less than 5 seconds timeout for Response
|
||||
ParamClear,
|
||||
/// no Timeout for Response
|
||||
Reset,
|
||||
}
|
||||
|
||||
impl RequestType {
|
||||
fn timeout_duration(self) -> Duration {
|
||||
match self {
|
||||
RequestType::OperationCommand => Duration::from_millis(12),
|
||||
RequestType::ReadWrite => Duration::from_millis(30),
|
||||
RequestType::ParamClear => Duration::from_millis(5000),
|
||||
RequestType::Reset => Duration::from_millis(30),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub enum MitsubishiModbusExceptionCode {
|
||||
IllegalFunction,
|
||||
IllegalDataAddress,
|
||||
IllegalDataValue,
|
||||
None,
|
||||
}
|
||||
|
||||
impl From<MitsubishiModbusExceptionCode> for u8 {
|
||||
fn from(value: MitsubishiModbusExceptionCode) -> Self {
|
||||
match value {
|
||||
MitsubishiModbusExceptionCode::None => 0,
|
||||
MitsubishiModbusExceptionCode::IllegalFunction => 1,
|
||||
MitsubishiModbusExceptionCode::IllegalDataAddress => 2,
|
||||
MitsubishiModbusExceptionCode::IllegalDataValue => 3,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<u8> for MitsubishiModbusExceptionCode {
|
||||
fn from(value: u8) -> Self {
|
||||
match value {
|
||||
1 => MitsubishiModbusExceptionCode::IllegalFunction,
|
||||
2 => MitsubishiModbusExceptionCode::IllegalDataAddress,
|
||||
3 => MitsubishiModbusExceptionCode::IllegalDataValue,
|
||||
_ => MitsubishiModbusExceptionCode::None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Handle different Response types
|
||||
impl MitsubishiInverterRS485Actor {
|
||||
// When we get respone from Pr. 40014 (Running Frequency) Convert to rpm and save it
|
||||
fn handle_motor_frequency(&mut self, resp: ModbusResponse) {
|
||||
let freq_bytes = &resp.data[1..3]; // bytes 1 and 2 are needed
|
||||
let raw_frequency = u16::from_be_bytes([freq_bytes[0], freq_bytes[1]]) as f64;
|
||||
self.frequency = Frequency::new::<centihertz>(raw_frequency);
|
||||
}
|
||||
|
||||
// Technically we could verify that every request also was successful with this match and return an Error, or not
|
||||
fn handle_response(&mut self, resp: ModbusResponse) {
|
||||
match self.next_response_type {
|
||||
ResponseType::ReadFrequency => (),
|
||||
ResponseType::ReadInverterStatus => (),
|
||||
ResponseType::WriteFrequency => (),
|
||||
ResponseType::ReadMotorFrequency => self.handle_motor_frequency(resp),
|
||||
ResponseType::InverterStatus => (),
|
||||
ResponseType::InverterControl => (),
|
||||
ResponseType::NoResponse => (),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for MitsubishiInverterRS485Actor {
|
||||
fn act(&mut self, now_ts: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let elapsed: Duration = now_ts.duration_since(self.last_ts);
|
||||
// State is uninitialized until serial interface init returns true, which takes a few cycles on the el6021
|
||||
if let State::Uninitialized = self.state {
|
||||
let res = (self.serial_interface.initialize)().await;
|
||||
if res == true {
|
||||
self.state = State::ReadyToSend;
|
||||
// every time when our inverter is "Uninitialzed" reset it first to clear any error states it may have
|
||||
self.add_request(MitsubishiControlRequests::ResetInverter.into());
|
||||
self.baudrate = (self.serial_interface.get_baudrate)().await;
|
||||
self.encoding = (self.serial_interface.get_serial_encoding)().await;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
let encoding = match self.encoding {
|
||||
Some(encoding) => encoding,
|
||||
None => return,
|
||||
};
|
||||
|
||||
let baudrate = match self.baudrate {
|
||||
Some(baudrate) => baudrate,
|
||||
None => return,
|
||||
};
|
||||
|
||||
let timeout = calculate_modbus_rtu_timeout(
|
||||
encoding.total_bits(),
|
||||
self.last_request_type.timeout_duration(),
|
||||
baudrate,
|
||||
self.last_message_size,
|
||||
);
|
||||
|
||||
// if we have no requests add ReadMotorFrequency
|
||||
if self.request_queue.is_empty() {
|
||||
self.add_request(MitsubishiControlRequests::ReadMotorFrequency.into());
|
||||
}
|
||||
|
||||
if elapsed < timeout {
|
||||
return;
|
||||
}
|
||||
|
||||
self.last_ts = now_ts;
|
||||
match self.state {
|
||||
State::WaitingForResponse => {
|
||||
let ret = self.read_modbus_response().await;
|
||||
match ret {
|
||||
Ok(ret) => self.handle_response(ret),
|
||||
Err(_) => (), // Do nothing for now
|
||||
}
|
||||
}
|
||||
State::ReadyToSend => self.send_modbus_request().await,
|
||||
State::WaitingForReceiveAccept => self.state = State::ReadyToSend,
|
||||
State::WaitingForRequestAccept => self.state = State::WaitingForResponse,
|
||||
_ => (),
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
pub mod analog_function_generator;
|
||||
pub mod analog_input_getter;
|
||||
pub mod digital_input_getter;
|
||||
pub mod digital_input_logger;
|
||||
pub mod digital_output_blinker;
|
||||
pub mod digital_output_blinkers;
|
||||
pub mod digital_output_setter;
|
||||
pub mod mitsubishi_inverter_rs485;
|
||||
pub mod stepper_driver_el70x1;
|
||||
pub mod stepper_driver_max_speed;
|
||||
pub mod stepper_driver_pulse_train;
|
||||
pub mod temperature_input_getter;
|
||||
pub mod temperature_input_logger;
|
||||
|
||||
use std::{pin::Pin, time::Instant};
|
||||
|
||||
pub trait Actor: Send + Sync {
|
||||
fn act(&mut self, now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>>;
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
use super::Actor;
|
||||
use core::panic;
|
||||
use ethercat_hal::{
|
||||
helpers::el70xx_velocity_converter::EL70x1VelocityConverter,
|
||||
io::stepper_velocity_el70x1::StepperVelocityEL70x1, shared_config::el70x1::EL70x1SpeedRange,
|
||||
};
|
||||
use std::{future::Future, pin::Pin, time::Instant};
|
||||
|
||||
/// Set a digital output high and low with a given interval
|
||||
#[derive(Debug)]
|
||||
pub struct StepperDriverEL70x1 {
|
||||
stepper: StepperVelocityEL70x1,
|
||||
enabled: bool,
|
||||
velocity: i16,
|
||||
position: i128,
|
||||
set_position: Option<i128>,
|
||||
pub converter: EL70x1VelocityConverter,
|
||||
}
|
||||
|
||||
impl StepperDriverEL70x1 {
|
||||
pub fn new(stepper: StepperVelocityEL70x1, speed_range: &EL70x1SpeedRange) -> Self {
|
||||
Self {
|
||||
stepper,
|
||||
enabled: false,
|
||||
velocity: 0,
|
||||
converter: EL70x1VelocityConverter::new(speed_range),
|
||||
position: 0,
|
||||
set_position: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Set the speed in steps per second
|
||||
pub fn set_speed(&mut self, steps_per_second: i32) {
|
||||
self.velocity = self.converter.steps_to_velocity(steps_per_second, true)
|
||||
}
|
||||
|
||||
/// Get the speed in steps per second
|
||||
pub fn get_speed(&self) -> i32 {
|
||||
self.converter.velocity_to_steps(self.velocity, true) as i32
|
||||
}
|
||||
|
||||
/// Enable or disable the stepper
|
||||
pub fn set_enabled(&mut self, enabled: bool) {
|
||||
self.enabled = enabled;
|
||||
}
|
||||
|
||||
/// Get the enabled state of the stepper
|
||||
pub fn is_enabled(&self) -> bool {
|
||||
self.enabled
|
||||
}
|
||||
|
||||
/// Get the current position of the stepper
|
||||
pub fn get_position(&self) -> i128 {
|
||||
self.position
|
||||
}
|
||||
|
||||
/// Set the position of the stepper
|
||||
pub fn set_position(&mut self, position: i128) {
|
||||
self.set_position = Some(position);
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for StepperDriverEL70x1 {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let state = match (self.stepper.state)().await {
|
||||
Ok(state) => state,
|
||||
Err(e) => {
|
||||
panic!("Error while reading StepperVelocity {:?}", e);
|
||||
}
|
||||
};
|
||||
// set the input
|
||||
self.position = state.input.counter_value;
|
||||
|
||||
let mut output = state.output.clone();
|
||||
|
||||
// set the output
|
||||
output.enable = self.enabled;
|
||||
output.velocity = self.velocity;
|
||||
output.set_counter = self.set_position.take();
|
||||
|
||||
// write the output
|
||||
match (self.stepper.write)(output).await {
|
||||
Ok(_) => {}
|
||||
Err(e) => {
|
||||
panic!("Error while writing StepperVelocity {:?}", e);
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::digital_output::DigitalOutput;
|
||||
use std::{
|
||||
future::Future,
|
||||
pin::Pin,
|
||||
sync::{Arc, RwLock},
|
||||
time::Instant,
|
||||
};
|
||||
|
||||
/// Set a digital output high and low with a given interval
|
||||
pub struct StepperDriverMaxSpeed {
|
||||
pulse: DigitalOutput,
|
||||
}
|
||||
|
||||
impl StepperDriverMaxSpeed {
|
||||
pub fn new(output: DigitalOutput) -> Self {
|
||||
Self { pulse: output }
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for StepperDriverMaxSpeed {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let state = (self.pulse.state)().await;
|
||||
match state.output.into() {
|
||||
true => (self.pulse.write)(false.into()).await,
|
||||
false => (self.pulse.write)(true.into()).await,
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl From<StepperDriverMaxSpeed> for Arc<RwLock<StepperDriverMaxSpeed>> {
|
||||
fn from(actor: StepperDriverMaxSpeed) -> Self {
|
||||
Arc::new(RwLock::new(actor))
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,222 @@
|
||||
/// Translates the encoder value to a larger space and handle underflow/overflow situations
|
||||
#[derive(Debug)]
|
||||
pub struct Encoder {
|
||||
pub position: i64,
|
||||
pub last_encoder_position: u32,
|
||||
}
|
||||
|
||||
impl Encoder {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
position: 0,
|
||||
last_encoder_position: 0,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update(&mut self, counter_value: u32, counter_overflow: bool, counter_underflow: bool) {
|
||||
// Handle overflow case
|
||||
if counter_overflow {
|
||||
// When overflow occurs, the counter wraps from MAX to 0
|
||||
// Calculate how much it moved past MAX
|
||||
let delta = counter_value as i64;
|
||||
// Add the remaining distance from last position to MAX, plus the overflow amount
|
||||
self.position += (u32::MAX - self.last_encoder_position) as i64 + 1 + delta;
|
||||
}
|
||||
// Handle underflow case
|
||||
else if counter_underflow {
|
||||
// When underflow occurs, the counter wraps from 0 to MAX
|
||||
// Calculate how much it moved below 0
|
||||
let delta = (u32::MAX - counter_value) as i64;
|
||||
// Subtract the distance from last position to 0, plus the underflow amount
|
||||
self.position -= self.last_encoder_position as i64 + 1 + delta;
|
||||
}
|
||||
// Normal case - no overflow/underflow
|
||||
else {
|
||||
// Calculate the signed difference between current and last position
|
||||
let delta = counter_value as i64 - self.last_encoder_position as i64;
|
||||
// Update the position
|
||||
self.position += delta;
|
||||
}
|
||||
|
||||
// Update the last encoder position for next time
|
||||
self.last_encoder_position = counter_value;
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// Tests that the position tracker correctly handles normal forward movement
|
||||
/// without any overflow conditions.
|
||||
///
|
||||
/// This test verifies:
|
||||
/// 1. Initial state is zeroed
|
||||
/// 2. Moving forward updates both position and last_encoder_position
|
||||
/// 3. Consecutive forward movements accumulate correctly
|
||||
#[test]
|
||||
fn test_normal_forward_movement() {
|
||||
let mut tracker = Encoder::new();
|
||||
|
||||
// Initial state
|
||||
assert_eq!(tracker.position, 0);
|
||||
assert_eq!(tracker.last_encoder_position, 0);
|
||||
|
||||
// Move forward by 100
|
||||
tracker.update(100, false, false);
|
||||
assert_eq!(tracker.position, 100);
|
||||
assert_eq!(tracker.last_encoder_position, 100);
|
||||
|
||||
// Move forward by another 50
|
||||
tracker.update(150, false, false);
|
||||
assert_eq!(tracker.position, 150);
|
||||
assert_eq!(tracker.last_encoder_position, 150);
|
||||
}
|
||||
|
||||
/// Tests that the position tracker correctly handles normal backward movement
|
||||
/// without any underflow conditions.
|
||||
///
|
||||
/// This test verifies:
|
||||
/// 1. Starting from a non-zero position
|
||||
/// 2. Moving backward updates both position and last_encoder_position
|
||||
/// 3. Consecutive backward movements accumulate correctly
|
||||
#[test]
|
||||
fn test_normal_backward_movement() {
|
||||
let mut tracker = Encoder::new();
|
||||
|
||||
// Start at position 1000
|
||||
tracker.position = 1000;
|
||||
tracker.last_encoder_position = 1000;
|
||||
|
||||
// Move backward by 100
|
||||
tracker.update(900, false, false);
|
||||
assert_eq!(tracker.position, 900);
|
||||
assert_eq!(tracker.last_encoder_position, 900);
|
||||
|
||||
// Move backward by another 50
|
||||
tracker.update(850, false, false);
|
||||
assert_eq!(tracker.position, 850);
|
||||
assert_eq!(tracker.last_encoder_position, 850);
|
||||
}
|
||||
|
||||
/// Tests that the position tracker correctly handles overflow conditions
|
||||
/// where the encoder counter wraps from near its maximum value to a small value.
|
||||
///
|
||||
/// This test verifies:
|
||||
/// 1. When overflow flag is set, the tracker correctly calculates the true movement
|
||||
/// 2. Position is updated to account for the full distance traveled
|
||||
/// 3. last_encoder_position is updated to the new counter value
|
||||
#[test]
|
||||
fn test_overflow() {
|
||||
let mut tracker = Encoder::new();
|
||||
|
||||
// Set position near the maximum u32 value
|
||||
tracker.position = 100;
|
||||
tracker.last_encoder_position = u32::MAX - 10;
|
||||
|
||||
// Overflow: counter goes from (MAX-10) to 5
|
||||
// This means we moved forward by 10 steps to MAX, then 1 step to overflow,
|
||||
// then 5 more steps to reach the current position
|
||||
tracker.update(5, true, false);
|
||||
|
||||
// Expected: position += (10 + 1 + 5) = position += 16
|
||||
assert_eq!(tracker.position, 116);
|
||||
assert_eq!(tracker.last_encoder_position, 5);
|
||||
}
|
||||
|
||||
/// Tests that the position tracker correctly handles underflow conditions
|
||||
/// where the encoder counter wraps from near zero to a value near its maximum.
|
||||
///
|
||||
/// This test verifies:
|
||||
/// 1. When underflow flag is set, the tracker correctly calculates the true movement
|
||||
/// 2. Position is updated to account for the full distance traveled (in negative direction)
|
||||
/// 3. last_encoder_position is updated to the new counter value
|
||||
#[test]
|
||||
fn test_underflow() {
|
||||
let mut tracker = Encoder::new();
|
||||
|
||||
// Set position near zero
|
||||
tracker.position = 100;
|
||||
tracker.last_encoder_position = 10;
|
||||
|
||||
// Underflow: counter goes from 10 to (MAX-5)
|
||||
// This means we moved backward by 10 steps to 0, then 1 step to underflow,
|
||||
// then 5 more steps to reach the current position
|
||||
tracker.update(u32::MAX - 5, false, true);
|
||||
|
||||
// Expected: position -= (10 + 1 + 5) = position -= 16
|
||||
assert_eq!(tracker.position, 84);
|
||||
assert_eq!(tracker.last_encoder_position, u32::MAX - 5);
|
||||
}
|
||||
|
||||
/// Tests that the position tracker maintains correct behavior across
|
||||
/// a sequence of different operations including normal movement,
|
||||
/// overflow, and underflow.
|
||||
///
|
||||
/// This test verifies:
|
||||
/// 1. The tracker maintains state correctly between operations
|
||||
/// 2. Different types of movements can be handled in sequence
|
||||
/// 3. Position calculations remain accurate throughout various scenarios
|
||||
#[test]
|
||||
fn test_multiple_operations() {
|
||||
let mut tracker = Encoder::new();
|
||||
|
||||
// Normal forward
|
||||
tracker.update(100, false, false);
|
||||
assert_eq!(tracker.position, 100);
|
||||
|
||||
// Normal backward
|
||||
tracker.update(50, false, false);
|
||||
assert_eq!(tracker.position, 50);
|
||||
|
||||
// Overflow
|
||||
tracker.last_encoder_position = u32::MAX - 5;
|
||||
tracker.position = 50; // Reset for clarity
|
||||
tracker.update(10, true, false);
|
||||
assert_eq!(tracker.position, 66); // 50 + (5 + 1 + 10)
|
||||
|
||||
// Underflow
|
||||
tracker.last_encoder_position = 5;
|
||||
tracker.position = 100; // Reset for clarity
|
||||
tracker.update(u32::MAX - 10, false, true);
|
||||
assert_eq!(tracker.position, 84); // 100 - (5 + 1 + 10)
|
||||
}
|
||||
|
||||
/// Tests that the position tracker can handle multiple overflow and underflow
|
||||
/// events, maintaining an accurate position count that exceeds the u32 range.
|
||||
///
|
||||
/// This test verifies:
|
||||
/// 1. Multiple overflow events accumulate correctly in the i64 position
|
||||
/// 2. Multiple underflow events correctly subtract from the position
|
||||
/// 3. The i64 position field properly extends the range beyond u32 limits
|
||||
#[test]
|
||||
fn test_large_movements() {
|
||||
let mut tracker = Encoder::new();
|
||||
|
||||
// Test tracking beyond u32 limits with multiple overflows
|
||||
for _ in 0..5 {
|
||||
// Simulate overflow: move from (MAX-100) to 100
|
||||
tracker.last_encoder_position = u32::MAX - 100;
|
||||
tracker.update(100, true, false);
|
||||
|
||||
// Each iteration adds (100 + 1 + 100) = 201 to position
|
||||
// We move 100 steps to MAX, 1 step to overflow, and 100 more steps to reach 100
|
||||
}
|
||||
|
||||
// After 5 iterations, position should be 5 * 201 = 1005
|
||||
assert_eq!(tracker.position, 1005);
|
||||
|
||||
// Test tracking with multiple underflows
|
||||
for _ in 0..3 {
|
||||
// Simulate underflow: move from 100 to (MAX-100)
|
||||
tracker.last_encoder_position = 100;
|
||||
tracker.update(u32::MAX - 100, false, true);
|
||||
|
||||
// Each iteration subtracts (100 + 1 + 100) = 201 from position
|
||||
// We move 100 steps to 0, 1 step to underflow, and 100 more steps to reach MAX-100
|
||||
}
|
||||
|
||||
// Final position: 1005 - (3 * 201) = 1005 - 603 = 402
|
||||
assert_eq!(tracker.position, 402);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
use super::Actor;
|
||||
use encoder::Encoder;
|
||||
use ethercat_hal::io::pulse_train_output::PulseTrainOutput;
|
||||
use std::{future::Future, pin::Pin, time::Instant};
|
||||
|
||||
pub mod encoder;
|
||||
|
||||
/// Set a digital output high and low with a given interval
|
||||
#[derive(Debug)]
|
||||
pub struct StepperDriverPulseTrain {
|
||||
pulse: PulseTrainOutput,
|
||||
encoder: Encoder,
|
||||
frequency: i32,
|
||||
}
|
||||
|
||||
impl<'ptodevice> StepperDriverPulseTrain {
|
||||
pub fn new(output: PulseTrainOutput) -> Self {
|
||||
Self {
|
||||
pulse: output,
|
||||
encoder: Encoder::new(),
|
||||
frequency: 0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<'ptodevice> Actor for StepperDriverPulseTrain {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let state = (self.pulse.state)().await;
|
||||
let mut output = state.output.clone();
|
||||
|
||||
// sync the counter to the encoder
|
||||
self.encoder.update(
|
||||
state.input.counter_value,
|
||||
state.input.counter_overflow,
|
||||
state.input.counter_underflow,
|
||||
);
|
||||
|
||||
// write frequency
|
||||
output.frequency_value = self.frequency;
|
||||
|
||||
(self.pulse.write)(output).await;
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl<'ptodevice> StepperDriverPulseTrain {
|
||||
pub fn set_frequency(&mut self, frequency: i32) {
|
||||
self.frequency = frequency;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
use std::{pin::Pin, time::Instant};
|
||||
|
||||
use ethercat_hal::io::temperature_input::TemperatureInput;
|
||||
|
||||
use super::Actor;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct TemperatureInputGetter {
|
||||
input: TemperatureInput,
|
||||
temperature: f64,
|
||||
wiring_error: bool,
|
||||
}
|
||||
|
||||
impl TemperatureInputGetter {
|
||||
pub fn new(input: TemperatureInput) -> Self {
|
||||
Self {
|
||||
input,
|
||||
temperature: 0.0,
|
||||
wiring_error: false,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn get_temperature(&self) -> f64 {
|
||||
self.temperature
|
||||
}
|
||||
|
||||
pub fn get_wiring_error(&self) -> bool {
|
||||
self.wiring_error
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for TemperatureInputGetter {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let input = (self.input.state)().await.input;
|
||||
self.temperature = input.temperature as f64;
|
||||
self.wiring_error = input.overvoltage | input.undervoltage;
|
||||
if self.wiring_error {
|
||||
self.temperature = 0.0;
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
use super::Actor;
|
||||
use ethercat_hal::io::temperature_input::TemperatureInput;
|
||||
use std::{future::Future, pin::Pin, time::Instant};
|
||||
|
||||
/// Log the state of a temperature input
|
||||
pub struct TemperatureInputLogger {
|
||||
input: TemperatureInput,
|
||||
}
|
||||
|
||||
impl TemperatureInputLogger {
|
||||
pub fn new(input: TemperatureInput) -> Self {
|
||||
Self { input }
|
||||
}
|
||||
}
|
||||
|
||||
impl Actor for TemperatureInputLogger {
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let state = (self.input.state)().await;
|
||||
println!("TemperatureInputLogger: {:?}C", state.input.temperature);
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -1,218 +0,0 @@
|
||||
use std::time::Instant;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct ClampingTimeagnosticPidController {
|
||||
// Params
|
||||
/// Proportional gain
|
||||
kp: f64,
|
||||
/// Integral gain
|
||||
ki: f64,
|
||||
/// Derivative gain
|
||||
kd: f64,
|
||||
// State
|
||||
/// Proportional error
|
||||
ep: f64,
|
||||
/// Integral error
|
||||
ei: f64,
|
||||
/// Derivative error
|
||||
ed: f64,
|
||||
|
||||
min_ep: Option<f64>,
|
||||
max_ep: Option<f64>,
|
||||
|
||||
min_ei: Option<f64>,
|
||||
max_ei: Option<f64>,
|
||||
|
||||
min_ed: Option<f64>,
|
||||
max_ed: Option<f64>,
|
||||
|
||||
min_signal: Option<f64>,
|
||||
max_signal: Option<f64>,
|
||||
|
||||
last: Option<Instant>,
|
||||
}
|
||||
|
||||
impl ClampingTimeagnosticPidController {
|
||||
pub const fn new(
|
||||
kp: f64,
|
||||
ki: f64,
|
||||
kd: f64,
|
||||
min_ep: Option<f64>,
|
||||
max_ep: Option<f64>,
|
||||
min_ei: Option<f64>,
|
||||
max_ei: Option<f64>,
|
||||
min_ed: Option<f64>,
|
||||
max_ed: Option<f64>,
|
||||
min_signal: Option<f64>,
|
||||
max_signal: Option<f64>,
|
||||
) -> Self {
|
||||
Self {
|
||||
kp,
|
||||
ki,
|
||||
kd,
|
||||
ep: 0.0,
|
||||
ei: 0.0,
|
||||
ed: 0.0,
|
||||
last: None,
|
||||
min_ep,
|
||||
max_ep,
|
||||
min_ei,
|
||||
max_ei,
|
||||
min_ed,
|
||||
max_ed,
|
||||
min_signal,
|
||||
max_signal,
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn simple_new(kp: f64, ki: f64, kd: f64) -> Self {
|
||||
Self {
|
||||
kp,
|
||||
ki,
|
||||
kd,
|
||||
ep: 0.0,
|
||||
ei: 0.0,
|
||||
ed: 0.0,
|
||||
min_ep: None,
|
||||
max_ep: None,
|
||||
min_ei: None,
|
||||
max_ei: None,
|
||||
min_ed: None,
|
||||
max_ed: None,
|
||||
last: None,
|
||||
min_signal: None,
|
||||
max_signal: None,
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn get_kp(&self) -> f64 {
|
||||
self.kp
|
||||
}
|
||||
|
||||
pub const fn get_ki(&self) -> f64 {
|
||||
self.ki
|
||||
}
|
||||
|
||||
pub const fn get_kd(&self) -> f64 {
|
||||
self.kd
|
||||
}
|
||||
|
||||
pub const fn configure(&mut self, ki: f64, kp: f64, kd: f64) {
|
||||
self.reset();
|
||||
self.kp = kp;
|
||||
self.ki = ki;
|
||||
self.kd = kd;
|
||||
}
|
||||
|
||||
pub const fn optional_clamp(value: f64, min: Option<f64>, max: Option<f64>) -> f64 {
|
||||
match (min, max) {
|
||||
(Some(min), Some(max)) => value.clamp(min, max),
|
||||
(Some(min), None) => value.max(min),
|
||||
(None, Some(max)) => value.min(max),
|
||||
(None, None) => value,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update(&mut self, error: f64, t: Instant) -> f64 {
|
||||
match self.last {
|
||||
// First update
|
||||
None => {
|
||||
// Calculate error
|
||||
let ep = error;
|
||||
|
||||
// Calculate signal
|
||||
let signal = self.kp * ep;
|
||||
let clamped_signal = Self::optional_clamp(signal, self.min_signal, self.max_signal);
|
||||
|
||||
// Set values
|
||||
self.ep = Self::optional_clamp(ep, self.min_ep, self.max_ep);
|
||||
self.ei = 0.0;
|
||||
self.ed = 0.0;
|
||||
self.last = Some(t);
|
||||
|
||||
clamped_signal
|
||||
}
|
||||
// Subsequent updates
|
||||
Some(last) => {
|
||||
// Calculate the time delta in seconds
|
||||
let dt = t.duration_since(last).as_secs_f64();
|
||||
|
||||
// Calculate errors
|
||||
let ep = Self::optional_clamp(error, self.min_ep, self.max_ep);
|
||||
|
||||
let ei = Self::optional_clamp(ep.mul_add(dt, self.ei), self.min_ei, self.max_ei);
|
||||
|
||||
let ed = Self::optional_clamp((ep - self.ep) / dt, self.min_ed, self.max_ed);
|
||||
|
||||
// Make factors timeagnostic
|
||||
let kp = self.kp * dt;
|
||||
let ki = self.ki * dt;
|
||||
let kd = self.kd * dt;
|
||||
|
||||
// Calculate signal
|
||||
let signal = kd.mul_add(ed, kp.mul_add(ep, ki * ei));
|
||||
let clamped_signal = Self::optional_clamp(signal, self.min_signal, self.max_signal);
|
||||
|
||||
// Set values
|
||||
self.ep = ep;
|
||||
self.ei = ei;
|
||||
self.ed = ed;
|
||||
self.last = Some(t);
|
||||
|
||||
clamped_signal
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn reset(&mut self) {
|
||||
self.ep = 0.0;
|
||||
self.ei = 0.0;
|
||||
self.ed = 0.0;
|
||||
self.last = None;
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use crate::controllers::clamping_timeagnostic_pid::ClampingTimeagnosticPidController;
|
||||
|
||||
#[test]
|
||||
fn test_optional_clamp_with_both_bounds() {
|
||||
let val = 5.0;
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, Some(1.0), Some(4.0));
|
||||
assert_eq!(clamped, 4.0);
|
||||
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, Some(6.0), Some(10.0));
|
||||
assert_eq!(clamped, 6.0);
|
||||
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, Some(2.0), Some(6.0));
|
||||
assert_eq!(clamped, 5.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optional_clamp_with_only_min() {
|
||||
let val = 3.0;
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, Some(5.0), None);
|
||||
assert_eq!(clamped, 5.0);
|
||||
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, Some(2.0), None);
|
||||
assert_eq!(clamped, 3.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optional_clamp_with_only_max() {
|
||||
let val = 7.0;
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, None, Some(6.0));
|
||||
assert_eq!(clamped, 6.0);
|
||||
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, None, Some(8.0));
|
||||
assert_eq!(clamped, 7.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_optional_clamp_with_no_bounds() {
|
||||
let val = 42.0;
|
||||
let clamped = ClampingTimeagnosticPidController::optional_clamp(val, None, None);
|
||||
assert_eq!(clamped, 42.0);
|
||||
}
|
||||
}
|
||||
+28
-176
@@ -2,17 +2,11 @@ use std::time::Instant;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct AccelerationSpeedController {
|
||||
/// Maximum deceleration in units per second (positive value)
|
||||
max_acceleration: f64,
|
||||
/// Maximum acceleration in units per second (positive value)
|
||||
acceleration: f64,
|
||||
|
||||
/// Maximum acceleratoin in units per second (negative value)
|
||||
min_acceleration: f64,
|
||||
|
||||
/// Minimum speed limit (None for no limit)
|
||||
min_speed: Option<f64>,
|
||||
|
||||
/// Maximum speed limit (None for no limit)
|
||||
max_speed: Option<f64>,
|
||||
/// Maximum deceleration in units per second (negative value)
|
||||
deceleration: f64,
|
||||
|
||||
/// Calculated speed at the last update
|
||||
last_speed: f64,
|
||||
@@ -22,36 +16,15 @@ pub struct AccelerationSpeedController {
|
||||
}
|
||||
|
||||
impl AccelerationSpeedController {
|
||||
pub const fn new(
|
||||
min_speed: Option<f64>,
|
||||
max_speed: Option<f64>,
|
||||
min_acceleration: f64,
|
||||
max_acceleration: f64,
|
||||
initial_speed: f64,
|
||||
) -> Self {
|
||||
pub fn new(acceleration: f64, deceleration: f64, initial_speed: f64) -> Self {
|
||||
Self {
|
||||
min_acceleration,
|
||||
max_acceleration,
|
||||
min_speed,
|
||||
max_speed,
|
||||
acceleration,
|
||||
deceleration,
|
||||
last_speed: initial_speed,
|
||||
last_t: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Creates a new acceleration speed controller with simplified parameters.
|
||||
/// Sets min_acceleration to -max_acceleration for symmetric behavior.
|
||||
/// No speed limits are applied.
|
||||
pub fn new_simple(max_acceleration: f64, initial_speed: f64) -> Self {
|
||||
Self::new(
|
||||
None, // min_speed
|
||||
None, // max_speed
|
||||
-max_acceleration, // min_acceleration (deceleration)
|
||||
max_acceleration, // max_acceleration
|
||||
initial_speed,
|
||||
)
|
||||
}
|
||||
|
||||
pub fn update(&mut self, target_speed: f64, t: Instant) -> f64 {
|
||||
// Calculate time delta
|
||||
let dt = match self.last_t {
|
||||
@@ -68,10 +41,10 @@ impl AccelerationSpeedController {
|
||||
// Get acceleration
|
||||
let acceleration = if target_speed > self.last_speed {
|
||||
// We are accelerating
|
||||
self.max_acceleration
|
||||
self.acceleration
|
||||
} else if target_speed < self.last_speed {
|
||||
// We are decelerating
|
||||
self.min_acceleration
|
||||
self.deceleration
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
@@ -90,55 +63,22 @@ impl AccelerationSpeedController {
|
||||
new_speed
|
||||
};
|
||||
|
||||
// Apply speed limits
|
||||
let new_speed = self.apply_speed_limits(new_speed);
|
||||
|
||||
self.last_speed = new_speed;
|
||||
|
||||
new_speed
|
||||
}
|
||||
|
||||
pub const fn reset(&mut self, initial_speed: f64) {
|
||||
pub fn reset(&mut self, initial_speed: f64) {
|
||||
self.last_speed = initial_speed;
|
||||
self.last_t = None; // Reset the last update time
|
||||
}
|
||||
|
||||
pub const fn set_max_acceleration(&mut self, acceleration: f64) {
|
||||
self.max_acceleration = acceleration;
|
||||
pub fn set_acceleration(&mut self, acceleration: f64) {
|
||||
self.acceleration = acceleration;
|
||||
}
|
||||
|
||||
const fn apply_speed_limits(&self, speed: f64) -> f64 {
|
||||
let mut limited_speed = speed;
|
||||
|
||||
if let Some(min) = self.min_speed {
|
||||
limited_speed = limited_speed.max(min);
|
||||
}
|
||||
|
||||
if let Some(max) = self.max_speed {
|
||||
limited_speed = limited_speed.min(max);
|
||||
}
|
||||
|
||||
limited_speed
|
||||
}
|
||||
|
||||
pub const fn get_min_speed(&self) -> Option<f64> {
|
||||
self.min_speed
|
||||
}
|
||||
|
||||
pub const fn get_max_speed(&self) -> Option<f64> {
|
||||
self.max_speed
|
||||
}
|
||||
|
||||
pub const fn set_min_speed(&mut self, min_speed: Option<f64>) {
|
||||
self.min_speed = min_speed;
|
||||
}
|
||||
|
||||
pub const fn set_max_speed(&mut self, max_speed: Option<f64>) {
|
||||
self.max_speed = max_speed;
|
||||
}
|
||||
|
||||
pub const fn set_min_acceleration(&mut self, deceleration: f64) {
|
||||
self.min_acceleration = deceleration;
|
||||
pub fn set_deceleration(&mut self, deceleration: f64) {
|
||||
self.deceleration = deceleration;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -157,16 +97,16 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_initialization() {
|
||||
let controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 5.0);
|
||||
assert_relative_eq!(controller.max_acceleration, 10.0, epsilon = EPSILON);
|
||||
assert_relative_eq!(controller.min_acceleration, -15.0, epsilon = EPSILON);
|
||||
let controller = AccelerationSpeedController::new(10.0, -15.0, 5.0);
|
||||
assert_relative_eq!(controller.acceleration, 10.0, epsilon = EPSILON);
|
||||
assert_relative_eq!(controller.deceleration, -15.0, epsilon = EPSILON);
|
||||
assert_relative_eq!(controller.last_speed, 5.0, epsilon = EPSILON);
|
||||
assert!(controller.last_t.is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_first_update() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 5.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -15.0, 5.0);
|
||||
let now = Instant::now();
|
||||
|
||||
let speed = controller.update(5.0, now);
|
||||
@@ -178,7 +118,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_acceleration() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 0.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -15.0, 0.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(0.0, t1); // Initialize last_t
|
||||
|
||||
@@ -197,7 +137,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_deceleration() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 10.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -15.0, 10.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(10.0, t1); // Initialize last_t
|
||||
|
||||
@@ -216,7 +156,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_constant_speed() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 7.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -15.0, 7.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(7.0, t1); // Initialize last_t
|
||||
|
||||
@@ -229,7 +169,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_acceleration_limit() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 0.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -15.0, 0.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(0.0, t1); // Initialize last_t
|
||||
|
||||
@@ -246,7 +186,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_deceleration_limit() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 20.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -15.0, 20.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(20.0, t1); // Initialize last_t
|
||||
|
||||
@@ -263,7 +203,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_zero_time_delta() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 5.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -15.0, 5.0);
|
||||
let now = Instant::now();
|
||||
|
||||
controller.update(5.0, now); // Initialize last_t
|
||||
@@ -276,7 +216,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_acceleration_capped_at_target() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 100.0, 3.0);
|
||||
let mut controller = AccelerationSpeedController::new(100.0, -15.0, 3.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(3.0, t1); // Initialize last_t
|
||||
|
||||
@@ -294,7 +234,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_deceleration_capped_at_target() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -100.0, 10.0, 8.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -100.0, 8.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(8.0, t1); // Initialize last_t
|
||||
|
||||
@@ -312,7 +252,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_exact_acceleration_to_target() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 20.0, 3.0);
|
||||
let mut controller = AccelerationSpeedController::new(20.0, -15.0, 3.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(3.0, t1); // Initialize last_t
|
||||
|
||||
@@ -329,7 +269,7 @@ mod tests {
|
||||
|
||||
#[test]
|
||||
fn test_exact_deceleration_to_target() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -30.0, 10.0, 8.0);
|
||||
let mut controller = AccelerationSpeedController::new(10.0, -30.0, 8.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(8.0, t1); // Initialize last_t
|
||||
|
||||
@@ -343,92 +283,4 @@ mod tests {
|
||||
|
||||
assert_relative_eq!(speed, 5.0, epsilon = EPSILON);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_speed_limits_initialization() {
|
||||
let controller =
|
||||
AccelerationSpeedController::new(Some(-50.0), Some(100.0), -10.0, 20.0, 5.0);
|
||||
assert_eq!(controller.get_min_speed(), Some(-50.0));
|
||||
assert_eq!(controller.get_max_speed(), Some(100.0));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_max_speed_limit() {
|
||||
let mut controller = AccelerationSpeedController::new(None, Some(50.0), -10.0, 100.0, 0.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(0.0, t1); // Initialize last_t
|
||||
|
||||
let dt = 1.0; // 1 second
|
||||
let t2 = future_instant(t1, dt);
|
||||
|
||||
// Try to reach 80.0, but should be limited to 50.0
|
||||
let speed = controller.update(80.0, t2);
|
||||
assert_eq!(speed, 50.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_min_speed_limit() {
|
||||
let mut controller = AccelerationSpeedController::new(Some(-30.0), None, -100.0, 10.0, 0.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(0.0, t1); // Initialize last_t
|
||||
|
||||
let dt = 1.0; // 1 second
|
||||
let t2 = future_instant(t1, dt);
|
||||
|
||||
// Try to reach -50.0, but should be limited to -30.0
|
||||
let speed = controller.update(-50.0, t2);
|
||||
assert_eq!(speed, -30.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_speed_within_limits() {
|
||||
let mut controller =
|
||||
AccelerationSpeedController::new(Some(-50.0), Some(50.0), -20.0, 20.0, 0.0);
|
||||
let t1 = Instant::now();
|
||||
controller.update(0.0, t1); // Initialize last_t
|
||||
|
||||
let dt = 0.5; // 0.5 seconds
|
||||
let t2 = future_instant(t1, dt);
|
||||
|
||||
// Target 10.0, with acceleration 20.0, should reach 10.0 in 0.5s
|
||||
let speed = controller.update(10.0, t2);
|
||||
assert_relative_eq!(speed, 10.0, epsilon = EPSILON);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_speed_limit_setters() {
|
||||
let mut controller = AccelerationSpeedController::new(None, None, -15.0, 10.0, 5.0);
|
||||
|
||||
// Initially no limits
|
||||
assert_eq!(controller.get_min_speed(), None);
|
||||
assert_eq!(controller.get_max_speed(), None);
|
||||
|
||||
// Set limits
|
||||
controller.set_min_speed(Some(-100.0));
|
||||
controller.set_max_speed(Some(100.0));
|
||||
|
||||
assert_eq!(controller.get_min_speed(), Some(-100.0));
|
||||
assert_eq!(controller.get_max_speed(), Some(100.0));
|
||||
|
||||
// Remove limits
|
||||
controller.set_min_speed(None);
|
||||
controller.set_max_speed(None);
|
||||
|
||||
assert_eq!(controller.get_min_speed(), None);
|
||||
assert_eq!(controller.get_max_speed(), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_new_simple_constructor() {
|
||||
let controller = AccelerationSpeedController::new_simple(20.0, 5.0);
|
||||
|
||||
// Check that acceleration values are symmetric
|
||||
assert_eq!(controller.min_acceleration, -20.0);
|
||||
assert_eq!(controller.max_acceleration, 20.0);
|
||||
assert_eq!(controller.last_speed, 5.0);
|
||||
|
||||
// Check that speed limits are None
|
||||
assert_eq!(controller.get_min_speed(), None);
|
||||
assert_eq!(controller.get_max_speed(), None);
|
||||
}
|
||||
}
|
||||
+9
-51
@@ -17,43 +17,23 @@ pub struct AngularAccelerationSpeedController {
|
||||
|
||||
impl AngularAccelerationSpeedController {
|
||||
pub fn new(
|
||||
min_speed: Option<AngularVelocity>,
|
||||
max_speed: Option<AngularVelocity>,
|
||||
min_acceleration: AngularAcceleration,
|
||||
max_acceleration: AngularAcceleration,
|
||||
acceleration: AngularAcceleration,
|
||||
deceleration: AngularAcceleration,
|
||||
initial_speed: AngularVelocity,
|
||||
) -> Self {
|
||||
Self {
|
||||
controller: AccelerationSpeedController::new(
|
||||
min_speed.map(|s| s.get::<radian_per_second>()),
|
||||
max_speed.map(|s| s.get::<radian_per_second>()),
|
||||
min_acceleration.get::<radian_per_second_squared>(),
|
||||
max_acceleration.get::<radian_per_second_squared>(),
|
||||
acceleration.get::<radian_per_second_squared>(),
|
||||
deceleration.get::<radian_per_second_squared>(),
|
||||
initial_speed.get::<radian_per_second>(),
|
||||
),
|
||||
}
|
||||
}
|
||||
|
||||
/// Creates a new angular acceleration speed controller with simplified parameters.
|
||||
/// Sets min_acceleration to -max_acceleration for symmetric behavior.
|
||||
/// No speed limits are applied.
|
||||
pub fn new_simple(
|
||||
max_acceleration: AngularAcceleration,
|
||||
initial_speed: AngularVelocity,
|
||||
) -> Self {
|
||||
Self::new(
|
||||
None, // min_speed
|
||||
None, // max_speed
|
||||
-max_acceleration, // min_acceleration (deceleration)
|
||||
max_acceleration, // max_acceleration
|
||||
initial_speed,
|
||||
)
|
||||
}
|
||||
|
||||
pub fn update(&mut self, target_speed: AngularVelocity, t: Instant) -> AngularVelocity {
|
||||
let target_speed = target_speed.get::<radian_per_second>();
|
||||
let new_speed = self.controller.update(target_speed, t);
|
||||
AngularVelocity::new::<radian_per_second>(new_speed)
|
||||
return AngularVelocity::new::<radian_per_second>(new_speed);
|
||||
}
|
||||
|
||||
pub fn reset(&mut self, initial_speed: AngularVelocity) {
|
||||
@@ -61,34 +41,12 @@ impl AngularAccelerationSpeedController {
|
||||
self.controller.reset(initial_speed);
|
||||
}
|
||||
|
||||
pub fn set_max_acceleration(&mut self, acceleration: AngularAcceleration) {
|
||||
pub fn set_acceleration(&mut self, acceleration: AngularAcceleration) {
|
||||
self.controller
|
||||
.set_max_acceleration(acceleration.get::<radian_per_second_squared>());
|
||||
.set_acceleration(acceleration.get::<radian_per_second_squared>());
|
||||
}
|
||||
pub fn set_min_acceleration(&mut self, deceleration: AngularAcceleration) {
|
||||
pub fn set_deceleration(&mut self, deceleration: AngularAcceleration) {
|
||||
self.controller
|
||||
.set_min_acceleration(deceleration.get::<radian_per_second_squared>());
|
||||
}
|
||||
|
||||
pub fn get_min_speed(&self) -> Option<AngularVelocity> {
|
||||
self.controller
|
||||
.get_min_speed()
|
||||
.map(AngularVelocity::new::<radian_per_second>)
|
||||
}
|
||||
|
||||
pub fn get_max_speed(&self) -> Option<AngularVelocity> {
|
||||
self.controller
|
||||
.get_max_speed()
|
||||
.map(AngularVelocity::new::<radian_per_second>)
|
||||
}
|
||||
|
||||
pub fn set_min_speed(&mut self, min_speed: Option<AngularVelocity>) {
|
||||
self.controller
|
||||
.set_min_speed(min_speed.map(|s| s.get::<radian_per_second>()));
|
||||
}
|
||||
|
||||
pub fn set_max_speed(&mut self, max_speed: Option<AngularVelocity>) {
|
||||
self.controller
|
||||
.set_max_speed(max_speed.map(|s| s.get::<radian_per_second>()));
|
||||
.set_deceleration(deceleration.get::<radian_per_second_squared>());
|
||||
}
|
||||
}
|
||||
+3
-37
@@ -17,38 +17,22 @@ pub struct LinearAccelerationLimitingController {
|
||||
|
||||
impl LinearAccelerationLimitingController {
|
||||
pub fn new(
|
||||
min_speed: Option<Velocity>,
|
||||
max_speed: Option<Velocity>,
|
||||
acceleration: Acceleration,
|
||||
deceleration: Acceleration,
|
||||
initial_speed: Velocity,
|
||||
) -> Self {
|
||||
Self {
|
||||
controller: AccelerationSpeedController::new(
|
||||
min_speed.map(|s| s.get::<meter_per_second>()),
|
||||
max_speed.map(|s| s.get::<meter_per_second>()),
|
||||
acceleration.get::<meter_per_second_squared>(),
|
||||
deceleration.get::<meter_per_second_squared>(),
|
||||
initial_speed.get::<meter_per_second>(),
|
||||
),
|
||||
}
|
||||
}
|
||||
/// Creates a new linear acceleration speed controller with simplified parameters.
|
||||
/// Sets deceleration to -acceleration for symmetric behavior.
|
||||
/// No speed limits are applied.
|
||||
pub fn new_simple(acceleration: Acceleration, initial_speed: Velocity) -> Self {
|
||||
Self::new(
|
||||
None, // min_speed
|
||||
None, // max_speed
|
||||
acceleration, // acceleration
|
||||
-acceleration, // deceleration (negative for deceleration)
|
||||
initial_speed,
|
||||
)
|
||||
}
|
||||
pub fn update(&mut self, target_speed: Velocity, t: Instant) -> Velocity {
|
||||
let target_speed = target_speed.get::<meter_per_second>();
|
||||
let new_speed = self.controller.update(target_speed, t);
|
||||
Velocity::new::<meter_per_second>(new_speed)
|
||||
return Velocity::new::<meter_per_second>(new_speed);
|
||||
}
|
||||
pub fn reset(&mut self, initial_speed: Velocity) {
|
||||
let initial_speed = initial_speed.get::<meter_per_second>();
|
||||
@@ -56,28 +40,10 @@ impl LinearAccelerationLimitingController {
|
||||
}
|
||||
pub fn set_acceleration(&mut self, acceleration: Acceleration) {
|
||||
self.controller
|
||||
.set_max_acceleration(acceleration.get::<meter_per_second_squared>());
|
||||
.set_acceleration(acceleration.get::<meter_per_second_squared>());
|
||||
}
|
||||
pub fn set_deceleration(&mut self, deceleration: Acceleration) {
|
||||
self.controller
|
||||
.set_min_acceleration(deceleration.get::<meter_per_second_squared>());
|
||||
}
|
||||
pub fn get_min_speed(&self) -> Option<Velocity> {
|
||||
self.controller
|
||||
.get_min_speed()
|
||||
.map(Velocity::new::<meter_per_second>)
|
||||
}
|
||||
pub fn get_max_speed(&self) -> Option<Velocity> {
|
||||
self.controller
|
||||
.get_max_speed()
|
||||
.map(Velocity::new::<meter_per_second>)
|
||||
}
|
||||
pub fn set_min_speed(&mut self, min_speed: Option<Velocity>) {
|
||||
self.controller
|
||||
.set_min_speed(min_speed.map(|s| s.get::<meter_per_second>()));
|
||||
}
|
||||
pub fn set_max_speed(&mut self, max_speed: Option<Velocity>) {
|
||||
self.controller
|
||||
.set_max_speed(max_speed.map(|s| s.get::<meter_per_second>()));
|
||||
.set_deceleration(deceleration.get::<meter_per_second_squared>());
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,3 @@
|
||||
pub mod clamping_timeagnostic_pid;
|
||||
pub mod first_degree_motion;
|
||||
pub mod pid;
|
||||
pub mod second_degree_motion;
|
||||
@@ -9,6 +9,7 @@ pub struct PidController {
|
||||
ki: f64,
|
||||
/// Derivative gain
|
||||
kd: f64,
|
||||
|
||||
// State
|
||||
/// Proportional error
|
||||
ep: f64,
|
||||
@@ -21,7 +22,7 @@ pub struct PidController {
|
||||
}
|
||||
|
||||
impl PidController {
|
||||
pub const fn new(kp: f64, ki: f64, kd: f64) -> Self {
|
||||
pub fn new(kp: f64, ki: f64, kd: f64) -> Self {
|
||||
Self {
|
||||
kp,
|
||||
ki,
|
||||
@@ -33,27 +34,8 @@ impl PidController {
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn configure(&mut self, ki: f64, kp: f64, kd: f64) {
|
||||
self.reset();
|
||||
self.kp = kp;
|
||||
self.ki = ki;
|
||||
self.kd = kd;
|
||||
}
|
||||
|
||||
pub const fn get_kp(&self) -> f64 {
|
||||
self.kp
|
||||
}
|
||||
|
||||
pub const fn get_ki(&self) -> f64 {
|
||||
self.ki
|
||||
}
|
||||
|
||||
pub const fn get_kd(&self) -> f64 {
|
||||
self.kd
|
||||
}
|
||||
|
||||
pub fn update(&mut self, error: f64, t: Instant) -> f64 {
|
||||
match self.last {
|
||||
let signal = match self.last {
|
||||
// First update
|
||||
None => {
|
||||
// Calculate error
|
||||
@@ -65,7 +47,6 @@ impl PidController {
|
||||
// Set values
|
||||
self.ep = ep;
|
||||
self.ei = 0.0;
|
||||
|
||||
self.ed = 0.0;
|
||||
self.last = Some(t);
|
||||
|
||||
@@ -78,11 +59,11 @@ impl PidController {
|
||||
|
||||
// Calculate errors
|
||||
let ep = error;
|
||||
let ei = ep.mul_add(dt, self.ei);
|
||||
let ei = self.ei + ep * dt;
|
||||
let ed = (ep - self.ep) / dt;
|
||||
|
||||
// Calculate signal
|
||||
let signal = self.kd.mul_add(ed, self.kp.mul_add(ep, self.ki * ei));
|
||||
let signal = self.kp * ep + self.ki * ei + self.kd * ed;
|
||||
|
||||
// Set values
|
||||
self.ep = ep;
|
||||
@@ -92,10 +73,12 @@ impl PidController {
|
||||
|
||||
signal
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
signal
|
||||
}
|
||||
|
||||
pub const fn reset(&mut self) {
|
||||
pub fn reset(&mut self) {
|
||||
self.ep = 0.0;
|
||||
self.ei = 0.0;
|
||||
self.ed = 0.0;
|
||||
|
||||
+46
-58
@@ -3,7 +3,7 @@ use std::fmt;
|
||||
// Remove the anyhow::Ok import as it conflicts with std::result::Ok
|
||||
|
||||
/// Errors that can occur when creating or using the motion controller
|
||||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub enum MotionControllerError {
|
||||
InvalidSpeedLimits,
|
||||
InvalidAccelerationLimits,
|
||||
@@ -14,25 +14,25 @@ pub enum MotionControllerError {
|
||||
impl fmt::Display for MotionControllerError {
|
||||
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
|
||||
match self {
|
||||
Self::InvalidSpeedLimits => {
|
||||
MotionControllerError::InvalidSpeedLimits => {
|
||||
write!(
|
||||
f,
|
||||
"Invalid speed limits: min_speed must be ≤ 0 and max_speed must be ≥ 0"
|
||||
)
|
||||
}
|
||||
Self::InvalidAccelerationLimits => {
|
||||
MotionControllerError::InvalidAccelerationLimits => {
|
||||
write!(
|
||||
f,
|
||||
"Invalid acceleration limits: min_acceleration must be < 0 and max_acceleration must be > 0"
|
||||
)
|
||||
}
|
||||
Self::InvalidPositionLimits => {
|
||||
MotionControllerError::InvalidPositionLimits => {
|
||||
write!(
|
||||
f,
|
||||
"Invalid position limits: min_position must be ≤ max_position"
|
||||
)
|
||||
}
|
||||
Self::ZeroDeceleration => {
|
||||
MotionControllerError::ZeroDeceleration => {
|
||||
write!(
|
||||
f,
|
||||
"Zero deceleration rate: cannot plan motion without deceleration capability"
|
||||
@@ -148,7 +148,7 @@ struct MotionConstants {
|
||||
}
|
||||
|
||||
/// Represents the current phase of motion
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum MotionPhase {
|
||||
Idle,
|
||||
IncreasingSpeed,
|
||||
@@ -170,7 +170,7 @@ pub struct ControllerBuilder {
|
||||
|
||||
impl ControllerBuilder {
|
||||
/// Create a new builder
|
||||
pub const fn new() -> Self {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
min_speed: None,
|
||||
max_speed: None,
|
||||
@@ -184,36 +184,28 @@ impl ControllerBuilder {
|
||||
}
|
||||
|
||||
/// Set speed limits (min_speed ≤ 0, max_speed ≥ 0)
|
||||
pub const fn speed_limits(mut self, min_speed: f64, max_speed: f64) -> Self {
|
||||
pub fn speed_limits(mut self, min_speed: f64, max_speed: f64) -> Self {
|
||||
self.min_speed = Some(min_speed);
|
||||
self.max_speed = Some(max_speed);
|
||||
self
|
||||
}
|
||||
|
||||
/// Set acceleration limits (min_acceleration < 0, max_acceleration > 0)
|
||||
pub const fn acceleration_limits(
|
||||
mut self,
|
||||
min_acceleration: f64,
|
||||
max_acceleration: f64,
|
||||
) -> Self {
|
||||
pub fn acceleration_limits(mut self, min_acceleration: f64, max_acceleration: f64) -> Self {
|
||||
self.min_acceleration = Some(min_acceleration);
|
||||
self.max_acceleration = Some(max_acceleration);
|
||||
self
|
||||
}
|
||||
|
||||
/// Set position limits (optional)
|
||||
pub const fn position_limits(
|
||||
mut self,
|
||||
min_position: Option<f64>,
|
||||
max_position: Option<f64>,
|
||||
) -> Self {
|
||||
pub fn position_limits(mut self, min_position: Option<f64>, max_position: Option<f64>) -> Self {
|
||||
self.min_position = min_position;
|
||||
self.max_position = max_position;
|
||||
self
|
||||
}
|
||||
|
||||
/// Set tolerances for position and speed comparisons
|
||||
pub const fn tolerances(mut self, position_tolerance: f64, speed_tolerance: f64) -> Self {
|
||||
pub fn tolerances(mut self, position_tolerance: f64, speed_tolerance: f64) -> Self {
|
||||
self.position_tolerance = position_tolerance;
|
||||
self.speed_tolerance = speed_tolerance;
|
||||
self
|
||||
@@ -330,7 +322,7 @@ impl AccelerationPositionController {
|
||||
|
||||
let motion_constants = Self::calculate_motion_constants(&config);
|
||||
|
||||
Ok(Self {
|
||||
Ok(AccelerationPositionController {
|
||||
config,
|
||||
current_position: 0.0,
|
||||
current_speed: 0.0,
|
||||
@@ -374,19 +366,19 @@ impl AccelerationPositionController {
|
||||
}
|
||||
|
||||
Self::new(
|
||||
-speed, // min_speed
|
||||
speed, // max_speed
|
||||
-acceleration, // min_acceleration
|
||||
acceleration, // max_acceleration
|
||||
position.map(|p| -p.abs()), // min_position
|
||||
position.map(|p| p.abs()), // max_position
|
||||
1e-6, // position_tolerance (default)
|
||||
1e-6, // speed_tolerance (default)
|
||||
-speed, // min_speed
|
||||
speed, // max_speed
|
||||
-acceleration, // min_acceleration
|
||||
acceleration, // max_acceleration
|
||||
position.map(|p| -p), // min_position
|
||||
position.map(|p| p), // max_position
|
||||
1e-6, // position_tolerance (default)
|
||||
1e-6, // speed_tolerance (default)
|
||||
)
|
||||
}
|
||||
|
||||
/// Create a new builder for the controller
|
||||
pub const fn builder() -> ControllerBuilder {
|
||||
pub fn builder() -> ControllerBuilder {
|
||||
ControllerBuilder::new()
|
||||
}
|
||||
|
||||
@@ -572,10 +564,8 @@ impl AccelerationPositionController {
|
||||
|
||||
// Calculate distance needed to reach cruise speed from current speed
|
||||
let speed_change_distance = if current_speed_in_direction < cruise_speed.abs() {
|
||||
current_speed_in_direction.max(0.0).mul_add(
|
||||
-current_speed_in_direction.max(0.0),
|
||||
cruise_speed.abs().powi(2),
|
||||
) / (2.0 * acceleration_rate)
|
||||
(cruise_speed.abs().powi(2) - current_speed_in_direction.max(0.0).powi(2))
|
||||
/ (2.0 * acceleration_rate)
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
@@ -589,10 +579,8 @@ impl AccelerationPositionController {
|
||||
// Using corrected kinematic equation: v_peak² = v_current² + 2*a*d_total*a_accel*a_decel/(a_accel + a_decel)
|
||||
let accel_decel_factor =
|
||||
(acceleration_rate * deceleration_rate) / (acceleration_rate + deceleration_rate);
|
||||
let discriminant = current_speed_in_direction.max(0.0).mul_add(
|
||||
current_speed_in_direction.max(0.0),
|
||||
2.0 * abs_position_change * accel_decel_factor,
|
||||
);
|
||||
let discriminant = current_speed_in_direction.max(0.0).powi(2)
|
||||
+ 2.0 * abs_position_change * accel_decel_factor;
|
||||
|
||||
if discriminant >= 0.0 {
|
||||
self.peak_speed = discriminant.sqrt() * cruise_speed.signum();
|
||||
@@ -646,7 +634,7 @@ impl AccelerationPositionController {
|
||||
self.current_acceleration = self.config.max_acceleration * self.direction as f64;
|
||||
|
||||
// Update speed
|
||||
let new_speed = self.current_acceleration.mul_add(dt, self.current_speed);
|
||||
let new_speed = self.current_speed + self.current_acceleration * dt;
|
||||
|
||||
// Check if we've reached peak speed
|
||||
if (self.direction > 0 && new_speed >= self.peak_speed)
|
||||
@@ -677,7 +665,7 @@ impl AccelerationPositionController {
|
||||
self.current_acceleration = self.config.min_acceleration * self.direction as f64;
|
||||
|
||||
// Update speed
|
||||
let new_speed = self.current_acceleration.mul_add(dt, self.current_speed);
|
||||
let new_speed = self.current_speed + self.current_acceleration * dt;
|
||||
|
||||
// Check if we've reached zero speed or target
|
||||
if (self.current_speed > 0.0 && new_speed <= 0.0)
|
||||
@@ -701,7 +689,7 @@ impl AccelerationPositionController {
|
||||
}
|
||||
|
||||
// Update position based on current speed
|
||||
let new_position = self.current_speed.mul_add(dt, self.current_position);
|
||||
let new_position = self.current_position + self.current_speed * dt;
|
||||
|
||||
// Check position limits and handle violations
|
||||
let mut position_limited = false;
|
||||
@@ -754,7 +742,7 @@ impl AccelerationPositionController {
|
||||
///
|
||||
/// Returns the current position of the controlled object.
|
||||
/// This value is updated with each call to `update()`.
|
||||
pub const fn get_position(&self) -> f64 {
|
||||
pub fn get_position(&self) -> f64 {
|
||||
self.current_position
|
||||
}
|
||||
|
||||
@@ -763,7 +751,7 @@ impl AccelerationPositionController {
|
||||
/// Returns the current speed (velocity) of the controlled object.
|
||||
/// Positive values indicate motion towards increasing position,
|
||||
/// negative values indicate motion towards decreasing position.
|
||||
pub const fn get_speed(&self) -> f64 {
|
||||
pub fn get_speed(&self) -> f64 {
|
||||
self.current_speed
|
||||
}
|
||||
|
||||
@@ -771,32 +759,32 @@ impl AccelerationPositionController {
|
||||
///
|
||||
/// Returns the current acceleration of the controlled object.
|
||||
/// Positive values indicate increasing speed, negative values indicate decreasing speed.
|
||||
pub const fn get_acceleration(&self) -> f64 {
|
||||
pub fn get_acceleration(&self) -> f64 {
|
||||
self.current_acceleration
|
||||
}
|
||||
|
||||
/// Get the target position
|
||||
pub const fn get_target_position(&self) -> f64 {
|
||||
pub fn get_target_position(&self) -> f64 {
|
||||
self.target_position
|
||||
}
|
||||
|
||||
/// Get the current motion phase
|
||||
pub const fn get_motion_phase(&self) -> MotionPhase {
|
||||
pub fn get_motion_phase(&self) -> MotionPhase {
|
||||
self.motion_phase
|
||||
}
|
||||
|
||||
/// Get the direction of motion
|
||||
pub const fn get_direction(&self) -> i8 {
|
||||
pub fn get_direction(&self) -> i8 {
|
||||
self.direction
|
||||
}
|
||||
|
||||
/// Get the calculated peak speed for current motion
|
||||
pub const fn get_peak_speed(&self) -> f64 {
|
||||
pub fn get_peak_speed(&self) -> f64 {
|
||||
self.peak_speed
|
||||
}
|
||||
|
||||
/// Get the position at which deceleration will begin
|
||||
pub const fn get_deceleration_position(&self) -> f64 {
|
||||
pub fn get_deceleration_position(&self) -> f64 {
|
||||
self.deceleration_position
|
||||
}
|
||||
|
||||
@@ -849,7 +837,7 @@ impl AccelerationPositionController {
|
||||
}
|
||||
|
||||
/// Force the controller to stop at current position
|
||||
pub const fn emergency_stop(&mut self) {
|
||||
pub fn emergency_stop(&mut self) {
|
||||
self.target_position = self.current_position;
|
||||
self.current_speed = 0.0;
|
||||
self.current_acceleration = 0.0;
|
||||
@@ -1018,32 +1006,32 @@ impl AccelerationPositionController {
|
||||
}
|
||||
|
||||
/// Get the minimum speed limit
|
||||
pub const fn get_min_speed(&self) -> f64 {
|
||||
pub fn get_min_speed(&self) -> f64 {
|
||||
self.config.min_speed
|
||||
}
|
||||
|
||||
/// Get the maximum speed limit
|
||||
pub const fn get_max_speed(&self) -> f64 {
|
||||
pub fn get_max_speed(&self) -> f64 {
|
||||
self.config.max_speed
|
||||
}
|
||||
|
||||
/// Get the minimum acceleration limit
|
||||
pub const fn get_min_acceleration(&self) -> f64 {
|
||||
pub fn get_min_acceleration(&self) -> f64 {
|
||||
self.config.min_acceleration
|
||||
}
|
||||
|
||||
/// Get the maximum acceleration limit
|
||||
pub const fn get_max_acceleration(&self) -> f64 {
|
||||
pub fn get_max_acceleration(&self) -> f64 {
|
||||
self.config.max_acceleration
|
||||
}
|
||||
|
||||
/// Get the minimum position limit
|
||||
pub const fn get_min_position(&self) -> Option<f64> {
|
||||
pub fn get_min_position(&self) -> Option<f64> {
|
||||
self.config.min_position
|
||||
}
|
||||
|
||||
/// Get the maximum position limit
|
||||
pub const fn get_max_position(&self) -> Option<f64> {
|
||||
pub fn get_max_position(&self) -> Option<f64> {
|
||||
self.config.max_position
|
||||
}
|
||||
|
||||
@@ -1080,17 +1068,17 @@ impl AccelerationPositionController {
|
||||
}
|
||||
|
||||
/// Get the position tolerance
|
||||
pub const fn get_position_tolerance(&self) -> f64 {
|
||||
pub fn get_position_tolerance(&self) -> f64 {
|
||||
self.config.position_tolerance
|
||||
}
|
||||
|
||||
/// Get the speed tolerance
|
||||
pub const fn get_speed_tolerance(&self) -> f64 {
|
||||
pub fn get_speed_tolerance(&self) -> f64 {
|
||||
self.config.speed_tolerance
|
||||
}
|
||||
|
||||
/// Set tolerances for position and speed comparisons
|
||||
pub const fn set_tolerances(&mut self, position_tolerance: f64, speed_tolerance: f64) {
|
||||
pub fn set_tolerances(&mut self, position_tolerance: f64, speed_tolerance: f64) {
|
||||
self.config.position_tolerance = position_tolerance;
|
||||
self.config.speed_tolerance = speed_tolerance;
|
||||
}
|
||||
|
||||
+6
-2
@@ -102,12 +102,16 @@ impl AngularAccelerationPositionController {
|
||||
|
||||
/// Get the minimum angle position limit
|
||||
pub fn get_min_position(&self) -> Option<Angle> {
|
||||
self.controller.get_min_position().map(Angle::new::<radian>)
|
||||
self.controller
|
||||
.get_min_position()
|
||||
.map(|angle| Angle::new::<radian>(angle))
|
||||
}
|
||||
|
||||
/// Get the maximum angle position limit
|
||||
pub fn get_max_position(&self) -> Option<Angle> {
|
||||
self.controller.get_max_position().map(Angle::new::<radian>)
|
||||
self.controller
|
||||
.get_max_position()
|
||||
.map(|angle| Angle::new::<radian>(angle))
|
||||
}
|
||||
|
||||
/// Set the minimum angle position limit
|
||||
|
||||
@@ -236,7 +236,7 @@ impl AngularJerkSpeedController {
|
||||
pub fn get_min_speed(&self) -> Option<AngularVelocity> {
|
||||
self.controller
|
||||
.get_min_speed()
|
||||
.map(AngularVelocity::new::<radian_per_second>)
|
||||
.map(|speed| AngularVelocity::new::<radian_per_second>(speed))
|
||||
}
|
||||
|
||||
/// Get the maximum angular velocity limit
|
||||
@@ -248,7 +248,7 @@ impl AngularJerkSpeedController {
|
||||
pub fn get_max_speed(&self) -> Option<AngularVelocity> {
|
||||
self.controller
|
||||
.get_max_speed()
|
||||
.map(AngularVelocity::new::<radian_per_second>)
|
||||
.map(|speed| AngularVelocity::new::<radian_per_second>(speed))
|
||||
}
|
||||
|
||||
/// Set the minimum angular velocity limit
|
||||
|
||||
@@ -88,7 +88,7 @@ impl JerkSpeedController {
|
||||
)
|
||||
.expect("Failed to create AccelerationPositionController");
|
||||
|
||||
Self { base_controller }
|
||||
JerkSpeedController { base_controller }
|
||||
}
|
||||
|
||||
/// Create a new speed controller with simple symmetric limits
|
||||
@@ -183,7 +183,7 @@ impl JerkSpeedController {
|
||||
///
|
||||
/// # Returns
|
||||
/// Current speed as a floating-point value
|
||||
pub const fn get_speed(&self) -> f64 {
|
||||
pub fn get_speed(&self) -> f64 {
|
||||
self.base_controller.get_position()
|
||||
}
|
||||
|
||||
@@ -193,7 +193,7 @@ impl JerkSpeedController {
|
||||
///
|
||||
/// # Returns
|
||||
/// Current acceleration as a floating-point value
|
||||
pub const fn get_acceleration(&self) -> f64 {
|
||||
pub fn get_acceleration(&self) -> f64 {
|
||||
self.base_controller.get_speed()
|
||||
}
|
||||
|
||||
@@ -203,7 +203,7 @@ impl JerkSpeedController {
|
||||
///
|
||||
/// # Returns
|
||||
/// Current jerk as a floating-point value
|
||||
pub const fn get_jerk(&self) -> f64 {
|
||||
pub fn get_jerk(&self) -> f64 {
|
||||
self.base_controller.get_acceleration()
|
||||
}
|
||||
|
||||
@@ -213,7 +213,7 @@ impl JerkSpeedController {
|
||||
///
|
||||
/// # Returns
|
||||
/// Target speed as a floating-point value
|
||||
pub const fn get_target_speed(&self) -> f64 {
|
||||
pub fn get_target_speed(&self) -> f64 {
|
||||
self.base_controller.get_target_position()
|
||||
}
|
||||
|
||||
@@ -223,7 +223,7 @@ impl JerkSpeedController {
|
||||
///
|
||||
/// # Returns
|
||||
/// Minimum speed limit as an Option<f64>, None if no limit is set
|
||||
pub const fn get_min_speed(&self) -> Option<f64> {
|
||||
pub fn get_min_speed(&self) -> Option<f64> {
|
||||
self.base_controller.get_min_position()
|
||||
}
|
||||
|
||||
@@ -233,7 +233,7 @@ impl JerkSpeedController {
|
||||
///
|
||||
/// # Returns
|
||||
/// Maximum speed limit as an Option<f64>, None if no limit is set
|
||||
pub const fn get_max_speed(&self) -> Option<f64> {
|
||||
pub fn get_max_speed(&self) -> Option<f64> {
|
||||
self.base_controller.get_max_position()
|
||||
}
|
||||
|
||||
|
||||
+6
-2
@@ -205,7 +205,9 @@ impl LinearAccelerationPositionController {
|
||||
/// # Returns
|
||||
/// Minimum position limit as an Option<Length>, None if no limit is set
|
||||
pub fn get_min_position(&self) -> Option<Length> {
|
||||
self.controller.get_min_position().map(Length::new::<meter>)
|
||||
self.controller
|
||||
.get_min_position()
|
||||
.map(|length| Length::new::<meter>(length))
|
||||
}
|
||||
|
||||
/// Get the maximum position limit
|
||||
@@ -215,7 +217,9 @@ impl LinearAccelerationPositionController {
|
||||
/// # Returns
|
||||
/// Maximum position limit as an Option<Length>, None if no limit is set
|
||||
pub fn get_max_position(&self) -> Option<Length> {
|
||||
self.controller.get_max_position().map(Length::new::<meter>)
|
||||
self.controller
|
||||
.get_max_position()
|
||||
.map(|length| Length::new::<meter>(length))
|
||||
}
|
||||
|
||||
/// Set the minimum position limit
|
||||
|
||||
@@ -195,14 +195,14 @@ impl LinearJerkSpeedController {
|
||||
pub fn get_min_speed(&self) -> Option<Velocity> {
|
||||
self.controller
|
||||
.get_min_speed()
|
||||
.map(Velocity::new::<meter_per_second>)
|
||||
.map(|speed| Velocity::new::<meter_per_second>(speed))
|
||||
}
|
||||
|
||||
/// Get the maximum velocity limit
|
||||
pub fn get_max_speed(&self) -> Option<Velocity> {
|
||||
self.controller
|
||||
.get_max_speed()
|
||||
.map(Velocity::new::<meter_per_second>)
|
||||
.map(|speed| Velocity::new::<meter_per_second>(speed))
|
||||
}
|
||||
|
||||
/// Set the minimum velocity limit
|
||||
|
||||
@@ -1,297 +0,0 @@
|
||||
use uom::si::{angle::radian, f64::Angle};
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct AngleConverter {
|
||||
pub flip_x: bool, // true to flip X-axis (mirror horizontally)
|
||||
pub flip_y: bool, // true to flip Y-axis (mirror vertically)
|
||||
pub is_cw: bool, // true for clockwise, false for counter-clockwise
|
||||
}
|
||||
|
||||
impl AngleConverter {
|
||||
/// Create a custom coordinate system
|
||||
pub const fn new(flip_x: bool, flip_y: bool, is_cw: bool) -> Self {
|
||||
Self {
|
||||
flip_x,
|
||||
flip_y,
|
||||
is_cw,
|
||||
}
|
||||
}
|
||||
|
||||
/// Standard mathematical coordinate system (CCW positive, 0° at positive X)
|
||||
pub const fn mathematical() -> Self {
|
||||
Self::new(false, false, false)
|
||||
}
|
||||
|
||||
/// Screen/graphics coordinate system (CW positive, 0° at positive X, Y-flipped)
|
||||
pub const fn screen() -> Self {
|
||||
Self::new(false, true, false)
|
||||
}
|
||||
|
||||
/// System where 0° points up, clockwise positive
|
||||
pub const fn y_up_cw() -> Self {
|
||||
Self::new(false, false, true)
|
||||
}
|
||||
|
||||
/// System where 0° points down, counter-clockwise positive
|
||||
pub const fn y_down_ccw() -> Self {
|
||||
Self::new(false, true, true)
|
||||
}
|
||||
|
||||
/// System where 0° points left, clockwise positive
|
||||
pub const fn x_left_cw() -> Self {
|
||||
Self::new(true, false, false)
|
||||
}
|
||||
|
||||
/// System where 0° points right, counter-clockwise positive (same as mathematical)
|
||||
pub const fn x_right_ccw() -> Self {
|
||||
Self::mathematical()
|
||||
}
|
||||
|
||||
/// System where 0° points up, counter-clockwise positive
|
||||
pub const fn y_up_ccw() -> Self {
|
||||
Self::new(false, false, false)
|
||||
}
|
||||
|
||||
/// System where 0° points down, clockwise positive
|
||||
pub const fn y_down_cw() -> Self {
|
||||
Self::new(false, true, false)
|
||||
}
|
||||
|
||||
/// System where 0° points left, counter-clockwise positive
|
||||
pub const fn x_left_ccw() -> Self {
|
||||
Self::new(true, false, true)
|
||||
}
|
||||
|
||||
/// Convert angle from mathematical system to this system (f32)
|
||||
pub fn degrees_encode(&self, math_angle_degrees: f64) -> f64 {
|
||||
let normalized_input = self.normalize_angle(math_angle_degrees);
|
||||
|
||||
let mut result = normalized_input;
|
||||
|
||||
// Apply axis flips
|
||||
match (self.flip_x, self.flip_y) {
|
||||
(false, false) => {} // No change
|
||||
(true, false) => result = 180.0 - result, // X-flip: mirror across Y-axis
|
||||
(false, true) => result = -result, // Y-flip: mirror across X-axis
|
||||
(true, true) => result += 180.0, // Both flips: 180° rotation
|
||||
}
|
||||
|
||||
// Handle rotation direction
|
||||
if self.is_cw {
|
||||
result = -result;
|
||||
}
|
||||
|
||||
self.normalize_angle(result)
|
||||
}
|
||||
|
||||
/// Convert angle from this system to mathematical system (f32)
|
||||
pub fn degrees_decode(&self, system_angle_degrees: f64) -> f64 {
|
||||
let normalized_input = self.normalize_angle(system_angle_degrees);
|
||||
|
||||
let mut result = normalized_input;
|
||||
|
||||
// Handle rotation direction
|
||||
if self.is_cw {
|
||||
result = -result;
|
||||
}
|
||||
|
||||
// Apply inverse axis flips
|
||||
match (self.flip_x, self.flip_y) {
|
||||
(false, false) => {} // No change
|
||||
(true, false) => result = 180.0 - result, // X-flip: mirror across Y-axis
|
||||
(false, true) => result = -result, // Y-flip: mirror across X-axis
|
||||
(true, true) => result -= 180.0, // Both flips: inverse 180° rotation
|
||||
}
|
||||
|
||||
self.normalize_angle(result)
|
||||
}
|
||||
|
||||
/// Convert angle in radians from mathematical system to this system (f32)
|
||||
pub fn radians_encode(&self, math_angle_radians: f64) -> f64 {
|
||||
let degrees = math_angle_radians.to_degrees();
|
||||
let converted_degrees = self.degrees_encode(degrees);
|
||||
converted_degrees.to_radians()
|
||||
}
|
||||
|
||||
/// Convert angle in radians from this system to mathematical system (f32)
|
||||
pub fn radians_decode(&self, system_angle_radians: f64) -> f64 {
|
||||
let degrees = system_angle_radians.to_degrees();
|
||||
let converted_degrees = self.degrees_decode(degrees);
|
||||
converted_degrees.to_radians()
|
||||
}
|
||||
|
||||
/// Convert angle from mathematical system to this system (f64)
|
||||
pub fn degrees_encode_f64(&self, math_angle_degrees: f64) -> f64 {
|
||||
let normalized_input = self.normalize_angle_f64(math_angle_degrees);
|
||||
|
||||
let mut result = normalized_input;
|
||||
|
||||
// Apply axis flips
|
||||
match (self.flip_x, self.flip_y) {
|
||||
(false, false) => {} // No change
|
||||
(true, false) => result = 180.0 - result, // X-flip: mirror across Y-axis
|
||||
(false, true) => result = -result, // Y-flip: mirror across X-axis
|
||||
(true, true) => result += 180.0, // Both flips: 180° rotation
|
||||
}
|
||||
|
||||
// Handle rotation direction
|
||||
if self.is_cw {
|
||||
result = -result;
|
||||
}
|
||||
|
||||
self.normalize_angle_f64(result)
|
||||
}
|
||||
|
||||
/// Convert angle from this system to mathematical system (f64)
|
||||
pub fn degrees_decode_f64(&self, system_angle_degrees: f64) -> f64 {
|
||||
let normalized_input = self.normalize_angle_f64(system_angle_degrees);
|
||||
|
||||
let mut result = normalized_input;
|
||||
|
||||
// Handle rotation direction
|
||||
if self.is_cw {
|
||||
result = -result;
|
||||
}
|
||||
|
||||
// Apply inverse axis flips
|
||||
match (self.flip_x, self.flip_y) {
|
||||
(false, false) => {} // No change
|
||||
(true, false) => result = 180.0 - result, // X-flip: mirror across Y-axis
|
||||
(false, true) => result = -result, // Y-flip: mirror across X-axis
|
||||
(true, true) => result -= 180.0, // Both flips: inverse 180° rotation
|
||||
}
|
||||
|
||||
self.normalize_angle_f64(result)
|
||||
}
|
||||
|
||||
/// Convert angle in radians from mathematical system to this system (f64)
|
||||
pub fn radians_encode_f64(&self, math_angle_radians: f64) -> f64 {
|
||||
let degrees = math_angle_radians.to_degrees();
|
||||
let converted_degrees = self.degrees_encode_f64(degrees);
|
||||
converted_degrees.to_radians()
|
||||
}
|
||||
|
||||
/// Convert angle in radians from this system to mathematical system (f64)
|
||||
pub fn radians_decode_f64(&self, system_angle_radians: f64) -> f64 {
|
||||
let degrees = system_angle_radians.to_degrees();
|
||||
let converted_degrees = self.degrees_decode_f64(degrees);
|
||||
converted_degrees.to_radians()
|
||||
}
|
||||
|
||||
/// Normalize angle to [0, 360) range (f32)
|
||||
fn normalize_angle(&self, angle: f64) -> f64 {
|
||||
let mut normalized = angle % 360.0;
|
||||
if normalized < 0.0 {
|
||||
normalized += 360.0;
|
||||
}
|
||||
normalized
|
||||
}
|
||||
|
||||
/// Normalize angle to [0, 360) range (f64)
|
||||
fn normalize_angle_f64(&self, angle: f64) -> f64 {
|
||||
let mut normalized = angle % 360.0;
|
||||
if normalized < 0.0 {
|
||||
normalized += 360.0;
|
||||
}
|
||||
normalized
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct AngleConverterUom {
|
||||
angle_converter: AngleConverter,
|
||||
}
|
||||
|
||||
impl AngleConverterUom {
|
||||
pub const fn new(angle_converter: AngleConverter) -> Self {
|
||||
Self { angle_converter }
|
||||
}
|
||||
|
||||
pub fn encode(&self, angle: Angle) -> Angle {
|
||||
Angle::new::<radian>(self.angle_converter.radians_encode(angle.get::<radian>()))
|
||||
}
|
||||
|
||||
pub fn decode(&self, angle: Angle) -> Angle {
|
||||
Angle::new::<radian>(self.angle_converter.radians_decode(angle.get::<radian>()))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use std::f32::consts::PI;
|
||||
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_constructors() {
|
||||
let math = AngleConverter::mathematical();
|
||||
assert_eq!(math.flip_x, false);
|
||||
assert_eq!(math.flip_y, false);
|
||||
assert_eq!(math.is_cw, false);
|
||||
|
||||
let screen = AngleConverter::screen();
|
||||
assert_eq!(screen.flip_x, false);
|
||||
assert_eq!(screen.flip_y, true);
|
||||
assert_eq!(screen.is_cw, false);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_screen_converter_bidirectional() {
|
||||
let converter = AngleConverter::screen();
|
||||
|
||||
// Math to screen (Y-flip only)
|
||||
assert_eq!(converter.degrees_encode(0.0), 0.0); // Right stays right
|
||||
assert_eq!(converter.degrees_encode(90.0), 270.0); // Up becomes down
|
||||
assert_eq!(converter.degrees_encode(270.0), 90.0); // Down becomes up
|
||||
assert_eq!(converter.degrees_encode(180.0), 180.0); // Left stays left
|
||||
|
||||
// Screen to math (should be inverse)
|
||||
assert_eq!(converter.degrees_decode(0.0), 0.0); // Right stays right
|
||||
assert_eq!(converter.degrees_decode(270.0), 90.0); // Down becomes up
|
||||
assert_eq!(converter.degrees_decode(90.0), 270.0); // Up becomes down
|
||||
assert_eq!(converter.degrees_decode(180.0), 180.0); // Left stays left
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_x_flip_converter() {
|
||||
let converter = AngleConverter::x_left_cw();
|
||||
|
||||
// Math to x-flipped system
|
||||
assert_eq!(converter.degrees_encode(0.0), 180.0); // Right becomes left
|
||||
assert_eq!(converter.degrees_encode(90.0), 90.0); // Up stays up
|
||||
assert_eq!(converter.degrees_encode(180.0), 0.0); // Left becomes right
|
||||
assert_eq!(converter.degrees_encode(270.0), 270.0); // Down stays down
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_both_flips_converter() {
|
||||
let converter = AngleConverter::new(true, true, false);
|
||||
|
||||
// Math to both-flipped system (180° rotation)
|
||||
assert_eq!(converter.degrees_encode(0.0), 180.0); // Right becomes left
|
||||
assert_eq!(converter.degrees_encode(90.0), 270.0); // Up becomes down
|
||||
assert_eq!(converter.degrees_encode(180.0), 0.0); // Left becomes right
|
||||
assert_eq!(converter.degrees_encode(270.0), 90.0); // Down becomes up
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_radians_conversion() {
|
||||
let converter = AngleConverter::screen();
|
||||
|
||||
// Test radians_encode
|
||||
let result = converter.radians_encode(PI as f64 / 2.0) as f64; // 90° in radians
|
||||
let expected: f64 = 270.0f64 * PI as f64 / 180.0f64; // 270° in radians
|
||||
assert!((result - expected).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_roundtrip_conversion() {
|
||||
let converter = AngleConverter::new(true, false, true); // X-flip + CW
|
||||
|
||||
let original = 75.0_f64;
|
||||
let converted = converter.degrees_encode(original);
|
||||
let roundtrip = converter.degrees_decode(converted);
|
||||
|
||||
assert!((original - roundtrip as f64).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
@@ -18,7 +18,7 @@ pub struct AngularStepConverter {
|
||||
|
||||
impl AngularStepConverter {
|
||||
/// Create a new converter with the specified steps per revolution
|
||||
pub const fn new(steps_per_revolution: i16) -> Self {
|
||||
pub fn new(steps_per_revolution: i16) -> Self {
|
||||
Self {
|
||||
steps_per_revolution,
|
||||
}
|
||||
|
||||
@@ -1,279 +0,0 @@
|
||||
//! # Circular Converter
|
||||
//!
|
||||
//! This module provides the `CircularConverter` which handles transformations between
|
||||
//! linear and angular measurements for circular motion systems.
|
||||
//!
|
||||
//! ## Usage
|
||||
//!
|
||||
//! The `CircularConverter` is designed to work alongside other converters:
|
||||
//! - Use `AngularStepConverter` for angular-to-step conversions
|
||||
//! - Use `LinearStepConverter` for complete linear-to-step conversions (uses `CircularConverter` internally)
|
||||
//! - Use `CircularConverter` directly when you need linear-to-angular conversions without steps
|
||||
//!
|
||||
//! ## Examples
|
||||
//!
|
||||
//! ```rust
|
||||
//! use control_core::converters::circular_converter::CircularConverter;
|
||||
//! use uom::si::{f64::{Length, Velocity}, length::meter, velocity::meter_per_second};
|
||||
//!
|
||||
//! let converter = CircularConverter::from_radius(Length::new::<meter>(0.1));
|
||||
//! let velocity = Velocity::new::<meter_per_second>(1.0);
|
||||
//! let angular_velocity = converter.linear_to_angular_velocity(velocity);
|
||||
//! ```
|
||||
|
||||
use uom::si::{
|
||||
acceleration::meter_per_second_squared,
|
||||
angle::radian,
|
||||
angular_acceleration::radian_per_second_squared,
|
||||
angular_velocity::radian_per_second,
|
||||
f64::{Acceleration, Angle, AngularAcceleration, AngularVelocity, Length, Velocity},
|
||||
length::meter,
|
||||
velocity::meter_per_second,
|
||||
};
|
||||
|
||||
/// A converter that handles transformations between linear and angular measurements
|
||||
/// for circular motion. This includes conversions for position, velocity, acceleration,
|
||||
/// and higher-order derivatives based on a circle's geometric properties.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct CircularConverter {
|
||||
radius: Length,
|
||||
circumference: Length,
|
||||
diameter: Length,
|
||||
}
|
||||
|
||||
impl CircularConverter {
|
||||
/// Create a new converter from radius
|
||||
pub fn from_radius(radius: Length) -> Self {
|
||||
let diameter = radius * 2.0;
|
||||
let circumference =
|
||||
Length::new::<meter>(2.0 * std::f64::consts::PI * radius.get::<meter>());
|
||||
Self {
|
||||
radius,
|
||||
diameter,
|
||||
circumference,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a new converter from diameter
|
||||
pub fn from_diameter(diameter: Length) -> Self {
|
||||
let radius = diameter / 2.0;
|
||||
Self::from_radius(radius)
|
||||
}
|
||||
|
||||
/// Create a new converter from circumference
|
||||
pub fn from_circumference(circumference: Length) -> Self {
|
||||
let radius =
|
||||
Length::new::<meter>(circumference.get::<meter>() / (2.0 * std::f64::consts::PI));
|
||||
Self::from_radius(radius)
|
||||
}
|
||||
|
||||
/// Get the radius
|
||||
pub fn radius(&self) -> Length {
|
||||
self.radius
|
||||
}
|
||||
|
||||
/// Get the diameter
|
||||
pub fn diameter(&self) -> Length {
|
||||
self.diameter
|
||||
}
|
||||
|
||||
/// Get the circumference
|
||||
pub fn circumference(&self) -> Length {
|
||||
self.circumference
|
||||
}
|
||||
|
||||
// Linear to Angular Position Conversions
|
||||
|
||||
/// Convert linear distance to angular position
|
||||
/// Formula: angle = distance / radius
|
||||
pub fn linear_to_angular_position(&self, distance: Length) -> Angle {
|
||||
let angle_rad = distance.get::<meter>() / self.radius.get::<meter>();
|
||||
Angle::new::<radian>(angle_rad)
|
||||
}
|
||||
|
||||
/// Convert angular position to linear distance
|
||||
/// Formula: distance = angle * radius
|
||||
pub fn angular_to_linear_position(&self, angle: Angle) -> Length {
|
||||
let distance = angle.get::<radian>() * self.radius.get::<meter>();
|
||||
Length::new::<meter>(distance)
|
||||
}
|
||||
|
||||
// Linear to Angular Velocity Conversions
|
||||
|
||||
/// Convert linear velocity to angular velocity
|
||||
/// Formula: angular_velocity = linear_velocity / radius
|
||||
pub fn linear_to_angular_velocity(&self, velocity: Velocity) -> AngularVelocity {
|
||||
let angular_velocity_rad_per_s =
|
||||
velocity.get::<meter_per_second>() / self.radius.get::<meter>();
|
||||
AngularVelocity::new::<radian_per_second>(angular_velocity_rad_per_s)
|
||||
}
|
||||
|
||||
/// Convert angular velocity to linear velocity
|
||||
/// Formula: linear_velocity = angular_velocity * radius
|
||||
pub fn angular_to_linear_velocity(&self, angular_velocity: AngularVelocity) -> Velocity {
|
||||
let velocity = angular_velocity.get::<radian_per_second>() * self.radius.get::<meter>();
|
||||
Velocity::new::<meter_per_second>(velocity)
|
||||
}
|
||||
|
||||
// Linear to Angular Acceleration Conversions
|
||||
|
||||
/// Convert linear acceleration to angular acceleration
|
||||
/// Formula: angular_acceleration = linear_acceleration / radius
|
||||
pub fn linear_to_angular_acceleration(
|
||||
&self,
|
||||
acceleration: Acceleration,
|
||||
) -> AngularAcceleration {
|
||||
let angular_acceleration_rad_per_s2 =
|
||||
acceleration.get::<meter_per_second_squared>() / self.radius.get::<meter>();
|
||||
AngularAcceleration::new::<radian_per_second_squared>(angular_acceleration_rad_per_s2)
|
||||
}
|
||||
|
||||
/// Convert angular acceleration to linear acceleration
|
||||
/// Formula: linear_acceleration = angular_acceleration * radius
|
||||
pub fn angular_to_linear_acceleration(
|
||||
&self,
|
||||
angular_acceleration: AngularAcceleration,
|
||||
) -> Acceleration {
|
||||
let acceleration =
|
||||
angular_acceleration.get::<radian_per_second_squared>() * self.radius.get::<meter>();
|
||||
Acceleration::new::<meter_per_second_squared>(acceleration)
|
||||
}
|
||||
|
||||
// Jerk Conversions (using f64 for now since UOM may not have jerk units)
|
||||
|
||||
/// Convert linear jerk to angular jerk
|
||||
/// Formula: angular_jerk = linear_jerk / radius
|
||||
/// Units: rad/s³ = (m/s³) / m
|
||||
pub fn linear_to_angular_jerk(&self, linear_jerk: f64) -> f64 {
|
||||
linear_jerk / self.radius.get::<meter>()
|
||||
}
|
||||
|
||||
/// Convert angular jerk to linear jerk
|
||||
/// Formula: linear_jerk = angular_jerk * radius
|
||||
/// Units: m/s³ = (rad/s³) * m
|
||||
pub fn angular_to_linear_jerk(&self, angular_jerk: f64) -> f64 {
|
||||
angular_jerk * self.radius.get::<meter>()
|
||||
}
|
||||
|
||||
// Arc Length Calculations
|
||||
|
||||
/// Calculate arc length from angle
|
||||
/// Formula: arc_length = angle * radius
|
||||
pub fn angle_to_arc_length(&self, angle: Angle) -> Length {
|
||||
self.angular_to_linear_position(angle)
|
||||
}
|
||||
|
||||
/// Calculate angle from arc length
|
||||
/// Formula: angle = arc_length / radius
|
||||
pub fn arc_length_to_angle(&self, arc_length: Length) -> Angle {
|
||||
self.linear_to_angular_position(arc_length)
|
||||
}
|
||||
|
||||
// Circumference-based calculations
|
||||
|
||||
/// Convert linear distance to number of revolutions
|
||||
/// Formula: revolutions = distance / circumference
|
||||
pub fn linear_distance_to_revolutions(&self, distance: Length) -> f64 {
|
||||
distance.get::<meter>() / self.circumference.get::<meter>()
|
||||
}
|
||||
|
||||
/// Convert number of revolutions to linear distance
|
||||
/// Formula: distance = revolutions * circumference
|
||||
pub fn revolutions_to_linear_distance(&self, revolutions: f64) -> Length {
|
||||
Length::new::<meter>(revolutions * self.circumference.get::<meter>())
|
||||
}
|
||||
|
||||
/// Convert linear velocity to revolutions per second
|
||||
/// Formula: rps = velocity / circumference
|
||||
pub fn linear_velocity_to_rps(&self, velocity: Velocity) -> f64 {
|
||||
velocity.get::<meter_per_second>() / self.circumference.get::<meter>()
|
||||
}
|
||||
|
||||
/// Convert revolutions per second to linear velocity
|
||||
/// Formula: velocity = rps * circumference
|
||||
pub fn rps_to_linear_velocity(&self, rps: f64) -> Velocity {
|
||||
Velocity::new::<meter_per_second>(rps * self.circumference.get::<meter>())
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use uom::si::length::centimeter;
|
||||
|
||||
#[test]
|
||||
fn test_constructors() {
|
||||
let radius = Length::new::<centimeter>(5.0);
|
||||
let diameter = Length::new::<centimeter>(10.0);
|
||||
let circumference = Length::new::<centimeter>(2.0 * std::f64::consts::PI * 5.0);
|
||||
|
||||
let converter1 = CircularConverter::from_radius(radius);
|
||||
let converter2 = CircularConverter::from_diameter(diameter);
|
||||
let converter3 = CircularConverter::from_circumference(circumference);
|
||||
|
||||
// All should have approximately the same values
|
||||
assert!((converter1.radius().get::<centimeter>() - 5.0).abs() < 1e-10);
|
||||
assert!((converter2.radius().get::<centimeter>() - 5.0).abs() < 1e-10);
|
||||
assert!((converter3.radius().get::<centimeter>() - 5.0).abs() < 1e-10);
|
||||
|
||||
assert!((converter1.diameter().get::<centimeter>() - 10.0).abs() < 1e-10);
|
||||
assert!((converter2.diameter().get::<centimeter>() - 10.0).abs() < 1e-10);
|
||||
assert!((converter3.diameter().get::<centimeter>() - 10.0).abs() < 1e-10);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_position_conversions() {
|
||||
let converter = CircularConverter::from_radius(Length::new::<meter>(1.0));
|
||||
|
||||
// Test linear to angular
|
||||
let distance = Length::new::<meter>(std::f64::consts::PI);
|
||||
let angle = converter.linear_to_angular_position(distance);
|
||||
assert!((angle.get::<radian>() - std::f64::consts::PI).abs() < 1e-10);
|
||||
|
||||
// Test angular to linear
|
||||
let angle = Angle::new::<radian>(std::f64::consts::PI / 2.0);
|
||||
let distance = converter.angular_to_linear_position(angle);
|
||||
assert!((distance.get::<meter>() - std::f64::consts::PI / 2.0).abs() < 1e-10);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_velocity_conversions() {
|
||||
let converter = CircularConverter::from_radius(Length::new::<meter>(0.5));
|
||||
|
||||
// Test linear to angular
|
||||
let velocity = Velocity::new::<meter_per_second>(1.0);
|
||||
let angular_velocity = converter.linear_to_angular_velocity(velocity);
|
||||
assert!((angular_velocity.get::<radian_per_second>() - 2.0).abs() < 1e-10);
|
||||
|
||||
// Test angular to linear
|
||||
let angular_velocity = AngularVelocity::new::<radian_per_second>(4.0);
|
||||
let velocity = converter.angular_to_linear_velocity(angular_velocity);
|
||||
assert!((velocity.get::<meter_per_second>() - 2.0).abs() < 1e-10);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_revolution_conversions() {
|
||||
let converter = CircularConverter::from_radius(Length::new::<meter>(1.0));
|
||||
|
||||
// One full revolution should equal circumference
|
||||
let revolutions = 1.0;
|
||||
let distance = converter.revolutions_to_linear_distance(revolutions);
|
||||
assert!((distance.get::<meter>() - converter.circumference().get::<meter>()).abs() < 1e-10);
|
||||
|
||||
// Converting back should give 1 revolution
|
||||
let calculated_revolutions = converter.linear_distance_to_revolutions(distance);
|
||||
assert!((calculated_revolutions - 1.0).abs() < 1e-10);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_jerk_conversions() {
|
||||
let converter = CircularConverter::from_radius(Length::new::<meter>(2.0));
|
||||
|
||||
let linear_jerk = 4.0; // m/s³
|
||||
let angular_jerk = converter.linear_to_angular_jerk(linear_jerk);
|
||||
assert!((angular_jerk - 2.0).abs() < 1e-10); // 4.0 / 2.0 = 2.0 rad/s³
|
||||
|
||||
let calculated_linear_jerk = converter.angular_to_linear_jerk(angular_jerk);
|
||||
assert!((calculated_linear_jerk - linear_jerk).abs() < 1e-10);
|
||||
}
|
||||
}
|
||||
@@ -1,11 +1,14 @@
|
||||
use uom::si::{
|
||||
acceleration::meter_per_second_squared,
|
||||
angle::revolution,
|
||||
angular_acceleration::radian_per_second_squared,
|
||||
angular_velocity::revolution_per_second,
|
||||
f64::{Acceleration, Angle, AngularAcceleration, AngularVelocity, Length, Velocity},
|
||||
length::meter,
|
||||
velocity::meter_per_second,
|
||||
};
|
||||
|
||||
use super::angular_step_converter::AngularStepConverter;
|
||||
use super::circular_converter::CircularConverter;
|
||||
|
||||
/// Converts between linear measurements and motor steps
|
||||
///
|
||||
@@ -15,52 +18,57 @@ use super::circular_converter::CircularConverter;
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct LinearStepConverter {
|
||||
angular_step_converter: AngularStepConverter,
|
||||
circular_converter: CircularConverter,
|
||||
radius: Length,
|
||||
circumference: Length,
|
||||
}
|
||||
|
||||
// Constructor and basic getters
|
||||
impl LinearStepConverter {
|
||||
/// Create a new converter from radius and steps per revolution
|
||||
pub fn from_radius(steps_per_revolution: i16, radius: Length) -> Self {
|
||||
let circumference =
|
||||
Length::new::<meter>(2.0 * std::f64::consts::PI * radius.get::<meter>());
|
||||
Self {
|
||||
angular_step_converter: AngularStepConverter::new(steps_per_revolution),
|
||||
circular_converter: CircularConverter::from_radius(radius),
|
||||
radius,
|
||||
circumference,
|
||||
}
|
||||
}
|
||||
|
||||
/// Create a new converter from diameter and steps per revolution
|
||||
pub fn from_diameter(steps_per_revolution: i16, diameter: Length) -> Self {
|
||||
Self {
|
||||
angular_step_converter: AngularStepConverter::new(steps_per_revolution),
|
||||
circular_converter: CircularConverter::from_diameter(diameter),
|
||||
}
|
||||
let radius = diameter / 2.0;
|
||||
Self::from_radius(steps_per_revolution, radius)
|
||||
}
|
||||
|
||||
/// Create a new converter from circumference and steps per revolution
|
||||
pub fn from_circumference(steps_per_revolution: i16, circumference: Length) -> Self {
|
||||
let radius =
|
||||
Length::new::<meter>(circumference.get::<meter>() / (2.0 * std::f64::consts::PI));
|
||||
Self {
|
||||
angular_step_converter: AngularStepConverter::new(steps_per_revolution),
|
||||
circular_converter: CircularConverter::from_circumference(circumference),
|
||||
radius,
|
||||
circumference,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get the radius used by the converter
|
||||
pub fn radius(&self) -> Length {
|
||||
self.circular_converter.radius()
|
||||
self.radius
|
||||
}
|
||||
|
||||
/// Get the diameter of the system
|
||||
pub fn diameter(&self) -> Length {
|
||||
self.circular_converter.diameter()
|
||||
self.radius * 2.0
|
||||
}
|
||||
|
||||
/// Get the circumference of the system
|
||||
pub fn circumference(&self) -> Length {
|
||||
self.circular_converter.circumference()
|
||||
self.circumference
|
||||
}
|
||||
|
||||
/// Get the steps per revolution
|
||||
pub const fn steps_per_revolution(&self) -> i16 {
|
||||
pub fn steps_per_revolution(&self) -> i16 {
|
||||
self.angular_step_converter.steps_per_revolution
|
||||
}
|
||||
}
|
||||
@@ -71,10 +79,8 @@ impl LinearStepConverter {
|
||||
///
|
||||
/// Formula: steps = (distance / circumference) * steps_per_revolution
|
||||
pub fn distance_to_steps(&self, distance: Length) -> f64 {
|
||||
// Convert distance to revolutions using CircularConverter
|
||||
let revolutions = self
|
||||
.circular_converter
|
||||
.linear_distance_to_revolutions(distance);
|
||||
// Convert distance to angle: angle = distance/circumference (in revolutions)
|
||||
let revolutions = distance.get::<meter>() / self.circumference.get::<meter>();
|
||||
let angle = Angle::new::<revolution>(revolutions);
|
||||
|
||||
// Convert angle to steps
|
||||
@@ -88,19 +94,21 @@ impl LinearStepConverter {
|
||||
// Convert steps to angle
|
||||
let angle = self.angular_step_converter.steps_to_angle(steps);
|
||||
|
||||
// Convert angle to distance using CircularConverter
|
||||
// Convert angle to distance: distance = angle * circumference
|
||||
let revolutions = angle.get::<revolution>();
|
||||
self.circular_converter
|
||||
.revolutions_to_linear_distance(revolutions)
|
||||
let distance = revolutions * self.circumference.get::<meter>();
|
||||
|
||||
Length::new::<meter>(distance)
|
||||
}
|
||||
|
||||
/// Convert linear velocity to steps/second
|
||||
///
|
||||
/// Formula: steps/second = (velocity / circumference) * steps_per_revolution
|
||||
pub fn velocity_to_steps(&self, velocity: Velocity) -> f64 {
|
||||
// Convert linear velocity to revolutions per second using CircularConverter
|
||||
let rps = self.circular_converter.linear_velocity_to_rps(velocity);
|
||||
let angular_velocity = AngularVelocity::new::<revolution_per_second>(rps);
|
||||
// Convert linear velocity to angular velocity: ω = v/circumference (in rev/s)
|
||||
let linear_velocity = velocity.get::<meter_per_second>();
|
||||
let angular_velocity_rps = linear_velocity / self.circumference.get::<meter>();
|
||||
let angular_velocity = AngularVelocity::new::<revolution_per_second>(angular_velocity_rps);
|
||||
|
||||
// Convert angular velocity to steps/second
|
||||
self.angular_step_converter
|
||||
@@ -116,19 +124,23 @@ impl LinearStepConverter {
|
||||
.angular_step_converter
|
||||
.steps_to_angular_velocity(steps_per_second);
|
||||
|
||||
// Convert angular velocity to linear velocity using CircularConverter
|
||||
let rps = angular_velocity.get::<revolution_per_second>();
|
||||
self.circular_converter.rps_to_linear_velocity(rps)
|
||||
// Convert angular velocity to linear velocity: v = ω * circumference
|
||||
let angular_velocity_rps = angular_velocity.get::<revolution_per_second>();
|
||||
let linear_velocity = angular_velocity_rps * self.circumference.get::<meter>();
|
||||
|
||||
Velocity::new::<meter_per_second>(linear_velocity)
|
||||
}
|
||||
|
||||
/// Convert linear acceleration to steps/second²
|
||||
///
|
||||
/// Formula: steps/second² = (acceleration / radius) * (steps_per_revolution / (2π))
|
||||
pub fn acceleration_to_steps(&self, acceleration: Acceleration) -> f64 {
|
||||
// Convert linear acceleration to angular acceleration using CircularConverter
|
||||
let angular_acceleration = self
|
||||
.circular_converter
|
||||
.linear_to_angular_acceleration(acceleration);
|
||||
// Convert linear acceleration to angular acceleration: α = a/r
|
||||
let linear_acceleration = acceleration.get::<meter_per_second_squared>();
|
||||
// Calculate angular acceleration in radians per second squared
|
||||
let angular_acceleration_rad_per_s2 = linear_acceleration / self.radius.get::<meter>();
|
||||
let angular_acceleration =
|
||||
AngularAcceleration::new::<radian_per_second_squared>(angular_acceleration_rad_per_s2);
|
||||
|
||||
// Convert angular acceleration to steps/second²
|
||||
self.angular_step_converter
|
||||
@@ -144,9 +156,12 @@ impl LinearStepConverter {
|
||||
.angular_step_converter
|
||||
.steps_to_angular_acceleration(steps_per_second_squared);
|
||||
|
||||
// Convert angular acceleration to linear acceleration using CircularConverter
|
||||
self.circular_converter
|
||||
.angular_to_linear_acceleration(angular_acceleration)
|
||||
// Convert angular acceleration to linear acceleration: a = α * r
|
||||
let angular_acceleration_rad_per_s2 =
|
||||
angular_acceleration.get::<radian_per_second_squared>();
|
||||
let linear_acceleration = angular_acceleration_rad_per_s2 * self.radius.get::<meter>();
|
||||
|
||||
Acceleration::new::<meter_per_second_squared>(linear_acceleration)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -156,9 +171,7 @@ impl LinearStepConverter {
|
||||
///
|
||||
/// Formula: angle (in revolutions) = distance / circumference
|
||||
pub fn distance_to_angle(&self, distance: Length) -> Angle {
|
||||
let revolutions = self
|
||||
.circular_converter
|
||||
.linear_distance_to_revolutions(distance);
|
||||
let revolutions = distance.get::<meter>() / self.circumference.get::<meter>();
|
||||
Angle::new::<revolution>(revolutions)
|
||||
}
|
||||
|
||||
@@ -167,24 +180,26 @@ impl LinearStepConverter {
|
||||
/// Formula: distance = angle (in revolutions) * circumference
|
||||
pub fn angle_to_distance(&self, angle: Angle) -> Length {
|
||||
let revolutions = angle.get::<revolution>();
|
||||
self.circular_converter
|
||||
.revolutions_to_linear_distance(revolutions)
|
||||
let distance = revolutions * self.circumference.get::<meter>();
|
||||
Length::new::<meter>(distance)
|
||||
}
|
||||
|
||||
/// Convert linear velocity to angular velocity
|
||||
///
|
||||
/// Formula: angular velocity (in rev/s) = velocity / circumference
|
||||
pub fn velocity_to_angular_velocity(&self, velocity: Velocity) -> AngularVelocity {
|
||||
let rps = self.circular_converter.linear_velocity_to_rps(velocity);
|
||||
AngularVelocity::new::<revolution_per_second>(rps)
|
||||
let linear_velocity = velocity.get::<meter_per_second>();
|
||||
let angular_velocity_rps = linear_velocity / self.circumference.get::<meter>();
|
||||
AngularVelocity::new::<revolution_per_second>(angular_velocity_rps)
|
||||
}
|
||||
|
||||
/// Convert angular velocity to linear velocity
|
||||
///
|
||||
/// Formula: velocity = angular velocity (in rev/s) * circumference
|
||||
pub fn angular_velocity_to_velocity(&self, angular_velocity: AngularVelocity) -> Velocity {
|
||||
let rps = angular_velocity.get::<revolution_per_second>();
|
||||
self.circular_converter.rps_to_linear_velocity(rps)
|
||||
let angular_velocity_rps = angular_velocity.get::<revolution_per_second>();
|
||||
let linear_velocity = angular_velocity_rps * self.circumference.get::<meter>();
|
||||
Velocity::new::<meter_per_second>(linear_velocity)
|
||||
}
|
||||
|
||||
/// Convert linear acceleration to angular acceleration
|
||||
@@ -194,8 +209,9 @@ impl LinearStepConverter {
|
||||
&self,
|
||||
acceleration: Acceleration,
|
||||
) -> AngularAcceleration {
|
||||
self.circular_converter
|
||||
.linear_to_angular_acceleration(acceleration)
|
||||
let linear_acceleration = acceleration.get::<meter_per_second_squared>();
|
||||
let angular_acceleration_rad_per_s2 = linear_acceleration / self.radius.get::<meter>();
|
||||
AngularAcceleration::new::<radian_per_second_squared>(angular_acceleration_rad_per_s2)
|
||||
}
|
||||
|
||||
/// Convert angular acceleration to linear acceleration
|
||||
@@ -205,8 +221,10 @@ impl LinearStepConverter {
|
||||
&self,
|
||||
angular_acceleration: AngularAcceleration,
|
||||
) -> Acceleration {
|
||||
self.circular_converter
|
||||
.angular_to_linear_acceleration(angular_acceleration)
|
||||
let angular_acceleration_rad_per_s2 =
|
||||
angular_acceleration.get::<radian_per_second_squared>();
|
||||
let linear_acceleration = angular_acceleration_rad_per_s2 * self.radius.get::<meter>();
|
||||
Acceleration::new::<meter_per_second_squared>(linear_acceleration)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -251,10 +269,6 @@ mod tests {
|
||||
use super::*;
|
||||
use approx::assert_relative_eq;
|
||||
use std::f64::EPSILON;
|
||||
use uom::si::{
|
||||
acceleration::meter_per_second_squared, angular_acceleration::radian_per_second_squared,
|
||||
length::meter, velocity::meter_per_second,
|
||||
};
|
||||
|
||||
#[test]
|
||||
fn test_new() {
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
pub mod angle_converter;
|
||||
pub mod angular_step_converter;
|
||||
pub mod circular_converter;
|
||||
pub mod linear_step_converter;
|
||||
pub mod motor_converter;
|
||||
pub mod transmission_converter;
|
||||
@@ -0,0 +1,22 @@
|
||||
use uom::si::f64::AngularVelocity;
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct TransmissionConverter {
|
||||
pub transmission_ratio: f64,
|
||||
}
|
||||
|
||||
impl TransmissionConverter {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
transmission_ratio: 34.0,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn calculate_screw_output_rpm(&self, rpm: AngularVelocity) -> AngularVelocity {
|
||||
rpm / self.transmission_ratio
|
||||
}
|
||||
|
||||
pub fn calculate_screw_input_rpm(&self, screw_rpm: AngularVelocity) -> AngularVelocity {
|
||||
screw_rpm * self.transmission_ratio
|
||||
}
|
||||
}
|
||||
@@ -1,36 +0,0 @@
|
||||
use anyhow::anyhow;
|
||||
use smol::lock::Mutex;
|
||||
use std::any::Any;
|
||||
use std::sync::Arc;
|
||||
|
||||
pub trait Downcast<T> {
|
||||
fn downcast(&self) -> Result<T, anyhow::Error>;
|
||||
}
|
||||
|
||||
pub trait DowncastRef<T> {
|
||||
fn downcast_ref(&self) -> Result<&T, anyhow::Error>;
|
||||
}
|
||||
|
||||
impl<T: 'static, U: Any + 'static> Downcast<Arc<Mutex<T>>> for Arc<Mutex<U>> {
|
||||
fn downcast(&self) -> Result<Arc<Mutex<T>>, anyhow::Error> {
|
||||
// Acquire a read lock on the RwLock
|
||||
let lock = self.lock_blocking();
|
||||
|
||||
// Check if the inner type can be downcasted to T
|
||||
let any: &dyn Any = &*lock;
|
||||
|
||||
if any.is::<T>() {
|
||||
// Clone the Arc and return it as the desired type
|
||||
let cloned: Arc<Mutex<dyn Any>> = self.clone();
|
||||
|
||||
// Transmute the Arc to the desired type
|
||||
unsafe {
|
||||
let arc = Arc::from_raw(Arc::into_raw(cloned) as *const Mutex<T>);
|
||||
|
||||
Ok(arc)
|
||||
}
|
||||
} else {
|
||||
Err(anyhow!("[{}::downcast] Downcast failed", module_path!()))
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -4,10 +4,8 @@ use ethercat_hal::devices::ek1100::EK1100_IDENTITY_A;
|
||||
use ethercat_hal::devices::el1002::EL1002_IDENTITY_A;
|
||||
use ethercat_hal::devices::el1008::EL1008_IDENTITY_A;
|
||||
use ethercat_hal::devices::el2002::EL2002_IDENTITY_A;
|
||||
use ethercat_hal::devices::el2002::EL2002_IDENTITY_B;
|
||||
use ethercat_hal::devices::el2004::EL2004_IDENTITY_A;
|
||||
use ethercat_hal::devices::el2008::EL2008_IDENTITY_A;
|
||||
use ethercat_hal::devices::el2008::EL2008_IDENTITY_B;
|
||||
use ethercat_hal::devices::el2521::{
|
||||
EL2521_IDENTITY_0000_A, EL2521_IDENTITY_0000_B, EL2521_IDENTITY_0024_A,
|
||||
};
|
||||
@@ -15,14 +13,13 @@ use ethercat_hal::devices::el2522::EL2522_IDENTITY_A;
|
||||
use ethercat_hal::devices::el3001::EL3001_IDENTITY_A;
|
||||
use ethercat_hal::devices::el3021::EL3021_IDENTITY_A;
|
||||
use ethercat_hal::devices::el3024::EL3024_IDENTITY_A;
|
||||
use ethercat_hal::devices::el3062_0030::EL3062_0030_IDENTITY_A;
|
||||
use ethercat_hal::devices::el3204::EL3204_IDENTITY_A;
|
||||
use ethercat_hal::devices::el6021::EL6021_IDENTITY_A;
|
||||
use ethercat_hal::devices::el6021::EL6021_IDENTITY_B;
|
||||
|
||||
use ethercat_hal::devices::el3204::EL3204_IDENTITY_B;
|
||||
use ethercat_hal::devices::el4002::EL4002_IDENTITY_A;
|
||||
use ethercat_hal::devices::el5152::EL5152_IDENTITY_A;
|
||||
use ethercat_hal::devices::el6021::{
|
||||
EL6021_IDENTITY_A, EL6021_IDENTITY_B, EL6021_IDENTITY_C, EL6021_IDENTITY_D,
|
||||
};
|
||||
|
||||
use ethercat_hal::devices::el6021::EL6021_IDENTITY_C;
|
||||
use ethercat_hal::devices::el7031::{EL7031_IDENTITY_A, EL7031_IDENTITY_B};
|
||||
use ethercat_hal::devices::el7031_0030::EL7031_0030_IDENTITY_A;
|
||||
use ethercat_hal::devices::el7041_0052::EL7041_0052_IDENTITY_A;
|
||||
@@ -46,12 +43,7 @@ pub struct MachineIdentificationAddresses {
|
||||
}
|
||||
|
||||
impl MachineIdentificationAddresses {
|
||||
pub const fn new(
|
||||
vendor_word: u16,
|
||||
serial_word: u16,
|
||||
machine_word: u16,
|
||||
device_word: u16,
|
||||
) -> Self {
|
||||
pub fn new(vendor_word: u16, serial_word: u16, machine_word: u16, device_word: u16) -> Self {
|
||||
Self {
|
||||
vendor_word,
|
||||
serial_word,
|
||||
@@ -65,8 +57,8 @@ impl Default for MachineIdentificationAddresses {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
vendor_word: 0x0028,
|
||||
machine_word: 0x0029,
|
||||
serial_word: 0x002a,
|
||||
serial_word: 0x0029,
|
||||
machine_word: 0x002a,
|
||||
role_word: 0x002b,
|
||||
}
|
||||
}
|
||||
@@ -81,7 +73,7 @@ pub async fn read_device_identifications<'maindevice>(
|
||||
) -> Vec<Result<DeviceMachineIdentification, Error>> {
|
||||
let mut result = Vec::new();
|
||||
for subdevice in subdevices.iter() {
|
||||
let identification = machine_device_identification(subdevice, maindevice).await;
|
||||
let identification = machine_device_identification(&subdevice, maindevice).await;
|
||||
result.push(identification);
|
||||
}
|
||||
result
|
||||
@@ -228,37 +220,33 @@ pub async fn write_machine_device_identification<'maindevice, const MAX_PDI: usi
|
||||
|
||||
/// Returns the EEPROM addresses for the machine device identification
|
||||
/// based on the subdevice's identity
|
||||
pub fn get_identification_addresses(
|
||||
pub fn get_identification_addresses<'maindevice>(
|
||||
subdevice_identity: &SubDeviceIdentity,
|
||||
subdevice_name: &str,
|
||||
) -> Result<MachineIdentificationAddresses, Error> {
|
||||
let identity_tuple = subdevice_identity_to_tuple(subdevice_identity);
|
||||
let identity_tuple = subdevice_identity_to_tuple(&subdevice_identity);
|
||||
|
||||
Ok(match identity_tuple {
|
||||
EK1100_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL1002_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL1008_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL2002_IDENTITY_A | EL2002_IDENTITY_B => MachineIdentificationAddresses::default(),
|
||||
EL2002_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL2004_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL2008_IDENTITY_A | EL2008_IDENTITY_B => MachineIdentificationAddresses::default(),
|
||||
EL3204_IDENTITY_A | EL3204_IDENTITY_B => MachineIdentificationAddresses::default(),
|
||||
EL2008_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL3001_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL2521_IDENTITY_0000_A | EL2521_IDENTITY_0000_B | EL2521_IDENTITY_0024_A => {
|
||||
MachineIdentificationAddresses::default()
|
||||
}
|
||||
EL2522_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL3001_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL3021_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL3024_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL3062_0030_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL3204_IDENTITY_A | EL3204_IDENTITY_B => MachineIdentificationAddresses::default(),
|
||||
EL4002_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL5152_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL6021_IDENTITY_A | EL6021_IDENTITY_B | EL6021_IDENTITY_C | EL6021_IDENTITY_D => {
|
||||
MachineIdentificationAddresses::default()
|
||||
}
|
||||
EL3021_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL7031_IDENTITY_A | EL7031_IDENTITY_B => MachineIdentificationAddresses::default(),
|
||||
EL7031_0030_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
EL7041_0052_IDENTITY_A => MachineIdentificationAddresses::default(),
|
||||
|
||||
EL6021_IDENTITY_A | EL6021_IDENTITY_B | EL6021_IDENTITY_C => {
|
||||
MachineIdentificationAddresses::default()
|
||||
}
|
||||
_ => {
|
||||
// block_on(u16dump(&subdevice, maindevice, 0x00, 0xff))?;
|
||||
Err(anyhow!(
|
||||
@@ -284,8 +272,8 @@ async fn u16dump<'maindevice>(
|
||||
words.push(subdevice.eeprom_read(maindevice, word).await?);
|
||||
}
|
||||
|
||||
println!(
|
||||
"EEPROM dump for {} from 0x{:04x} to 0x{:04x}",
|
||||
print!(
|
||||
"EEPROM dump for {} from 0x{:04x} to 0x{:04x}\n",
|
||||
subdevice.name(),
|
||||
start_byte / 2,
|
||||
end_byte / 2
|
||||
@@ -300,7 +288,7 @@ fn u16print(start_byte: u16, end_byte: u16, data: Vec<u16>) {
|
||||
let table_start_word = start_byte & 0xfff0;
|
||||
let table_end_word = (end_byte & 0xfff0_u16) + 0x10_u16;
|
||||
|
||||
let rows = (table_end_word - table_start_word) >> 4;
|
||||
let rows = table_end_word - table_start_word >> 4;
|
||||
|
||||
for row in 0..rows {
|
||||
print!("0x{:04x} | ", (table_start_word + row * 0x10) / 2);
|
||||
@@ -317,7 +305,7 @@ fn u16print(start_byte: u16, end_byte: u16, data: Vec<u16>) {
|
||||
}
|
||||
}
|
||||
}
|
||||
println!();
|
||||
print!("\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -42,17 +42,13 @@ pub async fn discover_ethercat_interface() -> Result<String, anyhow::Error> {
|
||||
|
||||
// Get eligible interfaces
|
||||
let mut interfaces = Interface::get_all()
|
||||
.map_err(|e| anyhow::anyhow!("Failed to get network interfaces: {}", e))?
|
||||
.or_else(|e| Err(anyhow::anyhow!("Failed to get network interfaces: {}", e)))?
|
||||
.into_iter()
|
||||
.filter(|iface| {
|
||||
iface.is_up()
|
||||
&& iface.is_running()
|
||||
&& !iface.is_loopback()
|
||||
&& !iface.name.starts_with("bridge")
|
||||
&& !iface.name.starts_with("utun") // Exclude tunnel interfaces
|
||||
&& !iface.name.starts_with("awdl") // Exclude Apple Wireless Direct Link
|
||||
&& !iface.name.starts_with("anpi") // Exclude Apple Network Privacy Interface
|
||||
&& !iface.name.starts_with("llw") // Exclude Apple low-latency WLAN
|
||||
})
|
||||
.collect::<Vec<_>>();
|
||||
|
||||
@@ -64,37 +60,34 @@ pub async fn discover_ethercat_interface() -> Result<String, anyhow::Error> {
|
||||
.iter()
|
||||
.map(|iface| {
|
||||
let name = iface.name.clone();
|
||||
std::thread::Builder::new()
|
||||
.name(format!("ethercat-test-{}", name))
|
||||
.spawn(move || {
|
||||
std::panic::catch_unwind(|| {
|
||||
smol::block_on(async {
|
||||
match test_interface(&name) {
|
||||
Ok(_) => {
|
||||
tracing::info!("Found working EtherCAT interface: {}", name);
|
||||
Some(name.clone())
|
||||
}
|
||||
Err(_) => {
|
||||
tracing::debug!("Interface {} failed", name);
|
||||
None
|
||||
}
|
||||
std::thread::spawn(move || {
|
||||
std::panic::catch_unwind(|| {
|
||||
smol::block_on(async {
|
||||
match test_interface(&name) {
|
||||
Ok(_) => {
|
||||
tracing::info!("Found working EtherCAT interface: {}", name);
|
||||
Some(name.clone())
|
||||
}
|
||||
})
|
||||
Err(e) => {
|
||||
tracing::debug!("Interface {} failed: {}", name, e);
|
||||
None
|
||||
}
|
||||
}
|
||||
})
|
||||
.unwrap_or(None)
|
||||
})
|
||||
.expect("Failed to spawn thread")
|
||||
.unwrap_or(None)
|
||||
})
|
||||
})
|
||||
.collect::<Vec<_>>();
|
||||
|
||||
// Wait for all tasks to complete and collect successful results
|
||||
let successful_interfaces: Vec<String> = tasks
|
||||
.into_iter()
|
||||
.filter_map(|task| task.join().expect("Should join thread"))
|
||||
.collect();
|
||||
|
||||
// Return the first successful interface (they're sorted by name)
|
||||
let result = successful_interfaces.into_iter().next();
|
||||
// Return first successful interface
|
||||
let result =
|
||||
tasks
|
||||
.into_iter()
|
||||
.find_map(|task| match task.join().expect("Should join thread") {
|
||||
Some(name) => Some(name),
|
||||
None => None,
|
||||
});
|
||||
|
||||
// Restore the default panic hook
|
||||
std::panic::set_hook(default_hook);
|
||||
@@ -140,12 +133,7 @@ fn test_interface(interface: &str) -> Result<(), anyhow::Error> {
|
||||
// Default 1000ms
|
||||
mailbox_response: Duration::from_millis(1000),
|
||||
},
|
||||
MainDeviceConfig {
|
||||
// Default 10000
|
||||
dc_static_sync_iterations: 10000,
|
||||
// Default None
|
||||
retry_behaviour: ethercrab::RetryBehaviour::Count(5),
|
||||
},
|
||||
MainDeviceConfig::default(),
|
||||
));
|
||||
|
||||
let result = maindevice
|
||||
|
||||
@@ -1,337 +0,0 @@
|
||||
use serde::Serialize;
|
||||
use serde::ser::{
|
||||
SerializeMap, SerializeSeq, SerializeStruct, SerializeStructVariant, SerializeTuple,
|
||||
SerializeTupleStruct, SerializeTupleVariant, Serializer,
|
||||
};
|
||||
use std::error::Error;
|
||||
use std::fmt::Display;
|
||||
use std::hash::{DefaultHasher, Hasher};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct HashSerializerError {}
|
||||
|
||||
impl std::fmt::Display for HashSerializerError {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
f.write_str(&format!("{:?}", self))
|
||||
// f.debug_struct("HashSerialiyerError")
|
||||
// .finish()
|
||||
}
|
||||
}
|
||||
|
||||
impl Error for HashSerializerError {}
|
||||
|
||||
impl serde::ser::Error for HashSerializerError {
|
||||
fn custom<T>(_msg: T) -> Self
|
||||
where
|
||||
T: Display,
|
||||
{
|
||||
Self {}
|
||||
}
|
||||
}
|
||||
|
||||
pub struct HashSerializer<'a, H: Hasher>(&'a mut H);
|
||||
impl<H: Hasher> SerializeSeq for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
|
||||
fn serialize_element<T>(&mut self, value: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
value.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn end(self) -> Result<Self::Ok, Self::Error> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<H: Hasher> SerializeTuple for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
|
||||
fn serialize_element<T>(&mut self, value: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
value.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn end(self) -> Result<Self::Ok, Self::Error> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<H: Hasher> SerializeTupleStruct for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
|
||||
fn serialize_field<T>(&mut self, value: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
value.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn end(self) -> Result<Self::Ok, Self::Error> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<H: Hasher> SerializeTupleVariant for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
|
||||
fn serialize_field<T>(&mut self, value: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
value.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn end(self) -> Result<Self::Ok, Self::Error> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<H: Hasher> SerializeMap for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
|
||||
fn serialize_key<T>(&mut self, key: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
key.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn serialize_value<T>(&mut self, value: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
value.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn end(self) -> Result<Self::Ok, Self::Error> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<H: Hasher> SerializeStruct for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
|
||||
fn serialize_field<T>(&mut self, key: &'static str, value: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
// Optional: hash the field name to avoid collisions
|
||||
self.0.write(key.as_bytes());
|
||||
value.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn end(self) -> Result<Self::Ok, Self::Error> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<H: Hasher> SerializeStructVariant for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
|
||||
fn serialize_field<T>(&mut self, key: &'static str, value: &T) -> Result<(), Self::Error>
|
||||
where
|
||||
T: ?Sized + Serialize,
|
||||
{
|
||||
self.0.write(key.as_bytes());
|
||||
value.serialize(HashSerializer(self.0))
|
||||
}
|
||||
|
||||
fn end(self) -> Result<Self::Ok, Self::Error> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl<H: Hasher> Serializer for HashSerializer<'_, H> {
|
||||
type Ok = ();
|
||||
type Error = HashSerializerError;
|
||||
type SerializeSeq = Self;
|
||||
type SerializeTuple = Self;
|
||||
type SerializeTupleStruct = Self;
|
||||
type SerializeTupleVariant = Self;
|
||||
type SerializeMap = Self;
|
||||
type SerializeStruct = Self;
|
||||
type SerializeStructVariant = Self;
|
||||
|
||||
fn serialize_bool(self, v: bool) -> Result<(), Self::Error> {
|
||||
self.0.write_u8(v as u8);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_i8(self, v: i8) -> Result<(), Self::Error> {
|
||||
self.0.write_i8(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_i16(self, v: i16) -> Result<(), Self::Error> {
|
||||
self.0.write_i16(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_i32(self, v: i32) -> Result<(), Self::Error> {
|
||||
self.0.write_i32(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_i64(self, v: i64) -> Result<(), Self::Error> {
|
||||
self.0.write_i64(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_u8(self, v: u8) -> Result<(), Self::Error> {
|
||||
self.0.write_u8(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_u16(self, v: u16) -> Result<(), Self::Error> {
|
||||
self.0.write_u16(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_u32(self, v: u32) -> Result<(), Self::Error> {
|
||||
self.0.write_u32(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_u64(self, v: u64) -> Result<(), Self::Error> {
|
||||
self.0.write_u64(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_f32(self, v: f32) -> Result<(), Self::Error> {
|
||||
self.0.write_u32(v.to_bits());
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_f64(self, v: f64) -> Result<(), Self::Error> {
|
||||
self.0.write_u64(v.to_bits());
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_char(self, v: char) -> Result<(), Self::Error> {
|
||||
let mut buf = [0; 4];
|
||||
self.0.write(v.encode_utf8(&mut buf).as_bytes());
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_str(self, v: &str) -> Result<(), Self::Error> {
|
||||
self.0.write(v.as_bytes());
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_bytes(self, v: &[u8]) -> Result<(), Self::Error> {
|
||||
self.0.write(v);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_none(self) -> Result<(), Self::Error> {
|
||||
self.0.write_u8(0); // type tag for None
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_some<T: ?Sized + Serialize>(self, value: &T) -> Result<(), Self::Error> {
|
||||
self.0.write_u8(1); // type tag for Some
|
||||
value.serialize(self)
|
||||
}
|
||||
|
||||
fn serialize_unit(self) -> Result<(), Self::Error> {
|
||||
self.0.write_u8(0);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_unit_struct(self, _name: &'static str) -> Result<(), Self::Error> {
|
||||
self.serialize_unit()
|
||||
}
|
||||
|
||||
fn serialize_unit_variant(
|
||||
self,
|
||||
_name: &'static str,
|
||||
variant_index: u32,
|
||||
_variant: &'static str,
|
||||
) -> Result<(), Self::Error> {
|
||||
self.0.write_u32(variant_index);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn serialize_newtype_struct<T: ?Sized + Serialize>(
|
||||
self,
|
||||
_name: &'static str,
|
||||
value: &T,
|
||||
) -> Result<(), Self::Error> {
|
||||
value.serialize(self)
|
||||
}
|
||||
|
||||
fn serialize_newtype_variant<T: ?Sized + Serialize>(
|
||||
self,
|
||||
_name: &'static str,
|
||||
variant_index: u32,
|
||||
_variant: &'static str,
|
||||
value: &T,
|
||||
) -> Result<(), Self::Error> {
|
||||
self.0.write_u32(variant_index);
|
||||
value.serialize(self)
|
||||
}
|
||||
|
||||
// For sequences, tuples, maps, structs, just return self for now
|
||||
fn serialize_seq(self, _len: Option<usize>) -> Result<Self::SerializeSeq, Self::Error> {
|
||||
Ok(self)
|
||||
}
|
||||
fn serialize_tuple(self, _len: usize) -> Result<Self::SerializeTuple, Self::Error> {
|
||||
Ok(self)
|
||||
}
|
||||
fn serialize_tuple_struct(
|
||||
self,
|
||||
_name: &'static str,
|
||||
_len: usize,
|
||||
) -> Result<Self::SerializeTupleStruct, Self::Error> {
|
||||
Ok(self)
|
||||
}
|
||||
fn serialize_tuple_variant(
|
||||
self,
|
||||
_name: &'static str,
|
||||
variant_index: u32,
|
||||
_variant: &'static str,
|
||||
_len: usize,
|
||||
) -> Result<Self::SerializeTupleVariant, Self::Error> {
|
||||
self.0.write_u32(variant_index);
|
||||
Ok(self)
|
||||
}
|
||||
fn serialize_map(self, _len: Option<usize>) -> Result<Self::SerializeMap, Self::Error> {
|
||||
Ok(self)
|
||||
}
|
||||
fn serialize_struct(
|
||||
self,
|
||||
_name: &'static str,
|
||||
_len: usize,
|
||||
) -> Result<Self::SerializeStruct, Self::Error> {
|
||||
Ok(self)
|
||||
}
|
||||
fn serialize_struct_variant(
|
||||
self,
|
||||
_name: &'static str,
|
||||
variant_index: u32,
|
||||
_variant: &'static str,
|
||||
_len: usize,
|
||||
) -> Result<Self::SerializeStructVariant, Self::Error> {
|
||||
self.0.write_u32(variant_index);
|
||||
Ok(self)
|
||||
}
|
||||
}
|
||||
|
||||
// A helper that uses Serialize to feed into a Hasher without allocating a buffer:
|
||||
pub fn hash_with_serde_model<T: Serialize>(value: T) -> u64 {
|
||||
let mut h = DefaultHasher::new();
|
||||
let ser = HashSerializer(&mut h);
|
||||
value.serialize(ser).expect("serialize to hasher failed");
|
||||
h.finish()
|
||||
}
|
||||
@@ -1,108 +1,40 @@
|
||||
/// Folds bytes using XOR to produce a u128 value.
|
||||
///
|
||||
/// Takes a slice of bytes and XORs them together in chunks to produce a u128.
|
||||
/// If there are fewer than 16 bytes, the remaining bytes are padded with zeros.
|
||||
pub fn byte_folding_u128(bytes: &[u8]) -> u128 {
|
||||
let mut result = 0u128;
|
||||
for (i, &byte) in bytes.iter().enumerate() {
|
||||
let shift = (i % 16) * 8;
|
||||
result ^= (byte as u128) << shift;
|
||||
}
|
||||
result
|
||||
pub fn xor_u128_to_u64(value: u128) -> u64 {
|
||||
(value as u64) ^ ((value >> 64) as u64)
|
||||
}
|
||||
|
||||
/// Folds bytes using XOR to produce a u64 value.
|
||||
///
|
||||
/// Takes a slice of bytes and XORs them together in chunks to produce a u64.
|
||||
/// If there are fewer than 8 bytes, the remaining bytes are padded with zeros.
|
||||
pub fn byte_folding_u64(bytes: &[u8]) -> u64 {
|
||||
let mut result = 0u64;
|
||||
for (i, &byte) in bytes.iter().enumerate() {
|
||||
let shift = (i % 8) * 8;
|
||||
result ^= (byte as u64) << shift;
|
||||
}
|
||||
result
|
||||
pub fn xor_u128_to_u32(value: u128) -> u32 {
|
||||
(value as u32) ^ ((value >> 32) as u32) ^ ((value >> 64) as u32) ^ ((value >> 96) as u32)
|
||||
}
|
||||
|
||||
/// Folds bytes using XOR to produce a u32 value.
|
||||
///
|
||||
/// Takes a slice of bytes and XORs them together in chunks to produce a u32.
|
||||
/// If there are fewer than 4 bytes, the remaining bytes are padded with zeros.
|
||||
pub fn byte_folding_u32(bytes: &[u8]) -> u32 {
|
||||
let mut result = 0u32;
|
||||
for (i, &byte) in bytes.iter().enumerate() {
|
||||
let shift = (i % 4) * 8;
|
||||
result ^= (byte as u32) << shift;
|
||||
}
|
||||
result
|
||||
pub fn xor_u128_to_u16(value: u128) -> u16 {
|
||||
value
|
||||
.to_le_bytes()
|
||||
.iter()
|
||||
.fold(0, |buf, &byte| buf ^ byte as u16)
|
||||
}
|
||||
|
||||
/// Folds bytes using XOR to produce a u16 value.
|
||||
///
|
||||
/// Takes a slice of bytes and XORs them together in chunks to produce a u16.
|
||||
/// If there are fewer than 2 bytes, the remaining bytes are padded with zeros.
|
||||
pub fn byte_folding_u16(bytes: &[u8]) -> u16 {
|
||||
let mut result = 0u16;
|
||||
for (i, &byte) in bytes.iter().enumerate() {
|
||||
let shift = (i % 2) * 8;
|
||||
result ^= (byte as u16) << shift;
|
||||
}
|
||||
result
|
||||
pub fn xor_u128_to_u8(value: u128) -> u8 {
|
||||
value.to_le_bytes().iter().fold(0, |buf, &byte| buf ^ byte)
|
||||
}
|
||||
|
||||
/// Folds bytes using XOR to produce a u8 value.
|
||||
///
|
||||
/// Takes a slice of bytes and XORs them all together to produce a u8.
|
||||
pub fn byte_folding_u8(bytes: &[u8]) -> u8 {
|
||||
bytes.iter().fold(0u8, |acc, &byte| acc ^ byte)
|
||||
}
|
||||
// hash algorithm djb2
|
||||
pub fn hashing(input_to_hash: &str) -> u128 {
|
||||
let mut hash: u128 = 5381;
|
||||
let chars = input_to_hash.chars();
|
||||
|
||||
/// Computes a djb2 hash of the input bytes.
|
||||
///
|
||||
/// The djb2 algorithm is a simple non-cryptographic hash function created by
|
||||
/// Daniel J. Bernstein. It uses the formula: `hash = hash * 33 + byte` for each byte.
|
||||
///
|
||||
/// This implementation uses the traditional 32-bit hash value and uses wrapping
|
||||
/// arithmetic to handle potential overflows gracefully.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `bytes` - A slice of bytes to hash
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// A 32-bit hash value as a `u32`
|
||||
///
|
||||
/// # Examples
|
||||
///
|
||||
/// ```
|
||||
/// use control_core::helpers::hashing::hash_djb2;
|
||||
///
|
||||
/// let hash = hash_djb2(b"hello world");
|
||||
/// assert_ne!(hash, 0);
|
||||
/// ```
|
||||
///
|
||||
/// # Algorithm Details
|
||||
///
|
||||
/// - Initial hash value: 5381 (djb2 magic number)
|
||||
/// - For each byte: `hash = hash * 33 + byte`
|
||||
/// - Uses wrapping arithmetic to prevent overflow panics
|
||||
pub fn hash_djb2(bytes: &[u8]) -> u32 {
|
||||
let mut hash: u32 = 5381;
|
||||
|
||||
for byte in bytes {
|
||||
hash = hash.wrapping_mul(33).wrapping_add(*byte as u32);
|
||||
for ch in chars {
|
||||
hash = ((hash << 5) + hash) + (ch as u128);
|
||||
}
|
||||
hash
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_hash() {
|
||||
let test_case = "/dev/ttyUSB1";
|
||||
let hash_u32 = hash_djb2(test_case.as_bytes());
|
||||
let hash_u16 = byte_folding_u16(&hash_u32.to_le_bytes());
|
||||
let hash = hashing(test_case);
|
||||
let hash_2bytes = xor_u128_to_u16(hash);
|
||||
tracing::info!("{}", test_case);
|
||||
assert_eq!(test_case, "/dev/ttyUSB1");
|
||||
assert_eq!(hash_u32, 266121182);
|
||||
assert_eq!(hash_u16, 40962);
|
||||
assert!(hash == 8977446972540388683742);
|
||||
assert!(hash_2bytes == 165);
|
||||
println!("{:?}", hash);
|
||||
}
|
||||
@@ -1,26 +1,4 @@
|
||||
/// Normalizes a value from a given range [min, max] to the range [0, 1].
|
||||
///
|
||||
/// This function maps a value from an arbitrary range to the standard
|
||||
/// normalized range [0, 1]. Values o/// Plot for Steepness = -1:ge are clamped
|
||||
/// to the boundaries (0.0 or 1.0).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `value` - The input value to normalize
|
||||
/// * `min` - The minimum value of the input range
|
||||
/// * `max` - The maximum value of the input range
|
||||
///
|
||||
/// # Returns
|
||||
/// * Normalized value in range [0.0, 1.0]
|
||||
///
|
||||
/// # Panics
|
||||
/// * Panics if `min >= max`
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use control_core::helpers::interpolation::normalize;
|
||||
/// let normalized = normalize(5.0, 0.0, 10.0); // Returns 0.5
|
||||
/// let clamped = normalize(-2.0, 0.0, 10.0); // Returns 0.0 (clamped)
|
||||
/// ```
|
||||
pub fn normalize<T>(value: T, min: T, max: T) -> f64
|
||||
where
|
||||
T: Into<f64>,
|
||||
@@ -41,425 +19,36 @@ where
|
||||
}
|
||||
|
||||
/// Scales a normalized value from the range [0, 1] to a new range [new_min, new_max].
|
||||
///
|
||||
/// This function takes a normalized value (typically from 0 to 1) and maps it
|
||||
/// to a new target range. It's the inverse operation of normalization.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `normalized_value` - Input value in range [0.0, 1.0]
|
||||
/// * `new_min` - The minimum value of the target range
|
||||
/// * `new_max` - The maximum value of the target range
|
||||
///
|
||||
/// # Returns
|
||||
/// * Scaled value in the range [new_min, new_max]
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use control_core::helpers::interpolation::scale;
|
||||
/// let scaled = scale(0.5, 0.0, 100.0); // Returns 50.0
|
||||
/// let negative_range = scale(0.25, -10.0, 10.0); // Returns -5.0
|
||||
/// ```
|
||||
pub fn scale<T>(normalized_value: f64, new_min: T, new_max: T) -> T
|
||||
where
|
||||
T: Into<f64> + From<f64>,
|
||||
{
|
||||
let normalized_value: f64 = normalized_value;
|
||||
let normalized_value: f64 = normalized_value.into();
|
||||
let new_min: f64 = new_min.into();
|
||||
let new_max: f64 = new_max.into();
|
||||
|
||||
let result = normalized_value.mul_add(new_max - new_min, new_min);
|
||||
let result = normalized_value * (new_max - new_min) + new_min;
|
||||
T::from(result)
|
||||
}
|
||||
|
||||
/// Performs hinge interpolation creating a piecewise linear function.
|
||||
///
|
||||
/// This function creates a piecewise linear curve that:
|
||||
/// - For values < x: linearly interpolates from (0, 0) to (x, y)
|
||||
/// - For values > x: linearly interpolates from (x, y) to (1, 1)
|
||||
/// - For value == x: returns y
|
||||
///
|
||||
/// The function assumes the input value is normalized to the range [0, 1],
|
||||
/// and creates a "hinge" or "elbow" shape at the point (x, y).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `value` - The input value (typically in range [0, 1])
|
||||
/// * `x` - The x-coordinate of the hinge point (typically in range [0, 1])
|
||||
/// * `y` - The y-coordinate of the hinge point (typically in range [0, 1])
|
||||
///
|
||||
/// # Returns
|
||||
/// * Interpolated value based on the hinge function
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use control_core::helpers::interpolation::interpolate_hinge;
|
||||
/// // Create a hinge at (0.3, 0.7)
|
||||
/// let result1 = interpolate_hinge(0.15, 0.3, 0.7); // Returns 0.35 (halfway to hinge)
|
||||
/// let result2 = interpolate_hinge(0.3, 0.3, 0.7); // Returns 0.7 (at hinge)
|
||||
/// let result3 = interpolate_hinge(0.65, 0.3, 0.7); // Returns 0.85 (halfway from hinge to 1)
|
||||
/// ```
|
||||
/// ```ignore
|
||||
/// Hinge at (0.8, 0.2)
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡇⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⣀⠤⠤⠒⠒⠊⠁⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⣀⣀⡠⠤⠒⠒⠉⠉⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⣀⣀⠤⠤⠔⠒⠉⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠉⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
pub fn interpolate_hinge<T>(value: T, x: T, y: T) -> f64
|
||||
where
|
||||
T: Into<f64>,
|
||||
{
|
||||
let value_f64: f64 = value.into();
|
||||
let x_f64: f64 = x.into();
|
||||
let y_f64: f64 = y.into();
|
||||
|
||||
// If we are under X we scale linearly from 0 to y
|
||||
// If we are over X we scale linearly from y to 1
|
||||
if value_f64 < x_f64 {
|
||||
normalize(value_f64, 0.0, x_f64) * y_f64
|
||||
} else if value_f64 > x_f64 {
|
||||
normalize(value_f64, x_f64, 1.0).mul_add(1.0 - y_f64, y_f64)
|
||||
} else {
|
||||
y_f64
|
||||
}
|
||||
}
|
||||
|
||||
/// Transforms a normalized value from the range [0, 1] to a [0, 1] range using a normalized exponential function.
|
||||
///
|
||||
/// This function creates smooth curves that are either convex (fast start, slow end)
|
||||
/// or concave (slow start, fast end) depending on the steepness parameter sign.
|
||||
/// The curve always passes through (0,0) and (1,1).
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `normalized_value` - Input value in range [0.0, 1.0]
|
||||
/// * `steepness` - Controls the curve shape:
|
||||
/// - `steepness = 0.0`: Linear interpolation (y = x)
|
||||
/// - `steepness > 0.0`: Convex curve (slow start → fast end)
|
||||
/// - `steepness < 0.0`: Concave curve (fast start → slow end)
|
||||
/// - Typical range: -5.0 to 5.0 for most applications
|
||||
///
|
||||
/// # Returns
|
||||
/// * Transformed value in range [0.0, 1.0]
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use control_core::helpers::interpolation::interpolate_exponential;
|
||||
/// let convex = interpolate_exponential(0.5, 2.0); // Slow start, fast end
|
||||
/// let concave = interpolate_exponential(0.5, -2.0); // Fast start, slow end
|
||||
/// let linear = interpolate_exponential(0.5, 0.0); // Linear (returns 0.5)
|
||||
/// ```
|
||||
///
|
||||
/// # Plots
|
||||
/// Steepness = -3
|
||||
/// Steepness = -3
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡠⠤⠔⠒⠒⠉⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠤⠔⠊⠉⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⡰⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⡜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⢰⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⢀⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⢤⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ -0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
///
|
||||
/// Steepness = -1
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠒⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠁⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠒⠁⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⢀⠜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⢀⠎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⢀⠜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡲⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ -0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
///
|
||||
/// Steepness = 0
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⢂⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
///
|
||||
/// Steepness = 1
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠃⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠊⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⢀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⢂⣀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
///
|
||||
/// Steepness = 3
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡎⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⣀⠤⠔⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⣀⣀⠤⠤⠒⠒⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠉⠉⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
pub fn interpolate_exponential(normalized_value: f64, steepness: f64) -> f64 {
|
||||
if steepness.abs() < 1e-10 {
|
||||
/// Transforma normalized value from the range [0, 1] to a [0, 1] range using an normalized exponential function.
|
||||
pub fn interpolate_exponential(normalized_value: f64, a: f64) -> f64 {
|
||||
if a.abs() < 1e-10 {
|
||||
// When a is very close to zero, the function becomes linear
|
||||
return normalized_value;
|
||||
}
|
||||
let numerator = steepness.exp().powf(normalized_value) - 1.0;
|
||||
let denominator = steepness.exp_m1();
|
||||
let numerator = a.exp().powf(normalized_value) - 1.0;
|
||||
let denominator = a.exp() - 1.0;
|
||||
|
||||
clip(numerator / denominator)
|
||||
}
|
||||
|
||||
/// A piecewise exponential interpolation function that creates smooth S-curves
|
||||
/// through the points (0,0), (0.5,0.5), and (1,1).
|
||||
///
|
||||
/// The function combines two normalized exponential segments:
|
||||
/// - First half (0 to 0.5): inverted exponential (concave, fast start → slow end)
|
||||
/// - Second half (0.5 to 1): standard exponential (convex, slow start → fast end)
|
||||
///
|
||||
/// This creates an S-like curve that's concave then convex, similar to
|
||||
/// a smoothed step function or sigmoid-like transition.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `x` - Input value, should be in range [0.0, 1.0]
|
||||
/// * `steepness` - Controls how steep the S-curve is
|
||||
/// - `steepness = 0.0`: Linear interpolation (y = x)
|
||||
/// - `steepness > 0.0`: More pronounced S-curve
|
||||
/// - Higher values create sharper transitions at the boundaries
|
||||
/// - Typical range: 0.5 to 5.0 for most applications
|
||||
///
|
||||
/// # Returns
|
||||
/// * Interpolated y value in range [0.0, 1.0]
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use control_core::helpers::interpolation::interpolate_inflected_exponential;
|
||||
/// let y = interpolate_inflected_exponential(0.25, 2.0); // Gentle S-curve
|
||||
/// let y = interpolate_inflected_exponential(0.75, 5.0); // Sharp S-curve
|
||||
/// ```
|
||||
///
|
||||
/// # Plots
|
||||
/// Steepness = 0.5
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠉⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠜⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠜⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠒⠁⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⡔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⢀⠤⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡢⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
///
|
||||
/// Steepness = 1
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢠⠒⠁⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⡠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⡰⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⡰⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⡜⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡲⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
///
|
||||
/// Steepness = 2
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡠⠃⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡸⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡰⠉⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠊⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⡠⠤⠒⠊⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⢀⠤⠔⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⣀⠔⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⡠⠊⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⢠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⡠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡞⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
///
|
||||
/// Steepness = 5
|
||||
/// ```ignore
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢰⠁ 1.0
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠇⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢀⠎⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⡜⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⣀⠔⠉⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⠀⠀⠀⠀⠀⠀⠀⣀⣀⡠⠤⠤⠤⠤⠤⠤⠤⠒⠒⠊⠉⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⠀⠀⠀⠀⢀⠔⠒⠉⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡁⠀⠀⢠⠒⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠄⠀⢠⠃⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠂⢀⠇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⣁⠎⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⢼⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⡇⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀
|
||||
/// ⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁⠈⠀⠁ 0.0
|
||||
/// 0.0 1.0
|
||||
/// ```
|
||||
pub fn interpolate_inflected_exponential(x: f64, steepness: f64) -> f64 {
|
||||
debug_assert!((0.0..=1.0).contains(&x), "x must be in range [0.0, 1.0]");
|
||||
debug_assert!(steepness >= 0.0, "steepness must be non-negative");
|
||||
|
||||
if steepness == 0.0 {
|
||||
// Linear case
|
||||
return x;
|
||||
}
|
||||
|
||||
if x <= 0.5 {
|
||||
// First segment: inverted exponential (concave)
|
||||
// Maps [0, 0.5] → [0, 0.5] with concave shape
|
||||
let x_norm = 2.0 * x; // Scale to [0, 1]
|
||||
|
||||
// Use inverted exponential interpolation
|
||||
let result = 1.0 - interpolate_exponential(1.0 - x_norm, steepness);
|
||||
0.5 * result
|
||||
} else {
|
||||
// Second segment: standard exponential (convex)
|
||||
// Maps [0.5, 1] → [0.5, 1] with convex shape
|
||||
let x_norm = 2.0 * (x - 0.5); // Scale to [0, 1]
|
||||
|
||||
// Use standard exponential interpolation
|
||||
let result = interpolate_exponential(x_norm, steepness);
|
||||
0.5f64.mul_add(result, 0.5)
|
||||
}
|
||||
}
|
||||
|
||||
/// Inverts a value from the range [0, 1] to [1, 0].
|
||||
///
|
||||
/// This function flips a normalized value, where 0 becomes 1 and 1 becomes 0.
|
||||
/// Values outside [0, 1] are first clamped to this range before inversion.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `value` - Input value (will be clamped to [0.0, 1.0])
|
||||
///
|
||||
/// # Returns
|
||||
/// * Inverted value in range [0.0, 1.0]
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use control_core::helpers::interpolation::invert;
|
||||
/// let inverted = invert(0.3); // Returns 0.7
|
||||
/// let boundary = invert(0.0); // Returns 1.0
|
||||
/// let clamped = invert(1.5); // Returns 0.0 (input clamped to 1.0 first)
|
||||
/// ```
|
||||
pub fn invert(value: f64) -> f64 {
|
||||
let value = clip(value);
|
||||
1.0 - value
|
||||
}
|
||||
|
||||
/// Clips a value to the range [0, 1].
|
||||
///
|
||||
/// This function ensures that any input value is constrained to the
|
||||
/// normalized range [0, 1] by clamping values below 0 to 0 and
|
||||
/// values above 1 to 1.
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `value` - Input value to clip
|
||||
///
|
||||
/// # Returns
|
||||
/// * Clipped value in range [0.0, 1.0]
|
||||
///
|
||||
/// # Example
|
||||
/// ```rust
|
||||
/// use control_core::helpers::interpolation::clip;
|
||||
/// let normal = clip(0.5); // Returns 0.5
|
||||
/// let low = clip(-0.3); // Returns 0.0
|
||||
/// let high = clip(1.7); // Returns 1.0
|
||||
/// ```
|
||||
pub fn clip(value: f64) -> f64 {
|
||||
if value < 0.0 {
|
||||
return 0.0;
|
||||
@@ -589,289 +178,6 @@ mod tests {
|
||||
assert_relative_eq!(clip(1.5), 1.0, epsilon = EPSILON);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_interpolate_inflected_exponential() {
|
||||
// Test boundary points (0,0), (0.5,0.5), and (1,1) for various steepness values
|
||||
for steepness in [0.0, 0.5, 1.0, 2.0, 5.0].iter() {
|
||||
assert_relative_eq!(
|
||||
interpolate_inflected_exponential(0.0, *steepness),
|
||||
0.0,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
assert_relative_eq!(
|
||||
interpolate_inflected_exponential(0.5, *steepness),
|
||||
0.5,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
assert_relative_eq!(
|
||||
interpolate_inflected_exponential(1.0, *steepness),
|
||||
1.0,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
}
|
||||
|
||||
// Test linear case (steepness = 0)
|
||||
for x in [0.0, 0.25, 0.5, 0.75, 1.0].iter() {
|
||||
assert_relative_eq!(
|
||||
interpolate_inflected_exponential(*x, 0.0),
|
||||
*x,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
}
|
||||
|
||||
// Test monotonicity - function should be strictly increasing
|
||||
for steepness in [0.5, 1.0, 2.0, 5.0].iter() {
|
||||
let mut prev = interpolate_inflected_exponential(0.0, *steepness);
|
||||
for i in 1..=20 {
|
||||
let x = i as f64 / 20.0;
|
||||
let current = interpolate_inflected_exponential(x, *steepness);
|
||||
assert!(
|
||||
current >= prev,
|
||||
"Failed monotonicity test at x={}, steepness={}, prev={}, current={}",
|
||||
x,
|
||||
steepness,
|
||||
prev,
|
||||
current
|
||||
);
|
||||
prev = current;
|
||||
}
|
||||
}
|
||||
|
||||
// Test S-curve property: first half should be concave, second half convex
|
||||
let steepness = 2.0;
|
||||
|
||||
// First half: check concave property (derivative decreasing)
|
||||
let x1 = 0.1;
|
||||
let x2 = 0.2;
|
||||
let x3 = 0.3;
|
||||
let y1 = interpolate_inflected_exponential(x1, steepness);
|
||||
let y2 = interpolate_inflected_exponential(x2, steepness);
|
||||
let y3 = interpolate_inflected_exponential(x3, steepness);
|
||||
|
||||
let slope1 = (y2 - y1) / (x2 - x1);
|
||||
let slope2 = (y3 - y2) / (x3 - x2);
|
||||
assert!(
|
||||
slope1 > slope2,
|
||||
"First half should be concave (decreasing slope)"
|
||||
);
|
||||
|
||||
// Second half: check convex property (derivative increasing)
|
||||
let x1 = 0.7;
|
||||
let x2 = 0.8;
|
||||
let x3 = 0.9;
|
||||
let y1 = interpolate_inflected_exponential(x1, steepness);
|
||||
let y2 = interpolate_inflected_exponential(x2, steepness);
|
||||
let y3 = interpolate_inflected_exponential(x3, steepness);
|
||||
|
||||
let slope1 = (y2 - y1) / (x2 - x1);
|
||||
let slope2 = (y3 - y2) / (x3 - x2);
|
||||
assert!(
|
||||
slope1 < slope2,
|
||||
"Second half should be convex (increasing slope)"
|
||||
);
|
||||
|
||||
// Test that output is always in [0, 1] range
|
||||
for steepness in [0.0, 1.0, 5.0, 10.0].iter() {
|
||||
for i in 0..=100 {
|
||||
let x = i as f64 / 100.0;
|
||||
let y = interpolate_inflected_exponential(x, *steepness);
|
||||
assert!(
|
||||
y >= 0.0 && y <= 1.0,
|
||||
"Output {} out of range [0,1] for x={}, steepness={}",
|
||||
y,
|
||||
x,
|
||||
steepness
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_interpolate_hinge() {
|
||||
// Test basic functionality with hinge at (0.3, 0.7)
|
||||
let x_hinge = 0.3;
|
||||
let y_hinge = 0.7;
|
||||
|
||||
// Test exact hinge point
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(x_hinge, x_hinge, y_hinge),
|
||||
y_hinge,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
|
||||
// Test left segment (value < x): should interpolate from (0,0) to (x,y)
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.0, x_hinge, y_hinge),
|
||||
0.0,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.15, x_hinge, y_hinge),
|
||||
0.35, // 0.15/0.3 * 0.7 = 0.5 * 0.7 = 0.35
|
||||
epsilon = EPSILON
|
||||
);
|
||||
|
||||
// Test right segment (value > x): should interpolate from (x,y) to (1,1)
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(1.0, x_hinge, y_hinge),
|
||||
1.0,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.65, x_hinge, y_hinge),
|
||||
0.85, // y + ((0.65-0.3)/(1.0-0.3)) * (1.0-0.7) = 0.7 + 0.5 * 0.3 = 0.85
|
||||
epsilon = EPSILON
|
||||
);
|
||||
|
||||
// Test with different hinge points
|
||||
// Hinge at (0.5, 0.5) - should be linear
|
||||
assert_relative_eq!(interpolate_hinge(0.25, 0.5, 0.5), 0.25, epsilon = EPSILON);
|
||||
assert_relative_eq!(interpolate_hinge(0.75, 0.5, 0.5), 0.75, epsilon = EPSILON);
|
||||
|
||||
// Hinge at (0.2, 0.9) - steep left, shallow right
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.1, 0.2, 0.9),
|
||||
0.45, // 0.1/0.2 * 0.9 = 0.5 * 0.9 = 0.45
|
||||
epsilon = EPSILON
|
||||
);
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.6, 0.2, 0.9),
|
||||
0.95, // 0.9 + ((0.6-0.2)/(1.0-0.2)) * (1.0-0.9) = 0.9 + 0.5 * 0.1 = 0.95
|
||||
epsilon = EPSILON
|
||||
);
|
||||
|
||||
// Test edge cases
|
||||
// Hinge at (0, 0)
|
||||
assert_relative_eq!(interpolate_hinge(0.0, 0.0, 0.0), 0.0, epsilon = EPSILON);
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.5, 0.0, 0.0),
|
||||
0.5, // Should interpolate from (0,0) to (1,1)
|
||||
epsilon = EPSILON
|
||||
);
|
||||
|
||||
// Hinge at (1, 1)
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.5, 1.0, 1.0),
|
||||
0.5, // Should interpolate from (0,0) to (1,1)
|
||||
epsilon = EPSILON
|
||||
);
|
||||
assert_relative_eq!(interpolate_hinge(1.0, 1.0, 1.0), 1.0, epsilon = EPSILON);
|
||||
|
||||
// Test with different numeric types
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(0.5f32, 0.3f32, 0.7f32),
|
||||
0.7857142857142857, // 0.7 + ((0.5-0.3)/(1.0-0.3)) * (1.0-0.7) = 0.7 + (2/7) * 0.3
|
||||
epsilon = 1e-7 // Relaxed epsilon for f32 precision
|
||||
);
|
||||
assert_relative_eq!(
|
||||
interpolate_hinge(1u8, 0u8, 1u8), // 1.0, 0.0, 1.0 - should return 1.0
|
||||
1.0,
|
||||
epsilon = EPSILON
|
||||
);
|
||||
|
||||
// Test monotonicity - function should be non-decreasing
|
||||
let test_x = 0.4;
|
||||
let test_y = 0.6;
|
||||
let mut prev = interpolate_hinge(0.0, test_x, test_y);
|
||||
|
||||
for i in 1..=20 {
|
||||
let value = i as f64 / 20.0;
|
||||
let current = interpolate_hinge(value, test_x, test_y);
|
||||
assert!(
|
||||
current >= prev,
|
||||
"Failed monotonicity test at value={}, x={}, y={}, prev={}, current={}",
|
||||
value,
|
||||
test_x,
|
||||
test_y,
|
||||
prev,
|
||||
current
|
||||
);
|
||||
prev = current;
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_interpolate_exponential_graph() {
|
||||
use textplots::{Chart, Plot, Shape};
|
||||
|
||||
println!("\n=== Exponential Interpolation Examples ===");
|
||||
|
||||
// Test different steepness values (both positive and negative)
|
||||
let steepness_values = [-3.0, -1.0, 0.0, 1.0, 3.0];
|
||||
|
||||
for steepness in steepness_values.iter() {
|
||||
println!("\nSteepness = {}", steepness);
|
||||
|
||||
// Generate data points
|
||||
let points: Vec<(f32, f32)> = (0..=50)
|
||||
.map(|i| {
|
||||
let x = i as f64 / 50.0;
|
||||
let y = interpolate_exponential(x, *steepness);
|
||||
(x as f32, y as f32)
|
||||
})
|
||||
.collect();
|
||||
|
||||
// Create and display chart
|
||||
Chart::new(60, 60, 0.0, 1.0)
|
||||
.lineplot(&Shape::Lines(&points))
|
||||
.display();
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_interpolate_hinge_graph() {
|
||||
use textplots::{Chart, Plot, Shape};
|
||||
|
||||
println!("\n=== hinge Interpolation Examples ===");
|
||||
|
||||
// Test different hinge points
|
||||
let hinges = [(0.3, 0.7), (0.5, 0.5), (0.2, 0.9), (0.8, 0.2)];
|
||||
|
||||
for &(x_hinge, y_hinge) in hinges.iter() {
|
||||
println!("\nHinge at ({}, {})", x_hinge, y_hinge);
|
||||
|
||||
// Generate data points
|
||||
let points: Vec<(f32, f32)> = (0..=50)
|
||||
.map(|i| {
|
||||
let x = i as f64 / 50.0;
|
||||
let y = interpolate_hinge(x, x_hinge, y_hinge);
|
||||
(x as f32, y as f32)
|
||||
})
|
||||
.collect();
|
||||
|
||||
// Create and display chart
|
||||
Chart::new(60, 60, 0.0, 1.0)
|
||||
.lineplot(&Shape::Lines(&points))
|
||||
.display();
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_interpolate_inflected_exponential_graph() {
|
||||
use textplots::{Chart, Plot, Shape};
|
||||
|
||||
// Test different steepness values
|
||||
let steepness_values = [0.5, 1.0, 2.0, 5.0];
|
||||
|
||||
for steepness in steepness_values.iter() {
|
||||
println!("\nSteepness = {}", steepness);
|
||||
|
||||
// Generate data points
|
||||
let points: Vec<(f32, f32)> = (0..=50)
|
||||
.map(|i| {
|
||||
let x = i as f64 / 50.0;
|
||||
let y = interpolate_inflected_exponential(x, *steepness);
|
||||
(x as f32, y as f32)
|
||||
})
|
||||
.collect();
|
||||
|
||||
// Create and display chart
|
||||
Chart::new(60, 60, 0.0, 1.0)
|
||||
.lineplot(&Shape::Lines(&points))
|
||||
.display();
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[should_panic]
|
||||
fn test_normalize_invalid_range() {
|
||||
|
||||
@@ -0,0 +1,190 @@
|
||||
use smol::Timer;
|
||||
use std::time::{Duration, Instant};
|
||||
use tracing::instrument;
|
||||
|
||||
pub struct LoopThrottle {
|
||||
target_cycle_time: Duration,
|
||||
update_interval_cycles: usize,
|
||||
window_start: Instant,
|
||||
total_cycles: usize,
|
||||
delay: Duration,
|
||||
last_avg_cycle_time: Duration,
|
||||
on_update: Option<Box<dyn Fn(LoopThrottleUpdate)>>,
|
||||
}
|
||||
|
||||
pub struct LoopThrottleUpdate {
|
||||
pub delay: Duration,
|
||||
pub avg_cycle_time: Duration,
|
||||
pub error: i128, // Error in nanoseconds
|
||||
}
|
||||
impl LoopThrottle {
|
||||
pub fn new(
|
||||
target_cycle_time: Duration,
|
||||
update_interval_cycles: usize,
|
||||
on_update: Option<Box<dyn Fn(LoopThrottleUpdate)>>,
|
||||
) -> Self {
|
||||
Self {
|
||||
target_cycle_time,
|
||||
update_interval_cycles,
|
||||
window_start: Instant::now(),
|
||||
total_cycles: 0,
|
||||
delay: target_cycle_time,
|
||||
last_avg_cycle_time: target_cycle_time,
|
||||
on_update,
|
||||
}
|
||||
}
|
||||
|
||||
#[instrument(skip_all)]
|
||||
pub async fn sleep(&mut self) {
|
||||
self.total_cycles += 1;
|
||||
|
||||
// Check if it's time to update (every N cycles)
|
||||
if self.total_cycles % self.update_interval_cycles == 0 {
|
||||
let actual_total_time = self.window_start.elapsed();
|
||||
|
||||
// Use integer arithmetic where possible
|
||||
let avg_actual_cycle_time_nanos =
|
||||
actual_total_time.as_nanos() / self.update_interval_cycles as u128;
|
||||
let target_cycle_time_nanos = self.target_cycle_time.as_nanos();
|
||||
|
||||
// Calculate error in nanoseconds (signed)
|
||||
let error_nanos = target_cycle_time_nanos as i128 - avg_actual_cycle_time_nanos as i128;
|
||||
|
||||
// Update current delay, allowing zero or negative values to become zero
|
||||
let new_delay_nanos = (self.delay.as_nanos() as i128 + error_nanos).max(0) as u64;
|
||||
|
||||
self.delay = Duration::from_nanos(new_delay_nanos);
|
||||
self.last_avg_cycle_time = Duration::from_nanos(avg_actual_cycle_time_nanos as u64);
|
||||
|
||||
// Reset for next measurement window
|
||||
self.window_start = Instant::now();
|
||||
self.total_cycles = 0; // Reset to prevent overflow
|
||||
|
||||
// if we have an update callback, call it
|
||||
if let Some(on_update) = &self.on_update {
|
||||
on_update(LoopThrottleUpdate {
|
||||
delay: self.delay,
|
||||
avg_cycle_time: self.last_avg_cycle_time,
|
||||
error: error_nanos,
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
Timer::after(self.delay).await;
|
||||
}
|
||||
|
||||
pub fn get_cycle_time(&self) -> Duration {
|
||||
self.last_avg_cycle_time
|
||||
}
|
||||
|
||||
pub fn get_delay(&self) -> Duration {
|
||||
self.delay
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use smol::block_on;
|
||||
|
||||
#[test]
|
||||
fn test_adaptive_throttle_timing() {
|
||||
block_on(async {
|
||||
let update_interval_cycles = 100;
|
||||
let target_cycle_time = Duration::from_micros(5);
|
||||
let mut throttle = LoopThrottle::new(target_cycle_time, update_interval_cycles, None);
|
||||
|
||||
let test_start = Instant::now();
|
||||
let mut cycle_times = Vec::new();
|
||||
|
||||
// Run for several cycles to trigger multiple updates
|
||||
for _ in 0..10000 {
|
||||
let cycle_start = Instant::now();
|
||||
|
||||
// Simulate some work
|
||||
Timer::after(Duration::from_micros(5)).await;
|
||||
|
||||
throttle.sleep().await;
|
||||
|
||||
let cycle_time = cycle_start.elapsed();
|
||||
cycle_times.push(cycle_time);
|
||||
}
|
||||
|
||||
let total_test_time = test_start.elapsed();
|
||||
let avg_cycle_time = cycle_times.iter().sum::<Duration>() / cycle_times.len() as u32;
|
||||
|
||||
println!("Test completed in: {:?}", total_test_time);
|
||||
println!("Average cycle time: {:?}", avg_cycle_time);
|
||||
println!("Target cycle time: {:?}", target_cycle_time);
|
||||
println!("Last measured avg: {:?}", throttle.get_cycle_time());
|
||||
|
||||
// Assert that we're reasonably close to target timing
|
||||
let tolerance = Duration::from_millis(10);
|
||||
assert!(
|
||||
avg_cycle_time >= target_cycle_time.saturating_sub(tolerance)
|
||||
&& avg_cycle_time <= target_cycle_time + tolerance,
|
||||
"Average cycle time {:?} not within tolerance of target {:?}",
|
||||
avg_cycle_time,
|
||||
target_cycle_time
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_throttle_adaptation() {
|
||||
block_on(async {
|
||||
let target_cycle_time = Duration::from_millis(100);
|
||||
let update_interval_cycles = 3;
|
||||
let mut throttle = LoopThrottle::new(target_cycle_time, update_interval_cycles, None);
|
||||
|
||||
let initial_delay = throttle.get_delay();
|
||||
|
||||
// Run enough cycles to trigger multiple updates
|
||||
for _ in 0..12 {
|
||||
// Add variable work to test adaptation
|
||||
Timer::after(Duration::from_millis(10)).await;
|
||||
throttle.sleep().await;
|
||||
}
|
||||
|
||||
println!("Initial delay: {:?}", initial_delay);
|
||||
println!("Final delay: {:?}", throttle.get_delay());
|
||||
println!("Last avg cycle time: {:?}", throttle.get_cycle_time());
|
||||
|
||||
assert!(throttle.get_cycle_time() > Duration::ZERO);
|
||||
});
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_throttle_initialization() {
|
||||
let update_interval_cycles = 10;
|
||||
let target_cycle_time = Duration::from_millis(75);
|
||||
let throttle = LoopThrottle::new(target_cycle_time, update_interval_cycles, None);
|
||||
|
||||
assert_eq!(throttle.get_delay(), target_cycle_time);
|
||||
assert_eq!(throttle.get_cycle_time(), target_cycle_time);
|
||||
assert_eq!(throttle.total_cycles, 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_overflow_protection() {
|
||||
block_on(async {
|
||||
let target_cycle_time = Duration::from_micros(1);
|
||||
let update_interval_cycles = 5;
|
||||
let mut throttle = LoopThrottle::new(target_cycle_time, update_interval_cycles, None);
|
||||
|
||||
// Run many cycles to ensure total_cycles resets properly
|
||||
for i in 0..50 {
|
||||
throttle.sleep().await;
|
||||
|
||||
// Verify that total_cycles never exceeds update_interval_cycles
|
||||
assert!(
|
||||
throttle.total_cycles <= update_interval_cycles,
|
||||
"Cycle {}: total_cycles ({}) exceeded update_interval_cycles ({})",
|
||||
i,
|
||||
throttle.total_cycles,
|
||||
update_interval_cycles
|
||||
);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,5 @@
|
||||
pub mod loop_trottle;
|
||||
pub mod compare_lists;
|
||||
pub mod hasher_serializer;
|
||||
pub mod hashing;
|
||||
pub mod interpolation;
|
||||
pub mod moving_time_window;
|
||||
pub mod retry;
|
||||
@@ -1,243 +0,0 @@
|
||||
use std::{
|
||||
collections::VecDeque,
|
||||
iter::Sum,
|
||||
time::{Duration, Instant},
|
||||
};
|
||||
|
||||
/// A helper trait to handle division for averaging
|
||||
pub trait DivideByCount {
|
||||
fn divide_by_count(self, count: usize) -> Self;
|
||||
}
|
||||
|
||||
impl DivideByCount for f64 {
|
||||
fn divide_by_count(self, count: usize) -> Self {
|
||||
self / (count as Self)
|
||||
}
|
||||
}
|
||||
|
||||
impl DivideByCount for f32 {
|
||||
fn divide_by_count(self, count: usize) -> Self {
|
||||
self / (count as Self)
|
||||
}
|
||||
}
|
||||
|
||||
impl DivideByCount for i32 {
|
||||
fn divide_by_count(self, count: usize) -> Self {
|
||||
self / (count as Self)
|
||||
}
|
||||
}
|
||||
|
||||
impl DivideByCount for i64 {
|
||||
fn divide_by_count(self, count: usize) -> Self {
|
||||
self / (count as Self)
|
||||
}
|
||||
}
|
||||
|
||||
impl DivideByCount for u32 {
|
||||
fn divide_by_count(self, count: usize) -> Self {
|
||||
self / (count as Self)
|
||||
}
|
||||
}
|
||||
|
||||
impl DivideByCount for u64 {
|
||||
fn divide_by_count(self, count: usize) -> Self {
|
||||
self / (count as Self)
|
||||
}
|
||||
}
|
||||
|
||||
// Type aliases for common use cases
|
||||
pub type F64Window = MovingTimeWindow<f64>;
|
||||
pub type F32Window = MovingTimeWindow<f32>;
|
||||
pub type I32Window = MovingTimeWindow<i32>;
|
||||
pub type I64Window = MovingTimeWindow<i64>;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct MovingTimeWindow<T>
|
||||
where
|
||||
T: Copy + PartialOrd + Sum + DivideByCount + std::fmt::Debug + Default,
|
||||
{
|
||||
max_samples: usize,
|
||||
samples: VecDeque<(T, Instant)>,
|
||||
last_sample: Option<(T, Instant)>,
|
||||
sample_time: Duration,
|
||||
cached_average: Option<T>,
|
||||
cached_max: Option<T>,
|
||||
cached_min: Option<T>,
|
||||
}
|
||||
|
||||
/// A time-based moving window that maintains samples within a specified duration and count limit.
|
||||
///
|
||||
/// This structure efficiently tracks values over time, automatically removing samples that fall
|
||||
/// outside the configured time window or exceed the maximum sample count. It provides cached
|
||||
/// average calculation to avoid redundant computations.
|
||||
impl<T> MovingTimeWindow<T>
|
||||
where
|
||||
T: Copy + PartialOrd + Sum + DivideByCount + std::fmt::Debug + Default,
|
||||
{
|
||||
/// Creates a new moving time window with the specified duration and maximum sample count.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `duration` - The time window duration; samples older than this will be automatically removed
|
||||
/// * `samples` - Maximum number of samples to retain, regardless of their age
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// A new `MovingTimeWindow` instance with empty sample collection and initialized cache
|
||||
pub fn new(duration: Duration, max_samples: usize) -> Self {
|
||||
Self {
|
||||
max_samples,
|
||||
samples: VecDeque::with_capacity(max_samples),
|
||||
last_sample: None,
|
||||
sample_time: duration / (max_samples as u32),
|
||||
cached_average: None,
|
||||
cached_max: None,
|
||||
cached_min: None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Updates the window with a new sample value at the given timestamp.
|
||||
///
|
||||
/// This method adds the new sample to the window and automatically removes samples that:
|
||||
/// - Are older than the configured duration from the current timestamp
|
||||
/// - Exceed the maximum sample count limit
|
||||
///
|
||||
/// The cached average is invalidated after this operation.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `value` - The numeric value to add to the window
|
||||
/// * `timestamp` - The time at which this sample was taken
|
||||
pub fn update(&mut self, value: T, timestamp: Instant) {
|
||||
// Chekc if we should add a new sample based on the sample time and the last sample
|
||||
if let Some((_, last_timestamp)) = self.last_sample {
|
||||
if timestamp.duration_since(last_timestamp) < self.sample_time {
|
||||
// If the new sample is too close to the last one, we skip adding it
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// If we have reached the maximum sample count, remove the oldest sample
|
||||
if self.samples.len() >= self.max_samples {
|
||||
self.samples.pop_back();
|
||||
}
|
||||
|
||||
// Add the new sample
|
||||
self.samples.push_front((value, timestamp));
|
||||
self.last_sample = Some((value, timestamp));
|
||||
|
||||
// Invalidate all cached values since we added a new sample
|
||||
self.cached_average = None;
|
||||
self.cached_max = None;
|
||||
self.cached_min = None;
|
||||
}
|
||||
|
||||
/// Calculates and returns the average of all samples currently in the window.
|
||||
///
|
||||
/// This method uses caching to avoid redundant calculations. The average is only
|
||||
/// recalculated when the window contents have changed since the last call.
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The arithmetic mean of all current samples, or T::default() if the window is empty
|
||||
pub fn average(&mut self) -> T {
|
||||
// Return cached value if available
|
||||
if let Some(cached) = self.cached_average {
|
||||
return cached;
|
||||
}
|
||||
|
||||
let sum: T = self.samples.iter().map(|(v, _)| *v).sum();
|
||||
let count = self.samples.len();
|
||||
|
||||
let avg = if count > 0 {
|
||||
sum.divide_by_count(count)
|
||||
} else {
|
||||
T::default()
|
||||
};
|
||||
|
||||
// Cache the calculated value
|
||||
self.cached_average = Some(avg);
|
||||
avg
|
||||
}
|
||||
|
||||
/// Calculates and returns the maximum value of all samples currently in the window.
|
||||
///
|
||||
/// This method uses caching to avoid redundant calculations. The maximum is only
|
||||
/// recalculated when the window contents have changed since the last call.
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The maximum value of all current samples, or T::default() if the window is empty
|
||||
pub fn max(&mut self) -> T {
|
||||
// Return cached value if available
|
||||
if let Some(cached) = self.cached_max {
|
||||
return cached;
|
||||
}
|
||||
|
||||
let result = self
|
||||
.samples
|
||||
.iter()
|
||||
.map(|(v, _)| *v)
|
||||
.fold(None, |acc, val| {
|
||||
acc.map_or(Some(val), |current_max| {
|
||||
if val > current_max {
|
||||
Some(val)
|
||||
} else {
|
||||
Some(current_max)
|
||||
}
|
||||
})
|
||||
})
|
||||
.unwrap_or_default();
|
||||
|
||||
// Cache the calculated value
|
||||
self.cached_max = Some(result);
|
||||
result
|
||||
}
|
||||
|
||||
/// Calculates and returns the minimum value of all samples currently in the window.
|
||||
///
|
||||
/// This method uses caching to avoid redundant calculations. The minimum is only
|
||||
/// recalculated when the window contents have changed since the last call.
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The minimum value of all current samples, or T::default() if the window is empty
|
||||
pub fn min(&mut self) -> T {
|
||||
// Return cached value if available
|
||||
if let Some(cached) = self.cached_min {
|
||||
return cached;
|
||||
}
|
||||
|
||||
let result = self
|
||||
.samples
|
||||
.iter()
|
||||
.map(|(v, _)| *v)
|
||||
.fold(None, |acc, val| {
|
||||
acc.map_or(Some(val), |current_min| {
|
||||
if val < current_min {
|
||||
Some(val)
|
||||
} else {
|
||||
Some(current_min)
|
||||
}
|
||||
})
|
||||
})
|
||||
.unwrap_or_default();
|
||||
|
||||
// Cache the calculated value
|
||||
self.cached_min = Some(result);
|
||||
result
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn test_generic() {
|
||||
// Test that the struct compiles with basic numeric types
|
||||
let _: MovingTimeWindow<f64> = MovingTimeWindow::new(Duration::from_secs(1), 1);
|
||||
let _: MovingTimeWindow<f32> = MovingTimeWindow::new(Duration::from_secs(1), 1);
|
||||
let _: MovingTimeWindow<i32> = MovingTimeWindow::new(Duration::from_secs(1), 1);
|
||||
}
|
||||
}
|
||||
@@ -1,38 +1,4 @@
|
||||
use std::future::Future;
|
||||
|
||||
/// Retries a fallible operation up to `n` times synchronously.
|
||||
///
|
||||
/// This function executes the provided closure `f` repeatedly until either:
|
||||
/// - The operation succeeds (returns `Ok(T)`)
|
||||
/// - All retry attempts are exhausted
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `n` - The maximum number of retry attempts (0 means only one attempt)
|
||||
/// * `f` - A mutable closure that returns a `Result<T, E>`
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Returns `Ok(T)` if any attempt succeeds, or `Err(E)` containing the error
|
||||
/// from the last failed attempt if all retries are exhausted.
|
||||
///
|
||||
/// # Examples
|
||||
///
|
||||
/// ```rust
|
||||
/// use control_core::helpers::retry::retry_n_times;
|
||||
///
|
||||
/// let mut attempt = 0;
|
||||
/// let result = retry_n_times(3, || {
|
||||
/// attempt += 1;
|
||||
/// if attempt < 3 {
|
||||
/// Err("failed")
|
||||
/// } else {
|
||||
/// Ok("success")
|
||||
/// }
|
||||
/// });
|
||||
/// assert_eq!(result, Ok("success"));
|
||||
/// ```
|
||||
pub fn retry_n_times<T, E, F>(n: usize, mut f: F) -> Result<T, E>
|
||||
pub fn retry<T, E, F>(n: usize, mut f: F) -> Result<T, E>
|
||||
where
|
||||
F: FnMut() -> Result<T, E>,
|
||||
{
|
||||
@@ -45,383 +11,3 @@ where
|
||||
}
|
||||
Err(last_err.unwrap())
|
||||
}
|
||||
|
||||
/// Retries a fallible async operation up to `n` times.
|
||||
///
|
||||
/// This function executes the provided async closure `f` repeatedly until either:
|
||||
/// - The operation succeeds (returns `Ok(T)`)
|
||||
/// - All retry attempts are exhausted
|
||||
///
|
||||
/// Each attempt is awaited before proceeding to the next retry or returning.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `n` - The maximum number of retry attempts (0 means only one attempt)
|
||||
/// * `f` - A mutable closure that returns a `Future<Output = Result<T, E>>`
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Returns `Ok(T)` if any attempt succeeds, or `Err(E)` containing the error
|
||||
/// from the last failed attempt if all retries are exhausted.
|
||||
///
|
||||
/// # Examples
|
||||
///
|
||||
/// ```rust
|
||||
/// use control_core::helpers::retry::retry_n_times_async;
|
||||
/// use std::sync::Arc;
|
||||
/// use std::sync::atomic::{AtomicUsize, Ordering};
|
||||
///
|
||||
/// async fn example() {
|
||||
/// let attempt = Arc::new(AtomicUsize::new(0));
|
||||
/// let attempt_clone = attempt.clone();
|
||||
/// let result = retry_n_times_async(3, move || {
|
||||
/// let attempt = attempt_clone.clone();
|
||||
/// async move {
|
||||
/// let current = attempt.fetch_add(1, Ordering::SeqCst) + 1;
|
||||
/// if current < 3 {
|
||||
/// Err("failed")
|
||||
/// } else {
|
||||
/// Ok("success")
|
||||
/// }
|
||||
/// }
|
||||
/// }).await;
|
||||
/// assert_eq!(result, Ok("success"));
|
||||
/// }
|
||||
/// ```
|
||||
pub async fn retry_n_times_async<T, E, F, Fut>(n: usize, mut f: F) -> Result<T, E>
|
||||
where
|
||||
F: FnMut() -> Fut,
|
||||
Fut: Future<Output = Result<T, E>>,
|
||||
{
|
||||
let mut last_err: Option<E> = None;
|
||||
for _ in 0..=n {
|
||||
match f().await {
|
||||
Ok(value) => return Ok(value),
|
||||
Err(err) => last_err = Some(err),
|
||||
}
|
||||
}
|
||||
Err(last_err.unwrap())
|
||||
}
|
||||
|
||||
/// Retries a fallible operation until a closure decides to stop retrying.
|
||||
///
|
||||
/// This function executes the provided closure `f` repeatedly until either:
|
||||
/// - The operation succeeds (returns `Ok(T)`)
|
||||
/// - The retry condition returns false
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `f` - A mutable closure that returns a `Result<T, E>`
|
||||
/// * `should_retry` - A closure that takes `&E` and returns `bool` to decide if retry should continue
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Returns `Ok(T)` if any attempt succeeds, or `Err(E)` containing the error
|
||||
/// from the last failed attempt when retries stop.
|
||||
///
|
||||
/// # Examples
|
||||
///
|
||||
/// ```rust
|
||||
/// use control_core::helpers::retry::retry_conditionally;
|
||||
///
|
||||
/// let mut attempt = 0;
|
||||
/// let result = retry_conditionally(
|
||||
/// || {
|
||||
/// attempt += 1;
|
||||
/// if attempt < 3 {
|
||||
/// Err(format!("failed attempt {}", attempt))
|
||||
/// } else {
|
||||
/// Ok("success")
|
||||
/// }
|
||||
/// },
|
||||
/// |err| {
|
||||
/// // Retry up to 3 times
|
||||
/// !err.contains("attempt 3")
|
||||
/// }
|
||||
/// );
|
||||
/// assert_eq!(result, Ok("success"));
|
||||
/// ```
|
||||
pub fn retry_conditionally<T, E, F, R>(mut f: F, mut should_retry: R) -> Result<T, E>
|
||||
where
|
||||
F: FnMut() -> Result<T, E>,
|
||||
R: FnMut(&E) -> bool,
|
||||
{
|
||||
loop {
|
||||
match f() {
|
||||
Ok(value) => return Ok(value),
|
||||
Err(err) => {
|
||||
if !should_retry(&err) {
|
||||
return Err(err);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Retries a fallible async operation until a closure decides to stop retrying.
|
||||
///
|
||||
/// This function executes the provided async closure `f` repeatedly until either:
|
||||
/// - The operation succeeds (returns `Ok(T)`)
|
||||
/// - The retry condition returns false
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `f` - A mutable closure that returns a `Future<Output = Result<T, E>>`
|
||||
/// * `should_retry` - A closure that takes `&E` and returns `bool` to decide if retry should continue
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Returns `Ok(T)` if any attempt succeeds, or `Err(E)` containing the error
|
||||
/// from the last failed attempt when retries stop.
|
||||
///
|
||||
/// # Examples
|
||||
///
|
||||
/// ```rust
|
||||
/// use control_core::helpers::retry::retry_conditionally_async;
|
||||
/// use std::sync::Arc;
|
||||
/// use std::sync::atomic::{AtomicUsize, Ordering};
|
||||
///
|
||||
/// async fn example() {
|
||||
/// let attempt = Arc::new(AtomicUsize::new(0));
|
||||
/// let attempt_clone = attempt.clone();
|
||||
/// let result = retry_conditionally_async(
|
||||
/// move || {
|
||||
/// let attempt = attempt_clone.clone();
|
||||
/// async move {
|
||||
/// let current = attempt.fetch_add(1, Ordering::SeqCst) + 1;
|
||||
/// if current < 3 {
|
||||
/// Err(format!("failed attempt {}", current))
|
||||
/// } else {
|
||||
/// Ok("success")
|
||||
/// }
|
||||
/// }
|
||||
/// },
|
||||
/// |err| {
|
||||
/// // Retry up to 3 times
|
||||
/// !err.contains("attempt 3")
|
||||
/// }
|
||||
/// ).await;
|
||||
/// assert_eq!(result, Ok("success"));
|
||||
/// }
|
||||
/// ```
|
||||
pub async fn retry_conditionally_async<T, E, F, Fut, R>(
|
||||
mut f: F,
|
||||
mut should_retry: R,
|
||||
) -> Result<T, E>
|
||||
where
|
||||
F: FnMut() -> Fut,
|
||||
Fut: Future<Output = Result<T, E>>,
|
||||
R: FnMut(&E) -> bool,
|
||||
{
|
||||
loop {
|
||||
match f().await {
|
||||
Ok(value) => return Ok(value),
|
||||
Err(err) => {
|
||||
if !should_retry(&err) {
|
||||
return Err(err);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use std::cell::RefCell;
|
||||
use std::rc::Rc;
|
||||
|
||||
// Simple blocking executor for async functions in tests
|
||||
fn block_on<F: std::future::Future>(future: F) -> F::Output {
|
||||
use std::sync::Arc;
|
||||
use std::task::{Context, Poll, Waker};
|
||||
use std::thread;
|
||||
|
||||
struct DummyWaker;
|
||||
impl std::task::Wake for DummyWaker {
|
||||
fn wake(self: Arc<Self>) {}
|
||||
}
|
||||
|
||||
let waker = Waker::from(Arc::new(DummyWaker));
|
||||
let mut context = Context::from_waker(&waker);
|
||||
let mut future = Box::pin(future);
|
||||
|
||||
loop {
|
||||
match future.as_mut().poll(&mut context) {
|
||||
Poll::Ready(result) => return result,
|
||||
Poll::Pending => {
|
||||
thread::yield_now();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_success_first_attempt() {
|
||||
let result = retry_n_times(3, || Ok::<i32, &str>(42));
|
||||
assert_eq!(result, Ok(42));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_success_after_failures() {
|
||||
let mut attempt = 0;
|
||||
let result = retry_n_times(3, || {
|
||||
attempt += 1;
|
||||
if attempt < 3 { Err("failed") } else { Ok(42) }
|
||||
});
|
||||
assert_eq!(result, Ok(42));
|
||||
assert_eq!(attempt, 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_all_attempts_fail() {
|
||||
let mut attempt = 0;
|
||||
let result: Result<i32, &str> = retry_n_times(2, || {
|
||||
attempt += 1;
|
||||
Err("always fails")
|
||||
});
|
||||
assert_eq!(result, Err("always fails"));
|
||||
assert_eq!(attempt, 3); // 0..=2 means 3 attempts
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_zero_retries() {
|
||||
let mut attempt = 0;
|
||||
let result: Result<i32, &str> = retry_n_times(0, || {
|
||||
attempt += 1;
|
||||
Err("failed")
|
||||
});
|
||||
assert_eq!(result, Err("failed"));
|
||||
assert_eq!(attempt, 1); // Only one attempt
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_async_success_first_attempt() {
|
||||
let result = block_on(retry_n_times_async(3, || async { Ok::<i32, &str>(42) }));
|
||||
assert_eq!(result, Ok(42));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_async_success_after_failures() {
|
||||
let attempt = Rc::new(RefCell::new(0));
|
||||
let attempt_clone = attempt.clone();
|
||||
let result = block_on(retry_n_times_async(3, move || {
|
||||
let attempt = attempt_clone.clone();
|
||||
async move {
|
||||
let mut a = attempt.borrow_mut();
|
||||
*a += 1;
|
||||
if *a < 3 { Err("failed") } else { Ok(42) }
|
||||
}
|
||||
}));
|
||||
assert_eq!(result, Ok(42));
|
||||
assert_eq!(*attempt.borrow(), 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_async_all_attempts_fail() {
|
||||
let attempt = Rc::new(RefCell::new(0));
|
||||
let attempt_clone = attempt.clone();
|
||||
let result: Result<i32, &str> = block_on(retry_n_times_async(2, move || {
|
||||
let attempt = attempt_clone.clone();
|
||||
async move {
|
||||
let mut a = attempt.borrow_mut();
|
||||
*a += 1;
|
||||
Err("always fails")
|
||||
}
|
||||
}));
|
||||
assert_eq!(result, Err("always fails"));
|
||||
assert_eq!(*attempt.borrow(), 3); // 0..=2 means 3 attempts
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_and_success_with_condition() {
|
||||
let mut attempt = 0;
|
||||
let result: Result<i32, String> = retry_conditionally(
|
||||
|| {
|
||||
attempt += 1;
|
||||
if attempt < 3 {
|
||||
Err(format!("failed attempt {}", attempt))
|
||||
} else {
|
||||
Ok(42)
|
||||
}
|
||||
},
|
||||
|err| {
|
||||
// Retry as long as it's not the 3rd attempt
|
||||
!err.contains("attempt 3")
|
||||
},
|
||||
);
|
||||
|
||||
assert_eq!(result, Ok(42));
|
||||
assert_eq!(attempt, 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_and_stop_early() {
|
||||
let mut attempt = 0;
|
||||
let result: Result<i32, String> = retry_conditionally(
|
||||
|| {
|
||||
attempt += 1;
|
||||
Err(format!("failed attempt {}", attempt))
|
||||
},
|
||||
|_err| {
|
||||
// Stop after first failure
|
||||
false
|
||||
},
|
||||
);
|
||||
|
||||
assert!(result.is_err());
|
||||
assert_eq!(attempt, 1);
|
||||
assert_eq!(result.unwrap_err(), "failed attempt 1");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_async_success_with_condition() {
|
||||
let attempt = Rc::new(RefCell::new(0));
|
||||
let attempt_clone = attempt.clone();
|
||||
let result: Result<i32, String> = block_on(retry_conditionally_async(
|
||||
move || {
|
||||
let attempt = attempt_clone.clone();
|
||||
async move {
|
||||
let mut a = attempt.borrow_mut();
|
||||
*a += 1;
|
||||
if *a < 3 {
|
||||
Err(format!("failed attempt {}", *a))
|
||||
} else {
|
||||
Ok(42)
|
||||
}
|
||||
}
|
||||
},
|
||||
|err| {
|
||||
// Retry as long as it's not the 3rd attempt
|
||||
!err.contains("attempt 3")
|
||||
},
|
||||
));
|
||||
|
||||
assert_eq!(result, Ok(42));
|
||||
assert_eq!(*attempt.borrow(), 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_retry_async_stop_early() {
|
||||
let attempt = Rc::new(RefCell::new(0));
|
||||
let attempt_clone = attempt.clone();
|
||||
let result: Result<i32, String> = block_on(retry_conditionally_async(
|
||||
move || {
|
||||
let attempt = attempt_clone.clone();
|
||||
async move {
|
||||
let mut a = attempt.borrow_mut();
|
||||
*a += 1;
|
||||
Err(format!("failed attempt {}", *a))
|
||||
}
|
||||
},
|
||||
|_err| {
|
||||
// Stop after first failure
|
||||
false
|
||||
},
|
||||
));
|
||||
|
||||
assert!(result.is_err());
|
||||
assert_eq!(*attempt.borrow(), 1);
|
||||
assert_eq!(result.unwrap_err(), "failed attempt 1");
|
||||
}
|
||||
}
|
||||
@@ -1,113 +0,0 @@
|
||||
#[cfg(target_os = "linux")]
|
||||
use libc;
|
||||
use std::ffi::CString;
|
||||
use std::io;
|
||||
use std::str;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
fn read_proc_interrupts() -> Result<String, io::Error> {
|
||||
let path = CString::new("/proc/interrupts").unwrap();
|
||||
// O_RDONLY
|
||||
let fd = unsafe { libc::open(path.as_ptr(), libc::O_RDONLY) };
|
||||
if fd < 0 {
|
||||
return Err(io::Error::from_raw_os_error(-fd));
|
||||
}
|
||||
|
||||
let mut buf: Vec<u8> = Vec::new();
|
||||
let mut chunk = [0u8; 8192];
|
||||
|
||||
loop {
|
||||
let n = unsafe { libc::read(fd, chunk.as_mut_ptr() as *mut _, chunk.len()) };
|
||||
if n < 0 {
|
||||
let e = io::Error::from_raw_os_error(-n as i32);
|
||||
unsafe { libc::close(fd) };
|
||||
return Err(e);
|
||||
} else if n == 0 {
|
||||
break;
|
||||
} else {
|
||||
buf.extend_from_slice(&chunk[..n as usize]);
|
||||
}
|
||||
}
|
||||
unsafe { libc::close(fd) };
|
||||
Ok(String::from_utf8_lossy(&buf).into_owned())
|
||||
}
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
fn get_interface_irq(proc_content: &str, interface_name: &str) -> Option<u32> {
|
||||
for line in proc_content.lines() {
|
||||
if line.contains(interface_name) {
|
||||
if let Some(colon_pos) = line.find(':') {
|
||||
let head = &line[..colon_pos].trim();
|
||||
// try parse head as integer IRQ number
|
||||
if let Ok(irq) = head.parse::<u32>() {
|
||||
return Some(irq);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
None
|
||||
}
|
||||
|
||||
/// Since kernel 3.0 it’s possible to use the /proc/irq/<IRQ-NUMBER>/smp_affinity_list
|
||||
/// With comma seperated values
|
||||
/// This function takes the irq identifier and writes the cpu string
|
||||
/// into /proc/irq/irq_number/smp_affinity_list
|
||||
#[cfg(target_os = "linux")]
|
||||
fn set_irq_affinity_raw(irq: u32, cpu: &str) -> Result<(), io::Error> {
|
||||
let path: String = format!("/proc/irq/{}/smp_affinity_list", irq);
|
||||
let cpath = CString::new(path.clone()).unwrap();
|
||||
// We want to completely overwrite the smp affinities, so that we guarentee execution on the specified cores
|
||||
let file_descriptor = unsafe { libc::open(cpath.as_ptr(), libc::O_WRONLY) };
|
||||
|
||||
if file_descriptor < 0 {
|
||||
tracing::info!("Permission denied to write: {}", path);
|
||||
return Err(io::Error::new(
|
||||
io::ErrorKind::PermissionDenied,
|
||||
format!("failed to open {}: errno {}", path, -file_descriptor),
|
||||
));
|
||||
}
|
||||
// convert to "raw bytes"
|
||||
// cpu needs to be a string because we need the raw ascii code 48 = '0' and so on as bytes
|
||||
let bytes = cpu.as_bytes();
|
||||
let mut bytes_written_total = 0usize;
|
||||
|
||||
while bytes_written_total < bytes.len() {
|
||||
let to_write = &bytes[bytes_written_total..];
|
||||
let bytes_written = unsafe {
|
||||
libc::write(
|
||||
file_descriptor,
|
||||
to_write.as_ptr() as *const _,
|
||||
to_write.len(),
|
||||
)
|
||||
};
|
||||
if bytes_written < 0 {
|
||||
let e = io::Error::from_raw_os_error(-bytes_written as i32);
|
||||
unsafe { libc::close(file_descriptor) };
|
||||
return Err(e);
|
||||
}
|
||||
bytes_written_total += bytes_written as usize;
|
||||
}
|
||||
unsafe { libc::close(file_descriptor) };
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// This is used to make sure that a given irq handler runs on the expected cpu
|
||||
/// On Realtime systems this is the make or break fix for certain usecases like ethercrab
|
||||
/// On our machines the 99.99th percentile of cycle times was very bad ~5-11 ms while ethercat is supposed to be in the microseconds
|
||||
/// After pinning the ethernet irq to the core that the ethercat code ran on 99.99th percentile went down to ~200us (microseconds)
|
||||
/// Example input: irq_name: "eno1" , cpu: 2
|
||||
/// Remember that cpu cores are counted from 0
|
||||
#[cfg(target_os = "linux")]
|
||||
pub fn set_irq_affinity(irq_name: &str, cpu: u32) -> Result<(), anyhow::Error> {
|
||||
let proc_contents = read_proc_interrupts()?;
|
||||
let irq = get_interface_irq(&proc_contents, irq_name);
|
||||
if irq.is_none() {
|
||||
return Err(anyhow::anyhow!("Couldnt find irq number!"));
|
||||
}
|
||||
let res = set_irq_affinity_raw(irq.unwrap(), &cpu.to_string());
|
||||
if res.is_err() {
|
||||
return Err(anyhow::anyhow!("Couldnt write affinity list!"));
|
||||
} else {
|
||||
return Ok(());
|
||||
}
|
||||
}
|
||||
@@ -1,20 +1,14 @@
|
||||
pub mod actors;
|
||||
pub mod controllers;
|
||||
pub mod converters;
|
||||
pub mod downcast;
|
||||
pub mod ethercat;
|
||||
pub mod helpers;
|
||||
pub mod irq_handling;
|
||||
pub mod machines;
|
||||
pub mod modbus;
|
||||
pub mod realtime;
|
||||
pub mod rest;
|
||||
pub mod serial;
|
||||
pub mod socketio;
|
||||
pub mod transmission;
|
||||
pub mod uom_extensions;
|
||||
|
||||
#[cfg(feature = "video-streaming")]
|
||||
pub mod video_streaming;
|
||||
|
||||
#[macro_use]
|
||||
extern crate uom;
|
||||
@@ -1,27 +1,7 @@
|
||||
use crate::socketio::namespace::NamespaceInterface;
|
||||
use serde_json::Value;
|
||||
use smol::lock::Mutex;
|
||||
use std::sync::Arc;
|
||||
|
||||
use crate::socketio::namespace::Namespace;
|
||||
|
||||
pub trait MachineApi {
|
||||
fn api_mutate(&mut self, value: Value) -> Result<(), anyhow::Error>;
|
||||
fn api_event_namespace(&mut self) -> Arc<Mutex<Namespace>>;
|
||||
|
||||
/// Returns a list of available video stream identifiers for this machine
|
||||
#[cfg(feature = "video-streaming")]
|
||||
fn api_video_streams(&self) -> Vec<String> {
|
||||
Vec::new()
|
||||
}
|
||||
|
||||
/// Get a video stream receiver for the specified stream ID
|
||||
/// Returns None if the stream doesn't exist
|
||||
#[cfg(feature = "video-streaming")]
|
||||
fn api_get_video_stream(
|
||||
&mut self,
|
||||
stream_id: &str,
|
||||
) -> Option<tokio::sync::broadcast::Receiver<Vec<u8>>> {
|
||||
let _ = stream_id;
|
||||
None
|
||||
}
|
||||
fn api_event_namespace(&mut self) -> &mut dyn NamespaceInterface;
|
||||
}
|
||||
@@ -1,208 +0,0 @@
|
||||
use crate::downcast::Downcast;
|
||||
use crate::machines::identification::MachineIdentificationUnique;
|
||||
use crate::machines::manager::MachineManager;
|
||||
use crate::socketio::{event::GenericEvent, namespace::Namespace};
|
||||
use serde::Serialize;
|
||||
use smol::block_on;
|
||||
use smol::lock::RwLock;
|
||||
use std::sync::Arc;
|
||||
use std::sync::Weak;
|
||||
|
||||
use super::Machine;
|
||||
use anyhow::anyhow;
|
||||
use smol::{channel::Sender, lock::Mutex};
|
||||
use socketioxide::extract::SocketRef;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum MachineConnection<M: Machine + ?Sized>
|
||||
where
|
||||
Self: Sized,
|
||||
{
|
||||
Error(anyhow::Error),
|
||||
Disconnected,
|
||||
Connected(Arc<Mutex<M>>),
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct MachineSlot<M: Machine + ?Sized>
|
||||
where
|
||||
Self: Sized,
|
||||
{
|
||||
pub machine_connection: MachineConnection<M>,
|
||||
pub namespace: Arc<Mutex<Namespace>>,
|
||||
}
|
||||
|
||||
pub type MachineConnectionGeneric = MachineConnection<dyn Machine>;
|
||||
|
||||
pub type MachineSlotGeneric = MachineSlot<dyn Machine>;
|
||||
|
||||
impl<M: Machine> Clone for MachineConnection<M> {
|
||||
fn clone(&self) -> Self {
|
||||
match self {
|
||||
Self::Connected(m) => Self::Connected(m.clone()),
|
||||
Self::Error(e) => Self::Error(anyhow!(e.to_string())),
|
||||
Self::Disconnected => Self::Disconnected,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<M: Machine + ?Sized> MachineConnection<M> {
|
||||
pub fn to_error(&self) -> Option<anyhow::Error> {
|
||||
match self {
|
||||
MachineConnection::Error(err) => Some(anyhow!(err.to_string())),
|
||||
Self::Disconnected => Some(anyhow!("Machine is disconnected")),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn to_machine(&self) -> Option<Arc<Mutex<M>>> {
|
||||
match self {
|
||||
Self::Connected(machine) => Some(machine.clone()),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<M: Machine + ?Sized> MachineSlot<M> {
|
||||
pub fn new(socket_queue_tx: Sender<(SocketRef, Arc<GenericEvent>)>) -> Self {
|
||||
let namespace = Namespace::new(socket_queue_tx);
|
||||
Self {
|
||||
machine_connection: MachineConnection::Disconnected,
|
||||
namespace: Arc::new(Mutex::new(namespace)),
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn is_connected(&self) -> bool {
|
||||
matches!(self.machine_connection, MachineConnection::Connected(_))
|
||||
}
|
||||
}
|
||||
|
||||
pub trait CrossConnectableMachine<
|
||||
F: CrossConnectableMachine<F, T>,
|
||||
T: CrossConnectableMachine<T, F>,
|
||||
>: Machine
|
||||
{
|
||||
fn get_cross_connection(&mut self) -> &mut MachineCrossConnection<F, T>;
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct MachineCrossConnection<
|
||||
F: CrossConnectableMachine<F, T>,
|
||||
T: CrossConnectableMachine<T, F>,
|
||||
> {
|
||||
machine_manager: Weak<RwLock<MachineManager>>,
|
||||
machine_identification_unique: MachineIdentificationUnique,
|
||||
pub connected_machine: Weak<Mutex<MachineSlot<T>>>,
|
||||
_make_compiler_happy: std::marker::PhantomData<F>, // This marks tells the typechecker, that we
|
||||
// need F in our type arguemnts. We are ok with
|
||||
// it being used only in a fake field. Without
|
||||
// F being fixed, we would not be anble to implement
|
||||
// reverse connecting and disconnecting properly,
|
||||
// as the type on the other side would decay
|
||||
// to an unknown type. See also https://github.com/qitechgmbh/control/pull/625#discussion_r2379566315
|
||||
}
|
||||
|
||||
#[derive(Serialize, Debug, Clone)]
|
||||
pub struct MachineCrossConnectionState {
|
||||
/// Connected Machine
|
||||
pub machine_identification_unique: Option<MachineIdentificationUnique>,
|
||||
pub is_available: bool,
|
||||
}
|
||||
|
||||
impl<F: CrossConnectableMachine<F, T>, T: CrossConnectableMachine<T, F>>
|
||||
MachineCrossConnection<F, T>
|
||||
{
|
||||
pub fn new(
|
||||
manager: Weak<RwLock<MachineManager>>,
|
||||
machine_identification_unique: &MachineIdentificationUnique,
|
||||
) -> Self {
|
||||
Self {
|
||||
machine_manager: manager,
|
||||
connected_machine: Weak::new(),
|
||||
machine_identification_unique: machine_identification_unique.clone(),
|
||||
_make_compiler_happy: std::marker::PhantomData,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn set_connected_machine(
|
||||
&mut self,
|
||||
machine_identification_unique: &MachineIdentificationUnique,
|
||||
) {
|
||||
if let Some(machine_manager) = self.machine_manager.upgrade() {
|
||||
let machine_manager = block_on(machine_manager.read());
|
||||
|
||||
let slot: Arc<Mutex<MachineSlotGeneric>> =
|
||||
match machine_manager.get(&machine_identification_unique) {
|
||||
Some(c) => c,
|
||||
None => return,
|
||||
};
|
||||
|
||||
let slot: Arc<Mutex<MachineSlot<T>>> = match Downcast::downcast(&slot) {
|
||||
Ok(s) => s,
|
||||
Err(_) => return,
|
||||
};
|
||||
|
||||
self.connected_machine = Arc::downgrade(&slot);
|
||||
} else {
|
||||
panic!("The machine manager has died, cannot do anything at this point");
|
||||
}
|
||||
}
|
||||
|
||||
pub fn reverse_connect(&self) {
|
||||
if let Some(slot) = self.connected_machine.upgrade() {
|
||||
let slot = &mut slot.lock_blocking();
|
||||
|
||||
if let MachineConnection::Connected(machine) = &slot.machine_connection {
|
||||
let mut machine = machine.lock_blocking();
|
||||
let cross_connection = machine.get_cross_connection();
|
||||
|
||||
if !cross_connection.is_connected() {
|
||||
cross_connection.set_connected_machine(&self.machine_identification_unique);
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
pub fn disconnect(&mut self) {
|
||||
self.connected_machine = Weak::new();
|
||||
}
|
||||
|
||||
pub fn reverse_disconnect(&mut self) {
|
||||
if let Some(slot) = self.connected_machine.upgrade() {
|
||||
let slot = slot.lock_blocking();
|
||||
|
||||
if let MachineConnection::Connected(machine) = &slot.machine_connection {
|
||||
let mut machine = machine.lock_blocking();
|
||||
let cross_connection = machine.get_cross_connection();
|
||||
cross_connection.disconnect();
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
pub fn is_connected(&self) -> bool {
|
||||
let arc = self.connected_machine.upgrade();
|
||||
arc.is_some()
|
||||
}
|
||||
|
||||
pub fn to_state(&self) -> MachineCrossConnectionState {
|
||||
if let Some(slot) = self.connected_machine.upgrade() {
|
||||
let connection = &slot.lock_blocking().machine_connection;
|
||||
|
||||
if let MachineConnection::Connected(machine) = connection {
|
||||
let machine = machine.lock_blocking();
|
||||
|
||||
return MachineCrossConnectionState {
|
||||
machine_identification_unique: Some(
|
||||
machine.get_machine_identification_unique(),
|
||||
),
|
||||
is_available: true,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
MachineCrossConnectionState {
|
||||
machine_identification_unique: None,
|
||||
is_available: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -12,7 +12,7 @@ pub struct MachineIdentificationUnique {
|
||||
|
||||
impl MachineIdentificationUnique {
|
||||
/// Check if values are non-zero
|
||||
pub const fn is_valid(&self) -> bool {
|
||||
pub fn is_valid(&self) -> bool {
|
||||
self.machine_identification.is_valid() && self.serial != 0
|
||||
}
|
||||
}
|
||||
@@ -36,12 +36,12 @@ pub struct MachineIdentification {
|
||||
|
||||
impl MachineIdentification {
|
||||
/// Check if values are non-zero
|
||||
pub const fn is_valid(&self) -> bool {
|
||||
pub fn is_valid(&self) -> bool {
|
||||
self.vendor != 0 && self.machine != 0
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
|
||||
pub struct DeviceMachineIdentification {
|
||||
pub machine_identification_unique: MachineIdentificationUnique,
|
||||
pub role: u16,
|
||||
@@ -49,7 +49,7 @@ pub struct DeviceMachineIdentification {
|
||||
|
||||
impl DeviceMachineIdentification {
|
||||
/// Check if values are non-zero
|
||||
pub const fn is_valid(&self) -> bool {
|
||||
pub fn is_valid(&self) -> bool {
|
||||
self.machine_identification_unique.is_valid()
|
||||
&& self
|
||||
.machine_identification_unique
|
||||
@@ -59,7 +59,7 @@ impl DeviceMachineIdentification {
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
|
||||
pub struct DeviceIdentification {
|
||||
pub device_machine_identification: Option<DeviceMachineIdentification>,
|
||||
pub device_hardware_identification: DeviceHardwareIdentification,
|
||||
@@ -67,14 +67,14 @@ pub struct DeviceIdentification {
|
||||
|
||||
impl From<DeviceIdentificationIdentified> for DeviceIdentification {
|
||||
fn from(value: DeviceIdentificationIdentified) -> Self {
|
||||
Self {
|
||||
DeviceIdentification {
|
||||
device_machine_identification: Some(value.device_machine_identification),
|
||||
device_hardware_identification: value.device_hardware_identification,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
|
||||
pub struct DeviceIdentificationIdentified {
|
||||
pub device_machine_identification: DeviceMachineIdentification,
|
||||
pub device_hardware_identification: DeviceHardwareIdentification,
|
||||
@@ -90,24 +90,24 @@ impl TryFrom<DeviceIdentification> for DeviceIdentificationIdentified {
|
||||
module_path!()
|
||||
))?;
|
||||
|
||||
Ok(Self {
|
||||
Ok(DeviceIdentificationIdentified {
|
||||
device_machine_identification,
|
||||
device_hardware_identification: value.device_hardware_identification,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
|
||||
pub enum DeviceHardwareIdentification {
|
||||
Ethercat(DeviceHardwareIdentificationEthercat),
|
||||
Serial(DeviceHardwareIdentificationSerial),
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
|
||||
pub struct DeviceHardwareIdentificationEthercat {
|
||||
pub subdevice_index: usize,
|
||||
}
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq, Eq)]
|
||||
#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)]
|
||||
pub struct DeviceHardwareIdentificationSerial {
|
||||
pub path: String,
|
||||
}
|
||||
@@ -1,41 +1,26 @@
|
||||
use smol::lock::{Mutex, RwLock};
|
||||
|
||||
use crate::{
|
||||
machines::{
|
||||
identification::MachineIdentificationUnique,
|
||||
new::{MachineNewHardware, MachineNewHardwareEthercat, MachineNewParams},
|
||||
},
|
||||
serial::SerialDevice,
|
||||
};
|
||||
use std::{collections::HashMap, sync::Arc};
|
||||
|
||||
use super::{
|
||||
Machine,
|
||||
identification::{DeviceIdentification, DeviceIdentificationIdentified},
|
||||
new::MachineNewHardwareSerial,
|
||||
registry::MachineRegistry,
|
||||
};
|
||||
use smol::{
|
||||
channel::Sender,
|
||||
lock::{Mutex, RwLock},
|
||||
};
|
||||
use socketioxide::extract::SocketRef;
|
||||
|
||||
use crate::{
|
||||
machines::{
|
||||
connection::{
|
||||
MachineConnection, MachineConnectionGeneric, MachineSlot, MachineSlotGeneric,
|
||||
},
|
||||
identification::MachineIdentificationUnique,
|
||||
new::{MachineNewHardware, MachineNewHardwareEthercat, MachineNewParams},
|
||||
},
|
||||
serial::SerialDevice,
|
||||
socketio::event::GenericEvent,
|
||||
};
|
||||
use std::{
|
||||
collections::HashMap,
|
||||
sync::{Arc, Weak},
|
||||
};
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct MachineManager {
|
||||
pub ethercat_machines: HashMap<MachineIdentificationUnique, Arc<Mutex<MachineSlotGeneric>>>,
|
||||
pub serial_machines: HashMap<MachineIdentificationUnique, Arc<Mutex<MachineSlotGeneric>>>,
|
||||
}
|
||||
|
||||
impl Default for MachineManager {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
pub ethercat_machines:
|
||||
HashMap<MachineIdentificationUnique, Result<Box<Mutex<dyn Machine>>, anyhow::Error>>,
|
||||
pub serial_machines:
|
||||
HashMap<MachineIdentificationUnique, Result<Box<Mutex<dyn Machine>>, anyhow::Error>>,
|
||||
}
|
||||
|
||||
impl MachineManager {
|
||||
@@ -51,14 +36,15 @@ impl MachineManager {
|
||||
device_identifications: &Vec<DeviceIdentification>,
|
||||
machine_registry: &MachineRegistry,
|
||||
hardware: &MachineNewHardwareEthercat,
|
||||
socket_queue_tx: Sender<(SocketRef, Arc<GenericEvent>)>,
|
||||
machine_manager: Weak<RwLock<Self>>,
|
||||
) {
|
||||
// empty ethercat machines
|
||||
self.ethercat_machines.clear();
|
||||
|
||||
// group devices by machine device identification
|
||||
let device_grouping_result = group_devices_by_identification(device_identifications);
|
||||
|
||||
tracing::info!(
|
||||
"Device Groups {}",
|
||||
"Device Groups {:?}",
|
||||
device_grouping_result.device_groups.len()
|
||||
);
|
||||
|
||||
@@ -67,59 +53,32 @@ impl MachineManager {
|
||||
// iterate over all identified device groups but ignore unaffected machines
|
||||
for device_group in device_grouping_result.device_groups.iter() {
|
||||
// get the machine identification
|
||||
let machine_identification_unique: MachineIdentificationUnique =
|
||||
match device_group.first() {
|
||||
Some(device_identification) => device_identification
|
||||
.device_machine_identification
|
||||
.machine_identification_unique
|
||||
.clone(),
|
||||
None => continue, // Skip this group if empty
|
||||
};
|
||||
|
||||
let slot =
|
||||
self.get_or_create_slot(socket_queue_tx.clone(), machine_identification_unique);
|
||||
let mut slot = slot.lock_blocking();
|
||||
let machine_identification = match device_group.first() {
|
||||
Some(device_identification) => &device_identification.device_machine_identification,
|
||||
None => continue, // Skip this group if empty
|
||||
};
|
||||
|
||||
// create the machine
|
||||
let new_machine = machine_registry.new_machine(&MachineNewParams {
|
||||
device_group,
|
||||
hardware: &machine_new_hardware,
|
||||
socket_queue_tx: socket_queue_tx.clone(),
|
||||
machine_manager: machine_manager.clone(),
|
||||
namespace: slot.namespace.clone(),
|
||||
});
|
||||
|
||||
slot.machine_connection = match new_machine {
|
||||
Err(err) => MachineConnectionGeneric::Error(err),
|
||||
Ok(machine) => MachineConnectionGeneric::Connected(machine),
|
||||
};
|
||||
// insert the machine into the ethercat machines map
|
||||
self.ethercat_machines.insert(
|
||||
machine_identification.machine_identification_unique.clone(),
|
||||
new_machine,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
fn get_or_create_slot(
|
||||
&mut self,
|
||||
socket_queue_tx: Sender<(SocketRef, Arc<GenericEvent>)>,
|
||||
machine_identification: MachineIdentificationUnique,
|
||||
) -> Arc<Mutex<MachineSlotGeneric>> {
|
||||
if let Some(slot) = self.get(&machine_identification) {
|
||||
return slot;
|
||||
}
|
||||
|
||||
let slot = Arc::new(Mutex::new(MachineSlot::new(socket_queue_tx)));
|
||||
self.ethercat_machines
|
||||
.insert(machine_identification, slot.clone());
|
||||
|
||||
return slot;
|
||||
}
|
||||
|
||||
pub fn get(
|
||||
&self,
|
||||
machine_identification: &MachineIdentificationUnique,
|
||||
) -> Option<Arc<Mutex<MachineSlotGeneric>>> {
|
||||
) -> Option<&Result<Box<Mutex<dyn Machine>>, anyhow::Error>> {
|
||||
self.ethercat_machines
|
||||
.get(machine_identification)
|
||||
.or_else(|| self.serial_machines.get(machine_identification))
|
||||
.map(|x| x.clone())
|
||||
}
|
||||
|
||||
pub fn add_serial_device(
|
||||
@@ -127,8 +86,6 @@ impl MachineManager {
|
||||
device_identification: &DeviceIdentification,
|
||||
device: Arc<RwLock<dyn SerialDevice>>,
|
||||
machine_registry: &MachineRegistry,
|
||||
socket_queue_tx: Sender<(SocketRef, Arc<GenericEvent>)>,
|
||||
machine_manager: Weak<RwLock<Self>>,
|
||||
) {
|
||||
let hardware = MachineNewHardwareSerial { device };
|
||||
|
||||
@@ -138,28 +95,19 @@ impl MachineManager {
|
||||
.try_into()
|
||||
.expect("Serial devices always have machine identification");
|
||||
|
||||
let machine_identification: MachineIdentificationUnique = device_identification_identified
|
||||
.device_machine_identification
|
||||
.machine_identification_unique
|
||||
.clone();
|
||||
|
||||
let slot = self.get_or_create_slot(socket_queue_tx.clone(), machine_identification);
|
||||
let mut slot = slot.lock_blocking();
|
||||
|
||||
let new_machine = machine_registry.new_machine(&MachineNewParams {
|
||||
device_group: &vec![device_identification_identified.clone()],
|
||||
hardware: &MachineNewHardware::Serial(&hardware),
|
||||
socket_queue_tx,
|
||||
machine_manager: machine_manager.clone(),
|
||||
namespace: slot.namespace.clone(),
|
||||
});
|
||||
|
||||
slot.machine_connection = match new_machine {
|
||||
Err(err) => MachineConnectionGeneric::Error(err),
|
||||
Ok(machine) => MachineConnectionGeneric::Connected(machine),
|
||||
};
|
||||
tracing::info!("Adding serial machine {:?}", new_machine);
|
||||
|
||||
tracing::info!("Adding serial machine {:?}", slot);
|
||||
self.serial_machines.insert(
|
||||
device_identification_identified
|
||||
.device_machine_identification
|
||||
.machine_identification_unique,
|
||||
new_machine,
|
||||
);
|
||||
}
|
||||
|
||||
pub fn remove_serial_device(&mut self, device_identification: &DeviceIdentification) {
|
||||
@@ -174,45 +122,11 @@ impl MachineManager {
|
||||
device_identification_identified
|
||||
);
|
||||
|
||||
let machine_identification_unique = &device_identification_identified
|
||||
.device_machine_identification
|
||||
.machine_identification_unique;
|
||||
|
||||
if let Some(slot) = self.serial_machines.get(machine_identification_unique) {
|
||||
let mut slot = slot.lock_blocking();
|
||||
slot.machine_connection = MachineConnection::Disconnected;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn get_ethercat_weak(
|
||||
&self,
|
||||
machine_identification: &MachineIdentificationUnique,
|
||||
) -> Option<Weak<Mutex<dyn Machine>>> {
|
||||
self.get_weak(&self.ethercat_machines, machine_identification)
|
||||
}
|
||||
|
||||
pub fn get_serial_weak(
|
||||
&self,
|
||||
machine_identification: &MachineIdentificationUnique,
|
||||
) -> Option<Weak<Mutex<dyn Machine>>> {
|
||||
self.get_weak(&self.serial_machines, machine_identification)
|
||||
}
|
||||
|
||||
fn get_weak(
|
||||
&self,
|
||||
machines: &HashMap<MachineIdentificationUnique, Arc<Mutex<MachineSlotGeneric>>>,
|
||||
machine_identification: &MachineIdentificationUnique,
|
||||
) -> Option<Weak<Mutex<dyn Machine>>> {
|
||||
let slot = machines.get(machine_identification).clone();
|
||||
|
||||
return slot.and_then(|slot| {
|
||||
let connection = &slot.lock_blocking().machine_connection;
|
||||
match connection {
|
||||
MachineConnectionGeneric::Error(_) => None,
|
||||
MachineConnectionGeneric::Disconnected => None,
|
||||
MachineConnectionGeneric::Connected(machine) => Some(Arc::downgrade(&machine)),
|
||||
}
|
||||
});
|
||||
self.serial_machines.remove(
|
||||
&device_identification_identified
|
||||
.device_machine_identification
|
||||
.machine_identification_unique,
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,25 +1,22 @@
|
||||
use std::collections::HashMap;
|
||||
use std::sync::Arc;
|
||||
|
||||
use smol::lock::Mutex;
|
||||
|
||||
use crate::machines::connection::MachineSlotGeneric;
|
||||
|
||||
use super::{identification::MachineIdentificationUnique, manager::MachineManager};
|
||||
use super::{Machine, identification::MachineIdentificationUnique, manager::MachineManager};
|
||||
|
||||
pub enum MachineManagerIter<'a> {
|
||||
EthercatMachines {
|
||||
iter: std::collections::hash_map::Iter<
|
||||
'a,
|
||||
MachineIdentificationUnique,
|
||||
Arc<Mutex<MachineSlotGeneric>>,
|
||||
Result<Box<Mutex<dyn Machine>>, anyhow::Error>,
|
||||
>,
|
||||
},
|
||||
SerialMachines {
|
||||
iter: std::collections::hash_map::Iter<
|
||||
'a,
|
||||
MachineIdentificationUnique,
|
||||
Arc<Mutex<MachineSlotGeneric>>,
|
||||
Result<Box<Mutex<dyn Machine>>, anyhow::Error>,
|
||||
>,
|
||||
},
|
||||
Done,
|
||||
@@ -27,13 +24,14 @@ pub enum MachineManagerIter<'a> {
|
||||
|
||||
pub struct MachineManagerIterator<'a> {
|
||||
iter: MachineManagerIter<'a>,
|
||||
serial_machines: &'a HashMap<MachineIdentificationUnique, Arc<Mutex<MachineSlotGeneric>>>,
|
||||
serial_machines:
|
||||
&'a HashMap<MachineIdentificationUnique, Result<Box<Mutex<dyn Machine>>, anyhow::Error>>,
|
||||
}
|
||||
|
||||
impl<'a> Iterator for MachineManagerIterator<'a> {
|
||||
type Item = (
|
||||
&'a MachineIdentificationUnique,
|
||||
&'a Arc<Mutex<MachineSlotGeneric>>,
|
||||
&'a Result<Box<Mutex<dyn Machine>>, anyhow::Error>,
|
||||
);
|
||||
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
@@ -73,7 +71,7 @@ impl<'a> Iterator for MachineManagerIterator<'a> {
|
||||
|
||||
impl MachineManager {
|
||||
// Returns an iterator over all machines (both ethercat and serial)
|
||||
pub fn iter(&self) -> MachineManagerIterator<'_> {
|
||||
pub fn iter(&self) -> MachineManagerIterator {
|
||||
MachineManagerIterator {
|
||||
iter: MachineManagerIter::EthercatMachines {
|
||||
iter: self.ethercat_machines.iter(),
|
||||
|
||||
@@ -1,23 +1,13 @@
|
||||
use crate::actors::Actor;
|
||||
use api::MachineApi;
|
||||
use new::MachineNewTrait;
|
||||
use std::any::Any;
|
||||
use std::fmt::Debug;
|
||||
|
||||
use crate::machines::{
|
||||
api::MachineApi, identification::MachineIdentificationUnique, new::MachineAct,
|
||||
};
|
||||
pub mod api;
|
||||
pub mod connection;
|
||||
pub mod identification;
|
||||
pub mod manager;
|
||||
pub mod manager_iter;
|
||||
pub mod new;
|
||||
pub mod registry;
|
||||
|
||||
pub trait Machine: MachineAct + MachineNewTrait + MachineApi + Any + Debug + Send + Sync {
|
||||
fn get_machine_identification_unique(&self) -> MachineIdentificationUnique;
|
||||
}
|
||||
|
||||
pub trait AnyGetters: Any {
|
||||
fn as_any(&self) -> &dyn Any;
|
||||
fn as_any_mut(&mut self) -> &mut dyn Any;
|
||||
}
|
||||
pub trait Machine: MachineNewTrait + MachineApi + Actor + Any + Debug {}
|
||||
@@ -1,8 +1,4 @@
|
||||
use crate::{
|
||||
machines::{identification::MachineIdentificationUnique, manager::MachineManager},
|
||||
serial::SerialDevice,
|
||||
socketio::namespace::Namespace,
|
||||
};
|
||||
use crate::serial::SerialDevice;
|
||||
|
||||
use super::identification::DeviceIdentificationIdentified;
|
||||
use anyhow::Error;
|
||||
@@ -10,28 +6,17 @@ use ethercat_hal::{
|
||||
devices::EthercatDevice, helpers::ethercrab_types::EthercrabSubDevicePreoperational,
|
||||
};
|
||||
use ethercrab::{SubDevice, SubDeviceRef};
|
||||
use smol::{
|
||||
channel::Sender,
|
||||
lock::{Mutex, RwLock},
|
||||
};
|
||||
use socketioxide::extract::SocketRef;
|
||||
use std::{
|
||||
sync::{Arc, Weak},
|
||||
time::Instant,
|
||||
};
|
||||
|
||||
use crate::socketio::event::GenericEvent;
|
||||
use smol::lock::RwLock;
|
||||
use std::sync::Arc;
|
||||
|
||||
pub trait MachineNewTrait {
|
||||
fn new(params: &MachineNewParams<'_, '_, '_, '_, '_, '_, '_>) -> Result<Self, Error>
|
||||
fn new<'maindevice, 'subdevices>(
|
||||
params: &MachineNewParams<'maindevice, 'subdevices, '_, '_, '_, '_, '_>,
|
||||
) -> Result<Self, Error>
|
||||
where
|
||||
Self: Sized;
|
||||
}
|
||||
|
||||
pub trait MachineAct {
|
||||
fn act(&mut self, now: Instant);
|
||||
}
|
||||
|
||||
pub struct MachineNewParams<
|
||||
'maindevice,
|
||||
'subdevices,
|
||||
@@ -54,21 +39,6 @@ pub struct MachineNewParams<
|
||||
'machine_new_hardware_etehrcat,
|
||||
'machine_new_hardware_serial,
|
||||
>,
|
||||
pub socket_queue_tx: Sender<(SocketRef, Arc<GenericEvent>)>,
|
||||
pub machine_manager: Weak<RwLock<MachineManager>>,
|
||||
|
||||
pub namespace: Arc<Mutex<Namespace>>,
|
||||
}
|
||||
|
||||
impl MachineNewParams<'_, '_, '_, '_, '_, '_, '_> {
|
||||
pub fn get_machine_identification_unique(&self) -> MachineIdentificationUnique {
|
||||
self.device_group
|
||||
.first()
|
||||
.expect("device group must have at least one device")
|
||||
.device_machine_identification
|
||||
.machine_identification_unique
|
||||
.clone()
|
||||
}
|
||||
}
|
||||
|
||||
pub enum MachineNewHardware<
|
||||
|
||||
@@ -1,21 +1,15 @@
|
||||
use super::{Machine, identification::MachineIdentification, new::MachineNewParams};
|
||||
use anyhow::Error;
|
||||
use smol::lock::Mutex;
|
||||
use std::{any::TypeId, collections::HashMap, sync::Arc};
|
||||
use std::{any::TypeId, collections::HashMap};
|
||||
|
||||
pub type MachineNewClosure =
|
||||
Box<dyn Fn(&MachineNewParams) -> Result<Arc<Mutex<dyn Machine>>, Error> + Send + Sync>;
|
||||
Box<dyn Fn(&MachineNewParams) -> Result<Box<Mutex<dyn Machine>>, Error> + Send + Sync>;
|
||||
|
||||
pub struct MachineRegistry {
|
||||
type_map: HashMap<TypeId, (MachineIdentification, MachineNewClosure)>,
|
||||
}
|
||||
|
||||
impl Default for MachineRegistry {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl MachineRegistry {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
@@ -30,10 +24,10 @@ impl MachineRegistry {
|
||||
self.type_map.insert(
|
||||
TypeId::of::<T>(),
|
||||
(
|
||||
machine_identficiation.clone(),
|
||||
machine_identficiation,
|
||||
// create a machine construction closure
|
||||
Box::new(|machine_new_params| {
|
||||
Ok(Arc::new(Mutex::new(T::new(machine_new_params)?)))
|
||||
Ok(Box::new(Mutex::new(T::new(machine_new_params)?)))
|
||||
}),
|
||||
),
|
||||
);
|
||||
@@ -42,7 +36,7 @@ impl MachineRegistry {
|
||||
pub fn new_machine(
|
||||
&self,
|
||||
machine_new_params: &MachineNewParams,
|
||||
) -> Result<Arc<Mutex<dyn Machine>>, anyhow::Error> {
|
||||
) -> Result<Box<Mutex<dyn Machine>>, anyhow::Error> {
|
||||
// get machiine identification
|
||||
let device_identification =
|
||||
&machine_new_params
|
||||
|
||||
@@ -1,10 +1,7 @@
|
||||
pub mod modbus_serial_interface;
|
||||
|
||||
use anyhow::Error;
|
||||
use crc::{CRC_16_MODBUS, Crc};
|
||||
use serialport::SerialPort;
|
||||
use serial;
|
||||
use std::time::Duration;
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum ParityType {
|
||||
Even,
|
||||
@@ -25,58 +22,6 @@ pub enum ModbusFunctionCode {
|
||||
DiagnoseFunction,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
#[repr(u8)]
|
||||
pub enum ModbusExceptionCode {
|
||||
None = 0,
|
||||
IllegalFunction = 1,
|
||||
IllegalDataAddress = 2,
|
||||
IllegalDataValue = 3,
|
||||
SlaveDeviceFailure = 4,
|
||||
Acknowledge = 5,
|
||||
SlaveDeviceBusy = 6,
|
||||
MemoryParityError = 8,
|
||||
GatewayPathUnavailable = 10,
|
||||
GatewayTargetDeviceFailedToRespond = 11,
|
||||
Unknown(u8), // For any unknown exception codes
|
||||
}
|
||||
|
||||
impl From<ModbusExceptionCode> for u8 {
|
||||
fn from(code: ModbusExceptionCode) -> Self {
|
||||
match code {
|
||||
ModbusExceptionCode::None => 0,
|
||||
ModbusExceptionCode::IllegalFunction => 1,
|
||||
ModbusExceptionCode::IllegalDataAddress => 2,
|
||||
ModbusExceptionCode::IllegalDataValue => 3,
|
||||
ModbusExceptionCode::SlaveDeviceFailure => 4,
|
||||
ModbusExceptionCode::Acknowledge => 5,
|
||||
ModbusExceptionCode::SlaveDeviceBusy => 6,
|
||||
ModbusExceptionCode::MemoryParityError => 8,
|
||||
ModbusExceptionCode::GatewayPathUnavailable => 10,
|
||||
ModbusExceptionCode::GatewayTargetDeviceFailedToRespond => 11,
|
||||
ModbusExceptionCode::Unknown(code) => code,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl From<u8> for ModbusExceptionCode {
|
||||
fn from(value: u8) -> Self {
|
||||
match value {
|
||||
0 => Self::None,
|
||||
1 => Self::IllegalFunction,
|
||||
2 => Self::IllegalDataAddress,
|
||||
3 => Self::IllegalDataValue,
|
||||
4 => Self::SlaveDeviceFailure,
|
||||
5 => Self::Acknowledge,
|
||||
6 => Self::SlaveDeviceBusy,
|
||||
8 => Self::MemoryParityError,
|
||||
10 => Self::GatewayPathUnavailable,
|
||||
11 => Self::GatewayTargetDeviceFailedToRespond,
|
||||
other => Self::Unknown(other),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||||
pub struct ModbusRequest {
|
||||
pub slave_id: u8,
|
||||
@@ -97,10 +42,10 @@ impl TryFrom<u8> for ModbusFunctionCode {
|
||||
|
||||
fn try_from(value: u8) -> Result<Self, Self::Error> {
|
||||
match value {
|
||||
0x03 => Ok(Self::ReadHoldingRegister),
|
||||
0x04 => Ok(Self::ReadInputRegister),
|
||||
0x06 => Ok(Self::PresetHoldingRegister),
|
||||
0x08 => Ok(Self::DiagnoseFunction),
|
||||
0x03 => Ok(ModbusFunctionCode::ReadHoldingRegister),
|
||||
0x04 => Ok(ModbusFunctionCode::ReadInputRegister),
|
||||
0x06 => Ok(ModbusFunctionCode::PresetHoldingRegister),
|
||||
0x08 => Ok(ModbusFunctionCode::DiagnoseFunction),
|
||||
_ => Err(anyhow::anyhow!("Error: Modbus Function Code doesnt exist!")),
|
||||
}
|
||||
}
|
||||
@@ -119,7 +64,7 @@ impl From<ModbusFunctionCode> for u8 {
|
||||
|
||||
impl From<ModbusRequest> for Vec<u8> {
|
||||
fn from(request: ModbusRequest) -> Self {
|
||||
let mut buffer = Self::new();
|
||||
let mut buffer = Vec::new();
|
||||
|
||||
buffer.push(request.slave_id);
|
||||
buffer.push(request.function_code.into()); // convert functioncode into u8 and push it
|
||||
@@ -132,7 +77,7 @@ impl From<ModbusRequest> for Vec<u8> {
|
||||
// convert u16 value to le_bytes and add it to the end of the frame
|
||||
buffer.extend_from_slice(&result_crc.to_le_bytes());
|
||||
|
||||
buffer
|
||||
return buffer;
|
||||
}
|
||||
}
|
||||
/// Reads data from a Modbus device via a serial port.
|
||||
@@ -143,7 +88,9 @@ impl From<ModbusRequest> for Vec<u8> {
|
||||
/// # Returns
|
||||
/// A `Result` containing an `Option` with the raw Modbus response as a `Vec<u8>` if successful,
|
||||
/// or `None` if the response is invalid or an error occurs.
|
||||
pub fn receive_data_modbus(port: &mut dyn SerialPort) -> Result<Option<Vec<u8>>, anyhow::Error> {
|
||||
pub fn receive_data_modbus(
|
||||
port: &mut dyn serial::SerialPort,
|
||||
) -> Result<Option<Vec<u8>>, anyhow::Error> {
|
||||
let mut buf: [u8; 256] = [0; 256];
|
||||
let data_length = port.read(&mut buf)?;
|
||||
if data_length == 0 {
|
||||
@@ -153,7 +100,7 @@ pub fn receive_data_modbus(port: &mut dyn SerialPort) -> Result<Option<Vec<u8>>,
|
||||
}
|
||||
|
||||
/// Computes Modbus CRC-16 using `crc` crate.
|
||||
pub const fn modbus_crc16(data: &[u8]) -> u16 {
|
||||
pub fn modbus_crc16(data: &[u8]) -> u16 {
|
||||
let modbus: Crc<u16> = Crc::<u16>::new(&CRC_16_MODBUS);
|
||||
modbus.checksum(data)
|
||||
}
|
||||
@@ -192,7 +139,7 @@ fn validate_modbus_response(raw_data: Vec<u8>) -> Result<Vec<u8>, Error> {
|
||||
|
||||
// expects to be given the entire raw message
|
||||
fn extract_crc(raw_data: &Vec<u8>) -> Result<u16, Error> {
|
||||
if raw_data.len() < 5 {
|
||||
if raw_data.len() < 2 {
|
||||
return Err(anyhow::anyhow!("Not enough data to extract CRC"));
|
||||
}
|
||||
|
||||
@@ -204,17 +151,23 @@ fn extract_crc(raw_data: &Vec<u8>) -> Result<u16, Error> {
|
||||
}
|
||||
impl TryFrom<Vec<u8>> for ModbusResponse {
|
||||
type Error = anyhow::Error;
|
||||
fn try_from(value: Vec<u8>) -> Result<Self, Error> {
|
||||
let crc = extract_crc(&value)?;
|
||||
fn try_from(value: Vec<u8>) -> Result<ModbusResponse, Error> {
|
||||
let crc = match extract_crc(&value) {
|
||||
Ok(crc_value) => crc_value,
|
||||
Err(err) => return Err(err),
|
||||
};
|
||||
|
||||
let function_code_res = ModbusFunctionCode::try_from(value[1]);
|
||||
let function_code = function_code_res?;
|
||||
let function_code = match function_code_res {
|
||||
Ok(code) => code,
|
||||
Err(err) => return Err(err),
|
||||
};
|
||||
|
||||
Ok(Self {
|
||||
Ok(ModbusResponse {
|
||||
slave_id: value[0],
|
||||
function_code,
|
||||
function_code: function_code,
|
||||
data: value[2..value.len() - 2].to_vec(), // get data without the crc
|
||||
crc,
|
||||
crc: crc,
|
||||
})
|
||||
}
|
||||
}
|
||||
@@ -226,23 +179,23 @@ impl TryFrom<Vec<u8>> for ModbusResponse {
|
||||
/// machine_operation_delay_nano: Delay for the given operation in nanoseconds as specified by the slaves datasheet (example: mitsubishi csfr84 has 12ms for read write in RAM)
|
||||
/// baudrate: bits per second
|
||||
/// message_size: size of original message in bytes
|
||||
pub const fn calculate_modbus_rtu_timeout(
|
||||
pub fn calculate_modbus_rtu_timeout(
|
||||
bits: u8,
|
||||
machine_operation_delay_nano: Duration,
|
||||
baudrate: u32,
|
||||
message_size: usize,
|
||||
) -> Duration {
|
||||
let nanoseconds_per_bit: u64 = (1000000 / baudrate) as u64;
|
||||
let nanoseconds_per_byte: u64 = bits as u64 * nanoseconds_per_bit;
|
||||
let nanoseconds_per_byte: u64 = bits as u64 * nanoseconds_per_bit as u64;
|
||||
|
||||
let transmission_timeout: u64 = nanoseconds_per_byte * message_size as u64;
|
||||
let silent_time: u64 = (nanoseconds_per_byte * (35)) / 10_u64; // silent_time is 3.5x of character length,which is 11 bit for 8E1
|
||||
let silent_time: u64 = (nanoseconds_per_byte * (35)) / 10 as u64; // silent_time is 3.5x of character length,which is 11 bit for 8E1
|
||||
|
||||
let mut full_timeout: u64 = transmission_timeout;
|
||||
full_timeout += machine_operation_delay_nano.as_nanos() as u64;
|
||||
full_timeout += silent_time;
|
||||
|
||||
Duration::from_nanos(full_timeout)
|
||||
return Duration::from_nanos(full_timeout);
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
|
||||
@@ -1,268 +0,0 @@
|
||||
use crate::modbus::{ModbusRequest, ModbusResponse};
|
||||
use ethercat_hal::io::serial_interface::{SerialEncoding, SerialInterface};
|
||||
use std::{
|
||||
collections::HashMap,
|
||||
time::{Duration, Instant},
|
||||
};
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
struct RequestMetaData {
|
||||
priority: u32,
|
||||
ignored_times: u32,
|
||||
/// This is used when a machine for example takes 4ms to process the request
|
||||
/// and is added ontop of the standard waiting time for a serial transfer
|
||||
extra_delay: Option<u32>,
|
||||
no_response_expected: bool,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum State {
|
||||
/// WaitingForResponse is set after sending a request through the serial_interface
|
||||
WaitingForResponse,
|
||||
/// After Sending a Request we need to wait atleast one ethercat cycle
|
||||
/// After one Cycle we check if el6021 status has transmit accepted toggled
|
||||
/// Then we can set state = ReadyToSend
|
||||
WaitingForRequestAccept,
|
||||
/// After Receiving a Response we need to wait atleast one ethercat cycle
|
||||
/// After one Cycle we check if el6021 status has received accepted toggled
|
||||
WaitingForReceiveAccept,
|
||||
/// ReadyToSend is set after receiving the response from the serial_interface
|
||||
ReadyToSend,
|
||||
/// Initial State
|
||||
Uninitialized,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct ModbusSerialInterface {
|
||||
serial_interface: SerialInterface,
|
||||
baudrate: Option<u32>,
|
||||
encoding: Option<SerialEncoding>,
|
||||
response: Option<ModbusResponse>,
|
||||
state: State,
|
||||
last_message_size: usize,
|
||||
last_message_delay: u32,
|
||||
pub last_message_id: u32,
|
||||
no_response_expected: bool,
|
||||
request_map: HashMap<u32, ModbusRequest>,
|
||||
request_metadata_map: HashMap<u32, RequestMetaData>,
|
||||
last_ts: Instant,
|
||||
}
|
||||
|
||||
impl ModbusSerialInterface {
|
||||
pub fn new(serial_interface: SerialInterface) -> Self {
|
||||
Self {
|
||||
serial_interface,
|
||||
baudrate: None,
|
||||
encoding: None,
|
||||
request_map: HashMap::new(),
|
||||
request_metadata_map: HashMap::new(),
|
||||
response: None,
|
||||
last_message_size: 0,
|
||||
last_message_delay: 0,
|
||||
state: State::Uninitialized,
|
||||
last_ts: Instant::now(),
|
||||
last_message_id: 0,
|
||||
no_response_expected: false,
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn is_initialized(&self) -> bool {
|
||||
!matches!(self.state, State::Uninitialized)
|
||||
}
|
||||
|
||||
pub fn add_request(
|
||||
&mut self,
|
||||
request_id: u32,
|
||||
priority: u32,
|
||||
request: ModbusRequest,
|
||||
no_response_expected: bool,
|
||||
delay: Option<u32>,
|
||||
) {
|
||||
self.request_map.insert(request_id, request);
|
||||
self.request_metadata_map
|
||||
.entry(request_id)
|
||||
.or_insert_with(|| RequestMetaData {
|
||||
priority,
|
||||
ignored_times: 0,
|
||||
extra_delay: delay,
|
||||
no_response_expected,
|
||||
});
|
||||
}
|
||||
|
||||
/// This is used internally to read the receive buffer of the el6021
|
||||
async fn read_modbus_response(&mut self) -> Result<ModbusResponse, anyhow::Error> {
|
||||
let raw_response = (self.serial_interface.read_message)()
|
||||
.await
|
||||
.unwrap_or_default();
|
||||
|
||||
let response = ModbusResponse::try_from(raw_response)?;
|
||||
self.last_message_size = response.data.len() + 4;
|
||||
self.state = State::WaitingForReceiveAccept;
|
||||
Ok(response)
|
||||
}
|
||||
|
||||
/// This is used internally to fill the write buffer of the el6021 with the modbus request
|
||||
/// Decides what requests to send first by finding the one with the highest priority
|
||||
/// For example Highest Priority requests: ResetInverter StopMotor
|
||||
async fn send_modbus_request(&mut self) {
|
||||
if self.request_map.is_empty() {
|
||||
return;
|
||||
}
|
||||
|
||||
let (highest_id, delay, no_response_expected) = self
|
||||
.request_metadata_map
|
||||
.iter_mut()
|
||||
.map(|(key, value)| {
|
||||
let effective_priority = value.priority + value.ignored_times;
|
||||
(
|
||||
*key,
|
||||
effective_priority,
|
||||
value.extra_delay.unwrap_or(0),
|
||||
value.no_response_expected,
|
||||
)
|
||||
})
|
||||
.max_by_key(|(_, priority, _, _)| *priority)
|
||||
.map(|(key, _, delay, no_response)| (key, delay, no_response))
|
||||
.unwrap(); // Safe because we checked is_empty() above
|
||||
|
||||
let request = &self.request_map[&highest_id];
|
||||
let modbus_request: Vec<u8> = request.clone().into();
|
||||
|
||||
if let Err(_) = (self.serial_interface.write_message)(modbus_request.clone()).await {
|
||||
tracing::error!("ERROR: serial_interface.write_message has failed");
|
||||
} else {
|
||||
self.last_message_delay = delay;
|
||||
self.last_message_id = highest_id;
|
||||
self.no_response_expected = no_response_expected;
|
||||
}
|
||||
|
||||
self.state = State::WaitingForRequestAccept;
|
||||
self.last_message_size = modbus_request.len();
|
||||
}
|
||||
|
||||
pub async fn initialize_communication_settings(&mut self) -> bool {
|
||||
let baudrate = (self.serial_interface.get_baudrate)().await;
|
||||
let encoding = (self.serial_interface.get_serial_encoding)().await;
|
||||
|
||||
match (baudrate, encoding) {
|
||||
(Some(b), Some(e)) => {
|
||||
self.baudrate = Some(b);
|
||||
self.encoding = Some(e);
|
||||
true
|
||||
}
|
||||
_ => false,
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn initialize(&mut self) -> bool {
|
||||
if matches!(self.state, State::Uninitialized) {
|
||||
let success = (self.serial_interface.initialize)().await;
|
||||
if success {
|
||||
self.state = State::ReadyToSend;
|
||||
self.initialize_communication_settings().await;
|
||||
}
|
||||
success
|
||||
} else {
|
||||
false
|
||||
}
|
||||
}
|
||||
|
||||
fn increment_ignored_times(&mut self) {
|
||||
for metadata in self.request_metadata_map.values_mut() {
|
||||
metadata.ignored_times += 1;
|
||||
}
|
||||
}
|
||||
|
||||
/// Modbus RTU has silent time between frames that needs to be adhered to, if you send before silent_time is over between frames, then there will be lost frames
|
||||
/// This silent time is needed to identify the start and end of messages
|
||||
/// This function also takes into account the time that the slave we are talking to needs to process our request
|
||||
/// bits: amount of bits sent per byte -> for a 8n1 coding: 8 data bits, 0 parity, 1 stop bit (1 start,1 stop) -> 10 bits
|
||||
/// machine_operation_delay_nano: Delay for the given operation in nanoseconds as specified by the slaves datasheet (example: mitsubishi csfr84 has 12ms for read write in RAM)
|
||||
/// baudrate: bits per second
|
||||
/// message_size: size of original message in bytes
|
||||
pub fn calculate_modbus_rtu_timeout(
|
||||
&self,
|
||||
machine_operation_delay: Duration,
|
||||
message_size: usize,
|
||||
) -> Option<Duration> {
|
||||
let baudrate = self.baudrate?;
|
||||
let encoding = self.encoding?;
|
||||
|
||||
let nanoseconds_per_bit = 1_000_000_000 / baudrate as u64;
|
||||
let nanoseconds_per_byte = encoding.total_bits() as u64 * nanoseconds_per_bit;
|
||||
|
||||
let transmission_timeout = nanoseconds_per_byte * message_size as u64;
|
||||
let silent_time = (nanoseconds_per_byte * 35) / 10; // silent_time is 3.5x of character length
|
||||
|
||||
let total_timeout =
|
||||
transmission_timeout + machine_operation_delay.as_nanos() as u64 + silent_time;
|
||||
Some(Duration::from_nanos(total_timeout))
|
||||
}
|
||||
|
||||
pub const fn get_response(&self) -> Option<&ModbusResponse> {
|
||||
self.response.as_ref()
|
||||
}
|
||||
|
||||
pub async fn act(&mut self, now_ts: Instant) {
|
||||
let elapsed = now_ts.duration_since(self.last_ts);
|
||||
|
||||
let timeout = self.calculate_modbus_rtu_timeout(
|
||||
Duration::from_nanos(self.last_message_delay as u64),
|
||||
self.last_message_size,
|
||||
);
|
||||
|
||||
let Some(timeout) = timeout else { return };
|
||||
|
||||
// Check if we need to wait for timeout (except for specific states)
|
||||
if !matches!(
|
||||
self.state,
|
||||
State::Uninitialized | State::WaitingForReceiveAccept | State::WaitingForRequestAccept
|
||||
) && elapsed < timeout
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
self.last_ts = now_ts;
|
||||
self.response = None;
|
||||
|
||||
match self.state {
|
||||
State::WaitingForResponse => match self.read_modbus_response().await {
|
||||
Ok(response) => {
|
||||
self.response = Some(response);
|
||||
}
|
||||
Err(_) => {
|
||||
self.response = None;
|
||||
if self.no_response_expected {
|
||||
self.state = State::ReadyToSend;
|
||||
}
|
||||
}
|
||||
},
|
||||
State::ReadyToSend => {
|
||||
self.send_modbus_request().await;
|
||||
|
||||
// Remove the sent request
|
||||
self.request_map.remove(&self.last_message_id);
|
||||
self.request_metadata_map.remove(&self.last_message_id);
|
||||
self.increment_ignored_times();
|
||||
}
|
||||
State::WaitingForReceiveAccept => {
|
||||
// Waste at least one Ethercat Cycle here to ensure that request/response stay in sync
|
||||
self.state = State::ReadyToSend;
|
||||
}
|
||||
State::WaitingForRequestAccept => {
|
||||
// An empty vec is used to check if we are finished with writing the message
|
||||
// This is to keep the Serialinterface more simple
|
||||
match (self.serial_interface.write_message)(vec![]).await {
|
||||
Ok(finished) if finished => {
|
||||
self.state = State::WaitingForResponse;
|
||||
}
|
||||
Err(_) => {
|
||||
self.state = State::ReadyToSend;
|
||||
}
|
||||
_ => {} // Still waiting
|
||||
}
|
||||
}
|
||||
State::Uninitialized => {}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,178 +0,0 @@
|
||||
use tracing::debug;
|
||||
use tracing::error;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
/// Makes the current thread a real-time thread using the PREEMPT_RT capabilities of Linux.
|
||||
///
|
||||
/// This function configures the current thread with real-time scheduling properties by:
|
||||
///
|
||||
/// 1. Setting the scheduling policy to SCHED_FIFO (First-In-First-Out), which is a real-time
|
||||
/// scheduling policy that allows the thread to run until it voluntarily yields, blocks on I/O,
|
||||
/// or is preempted by a higher-priority real-time thread.
|
||||
///
|
||||
/// 2. Setting the real-time priority to 80 (on a scale of 1-99, where higher values indicate
|
||||
/// higher priority). This priority level is chosen to be higher than most IRQ handlers
|
||||
/// (typically priority 50) but lower than critical kernel tasks (priority 99).
|
||||
///
|
||||
/// When running on a PREEMPT_RT patched Linux kernel, this combination provides deterministic
|
||||
/// scheduling with minimal latency, which is essential for real-time applications.
|
||||
///
|
||||
/// # Requirements
|
||||
///
|
||||
/// - The program must run with appropriate permissions to set real-time priorities.
|
||||
/// This typically means either:
|
||||
/// - Running as root
|
||||
/// - Configuring `/etc/security/limits.conf` to allow the user to set real-time priorities
|
||||
///
|
||||
/// - The system should be running a Linux kernel with PREEMPT_RT patch applied for
|
||||
/// optimal real-time performance.
|
||||
///
|
||||
/// # Example
|
||||
///
|
||||
/// ```ignore
|
||||
/// fn main() {
|
||||
/// // Lock memory to prevent page faults
|
||||
/// lock_memory();
|
||||
///
|
||||
/// // Create a thread and make it real-time
|
||||
/// std::thread::spawn(|| {
|
||||
/// set_realtime_priority();
|
||||
///
|
||||
/// // Real-time code here
|
||||
/// });
|
||||
/// }
|
||||
/// ```
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// - `Ok(())` if the real-time priority was successfully set
|
||||
/// - `Err` with the error message if setting the priority failed
|
||||
#[cfg(target_os = "linux")]
|
||||
pub fn set_realtime_priority() -> Result<(), anyhow::Error> {
|
||||
use anyhow::anyhow;
|
||||
use libc::{SCHED_FIFO, pthread_setschedparam, sched_param};
|
||||
use std::mem;
|
||||
|
||||
unsafe {
|
||||
// Get the pthread_t of the current thread using pthread_self()
|
||||
let pthread_self = libc::pthread_self();
|
||||
|
||||
// Set up the scheduling parameters
|
||||
let mut param: sched_param = mem::zeroed();
|
||||
param.sched_priority = 95;
|
||||
|
||||
// Set the thread to use SCHED_FIFO scheduling policy with our priority
|
||||
let result = pthread_setschedparam(pthread_self, SCHED_FIFO, ¶m);
|
||||
|
||||
if result != 0 {
|
||||
error!(
|
||||
"Failed to set real-time scheduling policy for thread \"{:?}\": {}",
|
||||
std::thread::current().name().unwrap_or("unknown"),
|
||||
std::io::Error::last_os_error()
|
||||
);
|
||||
return Err(anyhow!(
|
||||
"Failed to set real-time priority: {}",
|
||||
std::io::Error::last_os_error()
|
||||
));
|
||||
} else {
|
||||
debug!(
|
||||
"Successfully set real-time priority for the thread \"{:?}\"",
|
||||
std::thread::current().name().unwrap_or("unknown")
|
||||
);
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub fn set_realtime_priority() -> Result<(), anyhow::Error> {
|
||||
tracing::error!("This platform does not support realtime threads");
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Locks all current and future memory pages of the process into RAM to prevent page faults.
|
||||
///
|
||||
/// # When to call this function
|
||||
///
|
||||
/// This function should be called from the main thread, as early as possible in your program's
|
||||
/// execution, ideally before creating any threads and before allocating large amounts of memory.
|
||||
///
|
||||
/// Important: You only need to call this function ONCE per process, not in each thread.
|
||||
/// The memory locking applies to the entire process, including:
|
||||
/// - The main thread
|
||||
/// - All threads created after the call
|
||||
///
|
||||
/// This happens because all threads in a process share the same virtual memory space, and
|
||||
/// `mlockall()` operates on the entire process's memory space, not just the calling thread.
|
||||
///
|
||||
/// # Example: Correct usage
|
||||
///
|
||||
/// ```ignore
|
||||
/// fn main() -> Result<(), Box<dyn std::error::Error>> {
|
||||
/// // Call early in main, before creating threads
|
||||
/// lock_memory()?;
|
||||
///
|
||||
/// // Now create real-time threads...
|
||||
/// std::thread::spawn(|| {
|
||||
/// set_realtime_priority();
|
||||
/// // Real-time code here
|
||||
/// });
|
||||
///
|
||||
/// Ok(())
|
||||
/// }
|
||||
/// ```
|
||||
///
|
||||
/// # Example: Incorrect usage
|
||||
///
|
||||
/// ```ignore
|
||||
/// fn main() {
|
||||
/// // Don't do this - calling in each thread is unnecessary
|
||||
/// std::thread::spawn(|| {
|
||||
/// lock_memory().unwrap(); // Redundant if already called in main
|
||||
/// set_realtime_priority();
|
||||
/// });
|
||||
/// }
|
||||
/// ```
|
||||
///
|
||||
/// # How it works
|
||||
///
|
||||
/// [Rest of documentation as before...]
|
||||
#[cfg(target_os = "linux")]
|
||||
pub fn lock_memory() -> Result<(), Box<dyn std::error::Error>> {
|
||||
use libc::{MCL_FUTURE, mlockall};
|
||||
|
||||
unsafe {
|
||||
if mlockall(MCL_FUTURE) != 0 {
|
||||
error!("Failed to lock memory: {}", std::io::Error::last_os_error());
|
||||
return Err(
|
||||
format!("Failed to lock memory: {}", std::io::Error::last_os_error()).into(),
|
||||
);
|
||||
} else {
|
||||
debug!("Successfully locked memory for the process");
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub fn lock_memory() -> Result<(), Box<dyn std::error::Error>> {
|
||||
tracing::error!("This platform does not support memory locking");
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn set_core_affinity(core_id: usize) -> Result<(), anyhow::Error> {
|
||||
let cores = core_affinity::get_core_ids().unwrap_or_default();
|
||||
let core = cores.get(core_id);
|
||||
if let Some(core) = core {
|
||||
core_affinity::set_for_current(*core);
|
||||
debug!(
|
||||
"Set core affinity of thread \"{:?}\" to core {:?}",
|
||||
std::thread::current().name().unwrap_or("unknown"),
|
||||
core
|
||||
);
|
||||
Ok(())
|
||||
} else {
|
||||
error!("No cores available to set affinity");
|
||||
Err(anyhow::anyhow!("No cores available to set affinity"))
|
||||
}
|
||||
}
|
||||
@@ -11,13 +11,13 @@ pub struct MutationResponse {
|
||||
}
|
||||
|
||||
impl MutationResponse {
|
||||
pub const fn success() -> Self {
|
||||
pub fn success() -> Self {
|
||||
Self {
|
||||
success: true,
|
||||
error: None,
|
||||
}
|
||||
}
|
||||
pub const fn error(error: String) -> Self {
|
||||
pub fn error(error: String) -> Self {
|
||||
Self {
|
||||
success: false,
|
||||
error: Some(error),
|
||||
@@ -33,19 +33,3 @@ where
|
||||
pub machine_identification_unique: MachineIdentificationUnique,
|
||||
pub data: T,
|
||||
}
|
||||
|
||||
#[derive(Debug, serde::Deserialize)]
|
||||
pub struct MachineVideoStreamListBody {
|
||||
pub machine_identification_unique: MachineIdentificationUnique,
|
||||
}
|
||||
|
||||
#[derive(Debug, serde::Deserialize)]
|
||||
pub struct MachineVideoStreamBody {
|
||||
pub machine_identification_unique: MachineIdentificationUnique,
|
||||
pub stream_id: String,
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
pub struct VideoStreamListResponse {
|
||||
pub streams: Vec<String>,
|
||||
}
|
||||
@@ -1,13 +1,10 @@
|
||||
use std::{any::Any, sync::Arc};
|
||||
|
||||
use smol::{channel::Sender, lock::RwLock};
|
||||
use smol::lock::RwLock;
|
||||
use std::fmt::Debug;
|
||||
|
||||
use crate::{
|
||||
machines::identification::DeviceIdentification, serial::serial_detection::SerialDeviceRemoval,
|
||||
};
|
||||
use crate::machines::identification::DeviceIdentification;
|
||||
|
||||
pub mod panic;
|
||||
pub mod registry;
|
||||
pub mod serial_detection;
|
||||
|
||||
@@ -27,10 +24,10 @@ pub trait SerialDeviceThread {
|
||||
|
||||
pub struct SerialDeviceNewParams {
|
||||
pub path: String,
|
||||
pub device_thread_panic_tx: Sender<SerialDeviceRemoval<String>>,
|
||||
pub device_thread_panix_tx: smol::channel::Sender<(String, anyhow::Error)>,
|
||||
}
|
||||
|
||||
#[derive(PartialEq, Eq, Clone, Debug)]
|
||||
#[derive(PartialEq, Clone, Debug)]
|
||||
pub struct SerialDeviceIdentification {
|
||||
pub vendor_id: u16,
|
||||
pub product_id: u16,
|
||||
|
||||
@@ -1,53 +0,0 @@
|
||||
use smol::channel::Sender;
|
||||
|
||||
use crate::serial::serial_detection::SerialDeviceRemoval;
|
||||
|
||||
/// Device-level panic handler for individual device crashes
|
||||
/// Sends device identifier and error information for cleanup
|
||||
/// Used by serial devices and other individual components that need to be removed on panic
|
||||
pub fn send_serial_device_panic<T>(
|
||||
device_identifier: T,
|
||||
thread_panic_tx: Sender<SerialDeviceRemoval<T>>,
|
||||
) where
|
||||
T: Send + Sync + Clone + 'static,
|
||||
{
|
||||
// Ensure backtrace is enabled for panics
|
||||
if std::env::var("RUST_BACKTRACE").is_err() {
|
||||
unsafe {
|
||||
std::env::set_var("RUST_BACKTRACE", "full");
|
||||
}
|
||||
}
|
||||
|
||||
std::panic::set_hook(Box::new(move |panic_info| {
|
||||
let thread_name = std::thread::current()
|
||||
.name()
|
||||
.unwrap_or("unnamed")
|
||||
.to_string();
|
||||
let location = panic_info.location().map_or("unknown".to_string(), |loc| {
|
||||
format!("{}:{}:{}", loc.file(), loc.line(), loc.column())
|
||||
});
|
||||
let message = panic_info
|
||||
.payload()
|
||||
.downcast_ref::<&str>()
|
||||
.unwrap_or(&"Box<dyn Any>")
|
||||
.to_string();
|
||||
|
||||
let panic_message = format!(
|
||||
"Device panicked in thread '{}' at {}: {}",
|
||||
thread_name, location, message
|
||||
);
|
||||
|
||||
// Send device panic info through channel
|
||||
smol::block_on(async {
|
||||
let _ = thread_panic_tx
|
||||
.send(SerialDeviceRemoval::Error(
|
||||
device_identifier.clone(),
|
||||
anyhow::anyhow!("{}", panic_message),
|
||||
))
|
||||
.await;
|
||||
});
|
||||
|
||||
// Note: We don't call old_hook to avoid duplicate logging
|
||||
// All logging is handled in the main thread when receiving panic details
|
||||
}));
|
||||
}
|
||||
@@ -20,12 +20,6 @@ pub struct SerialDeviceRegistry {
|
||||
pub type_map: HashMap<TypeId, (SerialDeviceIdentification, SerialDeviceNewClosure)>,
|
||||
}
|
||||
|
||||
impl Default for SerialDeviceRegistry {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl SerialDeviceRegistry {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
|
||||
@@ -21,11 +21,6 @@ use super::{
|
||||
SerialDevice, SerialDeviceIdentification, SerialDeviceNewParams, registry::SerialDeviceRegistry,
|
||||
};
|
||||
|
||||
pub enum SerialDeviceRemoval<T> {
|
||||
Disconnect(T),
|
||||
Error(T, anyhow::Error),
|
||||
}
|
||||
|
||||
pub struct SerialDetection<'serialdeviceregistry> {
|
||||
pub serial_device_registry: &'serialdeviceregistry SerialDeviceRegistry,
|
||||
pub ports: HashMap<
|
||||
@@ -36,13 +31,14 @@ pub struct SerialDetection<'serialdeviceregistry> {
|
||||
Arc<RwLock<dyn SerialDevice>>,
|
||||
),
|
||||
>,
|
||||
pub device_removal_signal_rx: Receiver<SerialDeviceRemoval<String>>,
|
||||
pub device_removal_signal_tx: Sender<SerialDeviceRemoval<String>>,
|
||||
pub device_removal_signal_rx: Receiver<(String, anyhow::Error)>,
|
||||
pub device_removal_signal_tx: Sender<(String, anyhow::Error)>,
|
||||
}
|
||||
|
||||
impl<'serialdeviceregistry> SerialDetection<'serialdeviceregistry> {
|
||||
pub fn new(serial_device_registry: &'serialdeviceregistry SerialDeviceRegistry) -> Self {
|
||||
let (device_removal_signal_tx, device_removal_signal_rx) = unbounded();
|
||||
let (device_removal_signal_tx, device_removal_signal_rx) =
|
||||
unbounded::<(String, anyhow::Error)>();
|
||||
SerialDetection {
|
||||
serial_device_registry,
|
||||
ports: HashMap::new(),
|
||||
@@ -82,12 +78,15 @@ impl<'serialdeviceregistry> SerialDetection<'serialdeviceregistry> {
|
||||
let mut usb_ports: Vec<(String, UsbPortInfo)> = Vec::new();
|
||||
|
||||
for port in port_list {
|
||||
if let SerialPortType::UsbPort(usb_info) = &port.port_type {
|
||||
usb_ports.push((port.port_name.to_string(), usb_info.clone()));
|
||||
match &port.port_type {
|
||||
SerialPortType::UsbPort(usb_info) => {
|
||||
usb_ports.push((port.port_name.to_string(), usb_info.clone()));
|
||||
}
|
||||
_ => {}
|
||||
};
|
||||
}
|
||||
|
||||
usb_ports
|
||||
return usb_ports;
|
||||
}
|
||||
|
||||
pub async fn check_ports(&mut self) -> CheckPortsResult {
|
||||
@@ -130,7 +129,7 @@ impl<'serialdeviceregistry> SerialDetection<'serialdeviceregistry> {
|
||||
let device_result = self.serial_device_registry.new_serial_device(
|
||||
&SerialDeviceNewParams {
|
||||
path: added.0.clone(),
|
||||
device_thread_panic_tx: self.device_removal_signal_tx.clone(),
|
||||
device_thread_panix_tx: self.device_removal_signal_tx.clone(),
|
||||
},
|
||||
&serial_device_identification,
|
||||
);
|
||||
@@ -159,36 +158,18 @@ impl<'serialdeviceregistry> SerialDetection<'serialdeviceregistry> {
|
||||
|
||||
pub async fn check_remove_signals(&mut self) -> Vec<DeviceIdentification> {
|
||||
let mut removed_signals: Vec<DeviceIdentification> = Vec::new();
|
||||
match self.device_removal_signal_rx.try_recv() {
|
||||
Ok((path, error)) => {
|
||||
// remove the device when the tuple positon 1 equals signal
|
||||
if let Some(info) = self.ports.get(&path.clone()) {
|
||||
removed_signals.push(info.1.clone());
|
||||
};
|
||||
self.ports.remove(&path);
|
||||
|
||||
let mut add_removal_signal = |path: &String| {
|
||||
if let Some(info) = self.ports.get(path) {
|
||||
removed_signals.push(info.1.clone());
|
||||
};
|
||||
self.ports.remove(path);
|
||||
};
|
||||
|
||||
if self.device_removal_signal_rx.is_empty() {
|
||||
return removed_signals;
|
||||
}
|
||||
|
||||
let removal = self
|
||||
.device_removal_signal_rx
|
||||
.recv()
|
||||
.await
|
||||
.expect("Failed to receive removal signals");
|
||||
|
||||
match removal {
|
||||
SerialDeviceRemoval::Disconnect(path) => {
|
||||
add_removal_signal(&path);
|
||||
tracing::trace!("Removed serial device after disconnect: {:?}", path);
|
||||
}
|
||||
|
||||
SerialDeviceRemoval::Error(path, error) => {
|
||||
add_removal_signal(&path);
|
||||
tracing::error!("Removed erroring serial device: {:?}: {}", path, error);
|
||||
tracing::trace!("Removed device: {:?}: {}", path, error);
|
||||
}
|
||||
Err(_) => {}
|
||||
}
|
||||
|
||||
removed_signals
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,34 +1,20 @@
|
||||
use std::time::{SystemTime, UNIX_EPOCH};
|
||||
|
||||
use erased_serde::Serialize as ErasedSerialize;
|
||||
use serde::Serialize;
|
||||
use serde_json::Value;
|
||||
|
||||
#[derive(Serialize)]
|
||||
#[derive(Debug, Clone, Serialize)]
|
||||
pub struct GenericEvent {
|
||||
pub name: String,
|
||||
pub data: Box<dyn ErasedSerialize + Send + Sync>,
|
||||
pub data: Value,
|
||||
/// Timestamp in milliseconds
|
||||
pub ts: u64,
|
||||
}
|
||||
|
||||
impl std::fmt::Debug for GenericEvent {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
f.debug_struct("GenericEvent")
|
||||
.field("name", &self.name)
|
||||
.field("data", &"[erased]")
|
||||
.field("ts", &self.ts)
|
||||
.finish()
|
||||
}
|
||||
}
|
||||
|
||||
pub trait BuildEvent: Serialize + Send + Sync + Clone {
|
||||
fn build(&self) -> Event<Self>;
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Serialize)]
|
||||
pub struct Event<T>
|
||||
where
|
||||
T: Serialize + Send + Sync + 'static,
|
||||
T: Serialize,
|
||||
{
|
||||
pub name: String,
|
||||
pub data: T,
|
||||
@@ -36,35 +22,39 @@ where
|
||||
pub ts: u64,
|
||||
}
|
||||
|
||||
impl<T> From<Event<T>> for GenericEvent
|
||||
impl<T> TryFrom<Event<T>> for GenericEvent
|
||||
where
|
||||
T: Serialize + Send + Sync + 'static,
|
||||
T: Serialize,
|
||||
{
|
||||
fn from(event: Event<T>) -> Self {
|
||||
Self {
|
||||
type Error = serde_json::Error;
|
||||
|
||||
fn try_from(event: Event<T>) -> Result<Self, Self::Error> {
|
||||
Ok(Self {
|
||||
name: event.name,
|
||||
data: Box::new(event.data),
|
||||
data: serde_json::to_value(event.data)?,
|
||||
ts: event.ts,
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl<T> From<&Event<T>> for GenericEvent
|
||||
impl<T> TryFrom<&Event<T>> for GenericEvent
|
||||
where
|
||||
T: Serialize + Clone + Send + Sync + 'static,
|
||||
T: Serialize,
|
||||
{
|
||||
fn from(event: &Event<T>) -> Self {
|
||||
Self {
|
||||
type Error = serde_json::Error;
|
||||
|
||||
fn try_from(event: &Event<T>) -> Result<Self, Self::Error> {
|
||||
Ok(Self {
|
||||
name: event.name.clone(),
|
||||
data: Box::new(event.data.clone()),
|
||||
data: serde_json::to_value(&event.data)?,
|
||||
ts: event.ts,
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl<T> Event<T>
|
||||
where
|
||||
T: Serialize + Clone + Send + Sync + 'static,
|
||||
T: Serialize + Clone,
|
||||
{
|
||||
pub fn new(event: &str, data: T) -> Self {
|
||||
Self {
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
pub mod event;
|
||||
pub mod namespace;
|
||||
pub mod namespace_id;
|
||||
pub mod socket_queue;
|
||||
@@ -1,64 +1,24 @@
|
||||
use crate::socketio::event::GenericEvent;
|
||||
use smol::channel::Sender;
|
||||
use socketioxide::extract::SocketRef;
|
||||
use std::{collections::HashMap, sync::Arc, time::Duration};
|
||||
use socketioxide::{extract::SocketRef, socket::Sid};
|
||||
use std::{collections::HashMap, time::Duration};
|
||||
use tracing::instrument;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub struct Namespace {
|
||||
pub sockets: Vec<SocketRef>,
|
||||
pub events: HashMap<String, Vec<Arc<GenericEvent>>>,
|
||||
pub socket_queue_tx: Sender<(SocketRef, Arc<GenericEvent>)>,
|
||||
}
|
||||
use super::socket_queue::SocketQueue;
|
||||
|
||||
impl Namespace {
|
||||
pub fn new(socket_queue_tx: Sender<(SocketRef, Arc<GenericEvent>)>) -> Self {
|
||||
Self {
|
||||
sockets: vec![],
|
||||
events: HashMap::new(),
|
||||
socket_queue_tx,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Namespace {
|
||||
pub trait NamespaceInterface {
|
||||
/// Adds a socket to the namespace.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `socket` - A reference to the socket to be added
|
||||
#[instrument(skip_all)]
|
||||
pub fn subscribe(&mut self, socket: SocketRef) {
|
||||
// add the socket to the list
|
||||
self.sockets.push(socket);
|
||||
}
|
||||
fn subscribe(&mut self, socket: SocketRef);
|
||||
|
||||
/// Removes a socket from the namespace.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `socket` - A reference to the socket to be removed
|
||||
#[instrument(skip_all)]
|
||||
pub fn unsubscribe(&mut self, socket: SocketRef) {
|
||||
// remove the socket from the list
|
||||
self.sockets.retain(|s| s.id != socket.id);
|
||||
}
|
||||
|
||||
/// Disconnects all clients in the namespace.
|
||||
///
|
||||
/// This will forcefully disconnect all sockets and clear the socket list.
|
||||
#[instrument(skip_all)]
|
||||
pub fn disconnect_all(&mut self) {
|
||||
tracing::info!("Disconnecting {} sockets in namespace", self.sockets.len());
|
||||
|
||||
// Disconnect each socket
|
||||
for socket in &self.sockets {
|
||||
let _ = socket.clone().disconnect(); // Ignore errors if socket is already disconnected
|
||||
}
|
||||
|
||||
// Clear the socket list
|
||||
self.sockets.clear();
|
||||
}
|
||||
fn unsubscribe(&mut self, socket: SocketRef);
|
||||
|
||||
/// Re-emits cached events to a specific socket.
|
||||
///
|
||||
@@ -68,23 +28,14 @@ impl Namespace {
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `socket` - A reference to the socket that will receive the cached events
|
||||
#[instrument(skip_all)]
|
||||
pub fn reemit(&mut self, socket: SocketRef) {
|
||||
// Collect events grouped by name/kind with their counts for sorting
|
||||
let mut event_groups: Vec<(&String, &Vec<Arc<GenericEvent>>)> =
|
||||
self.events.iter().collect();
|
||||
fn reemit(&mut self, socket: SocketRef);
|
||||
|
||||
// Sort by event count (ascending - lowest count first)
|
||||
event_groups.sort_by(|a, b| a.1.len().cmp(&b.1.len()));
|
||||
|
||||
// Emit events in order of lowest count first
|
||||
for (_event_name, events) in event_groups {
|
||||
for event in events {
|
||||
// Send to global queue instead of per-socket queue
|
||||
self.send_to_queue(&socket, event, "reemit");
|
||||
}
|
||||
}
|
||||
}
|
||||
/// Emits an event to all sockets in the namespace.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `event` - The event to be emitted
|
||||
fn emit(&mut self, event: &GenericEvent);
|
||||
|
||||
/// Caches an event with a specific key for later retrieval.
|
||||
///
|
||||
@@ -93,82 +44,118 @@ impl Namespace {
|
||||
/// * `cache_key` - A string key to identify the cache
|
||||
/// * `event` - The event to be cached
|
||||
/// * `buffer_fn` - A function that defines how the event should be added to the cache buffer
|
||||
#[instrument(skip_all)]
|
||||
fn cache(
|
||||
&mut self,
|
||||
event: Arc<GenericEvent>,
|
||||
buffer_fn: &Box<dyn Fn(&mut Vec<Arc<GenericEvent>>, &Arc<GenericEvent>)>,
|
||||
) {
|
||||
let cached_events_for_key = self.events.entry(event.name.clone()).or_default();
|
||||
buffer_fn(cached_events_for_key, &event);
|
||||
}
|
||||
event: &GenericEvent,
|
||||
buffer_fn: &Box<dyn Fn(&mut Vec<GenericEvent>, &GenericEvent) -> ()>,
|
||||
);
|
||||
|
||||
/// Emits an event to all sockets in the namespace and caches it.
|
||||
///
|
||||
/// This is a convenience method that combines the functionality of
|
||||
/// [`Namespace::emit`] and [`Namespace::cache`].
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `event` - The event to be emitted and cached
|
||||
/// * `cache_key` - A string key to identify the cache
|
||||
/// * `buffer_fn` - A function that defines how the event should be added to the cache buffer
|
||||
#[instrument(skip_all)]
|
||||
pub fn emit(
|
||||
fn emit_cached(
|
||||
&mut self,
|
||||
event: Arc<GenericEvent>,
|
||||
buffer_fn: &Box<dyn Fn(&mut Vec<Arc<GenericEvent>>, &Arc<GenericEvent>)>,
|
||||
) {
|
||||
// cache the event
|
||||
self.cache(event.clone(), buffer_fn);
|
||||
event: &GenericEvent,
|
||||
buffer_fn: &Box<dyn Fn(&mut Vec<GenericEvent>, &GenericEvent) -> ()>,
|
||||
);
|
||||
}
|
||||
|
||||
// emit the event - inlined from emit function
|
||||
// Send to global queue for each socket in the namespace
|
||||
for socket in self.sockets.clone() {
|
||||
self.send_to_queue(&socket, &event, "emit");
|
||||
}
|
||||
}
|
||||
#[derive(Debug)]
|
||||
pub struct Namespace {
|
||||
sockets: Vec<SocketRef>,
|
||||
events: HashMap<String, Vec<GenericEvent>>,
|
||||
socket_queues: HashMap<Sid, SocketQueue>,
|
||||
}
|
||||
|
||||
/// Sends an event to the global queue for a specific socket.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `socket` - The socket to send the event to
|
||||
/// * `event` - The event to be sent
|
||||
/// * `operation` - A description of the operation for logging purposes (e.g., "reemit", "emit")
|
||||
#[instrument(skip_all)]
|
||||
fn send_to_queue(&self, socket: &SocketRef, event: &Arc<GenericEvent>, operation: &str) {
|
||||
tracing::trace!(
|
||||
socket_id = ?socket.id,
|
||||
event = %event.name,
|
||||
operation = %operation,
|
||||
"Sending event to global queue"
|
||||
);
|
||||
match self
|
||||
.socket_queue_tx
|
||||
.try_send((socket.clone(), event.clone()))
|
||||
{
|
||||
Ok(_) => {
|
||||
tracing::trace!(
|
||||
socket_id = ?socket.id,
|
||||
event = %event.name,
|
||||
operation = %operation,
|
||||
"Successfully sent event to global queue"
|
||||
);
|
||||
}
|
||||
Err(e) => {
|
||||
tracing::error!(
|
||||
socket_id = ?socket.id,
|
||||
event = %event.name,
|
||||
operation = %operation,
|
||||
error = %e,
|
||||
"Failed to send event to global queue"
|
||||
);
|
||||
}
|
||||
impl Namespace {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
sockets: vec![],
|
||||
events: HashMap::new(),
|
||||
socket_queues: HashMap::new(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Drop for Namespace {
|
||||
fn drop(&mut self) {
|
||||
self.disconnect_all();
|
||||
impl NamespaceInterface for Namespace {
|
||||
#[instrument(skip_all)]
|
||||
fn subscribe(&mut self, socket: SocketRef) {
|
||||
// add the socket to the list
|
||||
self.sockets.push(socket.clone());
|
||||
// create a new queue for this socket
|
||||
self.socket_queues.insert(socket.id, SocketQueue::new());
|
||||
}
|
||||
|
||||
#[instrument(skip_all)]
|
||||
fn unsubscribe(&mut self, socket: SocketRef) {
|
||||
// remove the socket from the list
|
||||
self.sockets.retain(|s| s.id != socket.id);
|
||||
// remove the socket's queue
|
||||
self.socket_queues.remove(&socket.id);
|
||||
}
|
||||
|
||||
#[instrument(skip_all)]
|
||||
fn reemit(&mut self, socket: SocketRef) {
|
||||
if let Some(queue) = self.socket_queues.get(&socket.id) {
|
||||
// Collect events grouped by name/kind with their counts for sorting
|
||||
let mut event_groups: Vec<(&String, &Vec<GenericEvent>)> = self.events.iter().collect();
|
||||
|
||||
// Sort by event count (ascending - lowest count first)
|
||||
event_groups.sort_by(|a, b| a.1.len().cmp(&b.1.len()));
|
||||
|
||||
// Emit events in order of lowest count first
|
||||
for (_event_name, events) in event_groups {
|
||||
for event in events {
|
||||
queue.push(event.clone());
|
||||
}
|
||||
}
|
||||
|
||||
queue.flush(socket.clone());
|
||||
}
|
||||
}
|
||||
|
||||
#[instrument(skip_all)]
|
||||
fn emit(&mut self, event: &GenericEvent) {
|
||||
// Use the new emit function which combines push and flush
|
||||
for socket in self.sockets.clone() {
|
||||
if let Some(queue) = self.socket_queues.get(&socket.id) {
|
||||
queue.push(event.clone());
|
||||
queue.flush(socket.clone());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[instrument(skip_all)]
|
||||
fn cache(
|
||||
&mut self,
|
||||
event: &GenericEvent,
|
||||
buffer_fn: &Box<dyn Fn(&mut Vec<GenericEvent>, &GenericEvent) -> ()>,
|
||||
) {
|
||||
let mut cached_events_for_key = self
|
||||
.events
|
||||
.entry(event.name.clone())
|
||||
.or_insert_with(Vec::new);
|
||||
buffer_fn(&mut cached_events_for_key, event);
|
||||
}
|
||||
|
||||
#[instrument(skip_all)]
|
||||
fn emit_cached(
|
||||
&mut self,
|
||||
event: &GenericEvent,
|
||||
buffer_fn: &Box<dyn Fn(&mut Vec<GenericEvent>, &GenericEvent) -> ()>,
|
||||
) {
|
||||
// cache the event
|
||||
self.cache(event, buffer_fn);
|
||||
|
||||
// emit the event
|
||||
self.emit(event);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -180,17 +167,17 @@ pub trait NamespaceCacheingLogic<V>
|
||||
where
|
||||
V: CacheableEvents<V>,
|
||||
{
|
||||
fn emit(&mut self, event: V);
|
||||
fn emit_cached(&mut self, event: V);
|
||||
}
|
||||
|
||||
pub trait CacheableEvents<Events> {
|
||||
fn event_value(&self) -> GenericEvent;
|
||||
fn event_value(&self) -> Result<GenericEvent, serde_json::Error>;
|
||||
fn event_cache_fn(&self) -> CacheFn;
|
||||
}
|
||||
|
||||
pub type CacheFn = Box<dyn Fn(&mut Vec<Arc<GenericEvent>>, &Arc<GenericEvent>)>;
|
||||
pub type CacheFn = Box<dyn Fn(&mut Vec<GenericEvent>, &GenericEvent) -> ()>;
|
||||
|
||||
/// [`BufferFn`] that stores the last n events
|
||||
/// [BufferFn] that stores the last n events
|
||||
pub fn cache_n_events(n: usize) -> CacheFn {
|
||||
Box::new(move |events, event| {
|
||||
if events.len() >= n {
|
||||
@@ -200,41 +187,21 @@ pub fn cache_n_events(n: usize) -> CacheFn {
|
||||
})
|
||||
}
|
||||
|
||||
/// [`BufferFn`] that stores only one event
|
||||
/// [BufferFn] that stores only one event
|
||||
pub fn cache_one_event() -> CacheFn {
|
||||
cache_n_events(1)
|
||||
}
|
||||
|
||||
/// [`BufferFn`] that stores first and last event
|
||||
///
|
||||
/// The primary use case of this function is to cache both the default state of a machine, which should be emitted first,
|
||||
/// and the last event, which is the most recent state of the machine.
|
||||
pub fn cache_first_and_last_event() -> CacheFn {
|
||||
Box::new(move |events, event| {
|
||||
// if the events length 0 or 1, we just push the event
|
||||
if events.is_empty() || events.len() == 1 {
|
||||
events.push(event.clone());
|
||||
return;
|
||||
}
|
||||
// if the event length is 2 we remove the last event and append a new one
|
||||
if events.len() == 2 {
|
||||
events.remove(1);
|
||||
events.push(event.clone());
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
/// [`BufferFn`] that stores events for a certain duration
|
||||
/// [BufferFn] that stores events for a certain duration
|
||||
pub fn cache_duration(duration: Duration, bucket_size: Duration) -> CacheFn {
|
||||
Box::new(move |events, event| {
|
||||
// Use event.ts instead of system time
|
||||
let current_time = event.ts as u128;
|
||||
|
||||
// calculate current bucket & last bucket
|
||||
let bucket_size_ms = bucket_size.as_millis().max(1);
|
||||
let bucket = current_time / bucket_size_ms; // Avoid division by zero
|
||||
let bucket = current_time / bucket_size.as_millis();
|
||||
let last_bucket = match events.last() {
|
||||
Some(last_event) => last_event.ts as u128 / bucket_size_ms,
|
||||
Some(last_event) => last_event.ts as u128 / bucket_size.as_millis(),
|
||||
None => 0,
|
||||
};
|
||||
|
||||
@@ -264,32 +231,24 @@ pub fn cache_duration(duration: Duration, bucket_size: Duration) -> CacheFn {
|
||||
mod tests {
|
||||
use std::cmp::min;
|
||||
|
||||
use serde::Serialize;
|
||||
|
||||
use super::*;
|
||||
|
||||
#[derive(Debug, Clone, Serialize)]
|
||||
struct TestEventData {
|
||||
value: usize,
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_cache_n_events() {
|
||||
// Create a cache function that stores the last 2 events
|
||||
let cache_fn = cache_n_events(2);
|
||||
|
||||
// Create namespace with dummy queue_tx
|
||||
let (queue_tx, _queue_rx) = smol::channel::unbounded();
|
||||
let mut namespace = Namespace::new(queue_tx);
|
||||
// Create namespace
|
||||
let mut namespace = Namespace::new();
|
||||
assert!(namespace.events.is_empty());
|
||||
|
||||
// Add event
|
||||
let event1 = Arc::new(GenericEvent {
|
||||
let event1 = GenericEvent {
|
||||
name: "test_event".to_string(),
|
||||
data: Box::new(TestEventData { value: 1 }),
|
||||
data: serde_json::json!({"value": 1}),
|
||||
ts: 0,
|
||||
});
|
||||
namespace.cache(event1, &cache_fn);
|
||||
};
|
||||
namespace.cache(&event1, &cache_fn);
|
||||
|
||||
// Check that we have one event name in the map
|
||||
assert_eq!(namespace.events.len(), 1);
|
||||
@@ -298,12 +257,12 @@ mod tests {
|
||||
assert_eq!(namespace.events.get("test_event").unwrap()[0].ts, 0);
|
||||
|
||||
// Add another event
|
||||
let event2 = Arc::new(GenericEvent {
|
||||
let event2 = GenericEvent {
|
||||
name: "test_event".to_string(),
|
||||
data: Box::new(TestEventData { value: 2 }),
|
||||
data: serde_json::json!({"value": 2}),
|
||||
ts: 1,
|
||||
});
|
||||
namespace.cache(event2, &cache_fn);
|
||||
};
|
||||
namespace.cache(&event2, &cache_fn);
|
||||
|
||||
// Still one event name
|
||||
assert_eq!(namespace.events.len(), 1);
|
||||
@@ -313,12 +272,12 @@ mod tests {
|
||||
assert_eq!(namespace.events.get("test_event").unwrap()[1].ts, 1);
|
||||
|
||||
// Add a third event, which should remove the first one
|
||||
let event3 = Arc::new(GenericEvent {
|
||||
let event3 = GenericEvent {
|
||||
name: "test_event".to_string(),
|
||||
data: Box::new(TestEventData { value: 3 }),
|
||||
data: serde_json::json!({"value": 3}),
|
||||
ts: 2,
|
||||
});
|
||||
namespace.cache(event3, &cache_fn);
|
||||
};
|
||||
namespace.cache(&event3, &cache_fn);
|
||||
|
||||
// Still one event name
|
||||
assert_eq!(namespace.events.len(), 1);
|
||||
@@ -337,20 +296,19 @@ mod tests {
|
||||
let duration = Duration::new(10, 0);
|
||||
let bucket_size = Duration::new(1, 0);
|
||||
let cache_fn = cache_duration(duration, bucket_size);
|
||||
let (queue_tx, _queue_rx) = smol::channel::unbounded();
|
||||
|
||||
// Create namespace
|
||||
let mut namespace = Namespace::new(queue_tx);
|
||||
let mut namespace = Namespace::new();
|
||||
assert!(namespace.events.is_empty());
|
||||
|
||||
// Add events every 100ms for 20 seconds
|
||||
for i in 0..200 {
|
||||
let event = Arc::new(GenericEvent {
|
||||
let event = GenericEvent {
|
||||
name: "test_event".to_string(),
|
||||
data: Box::new(TestEventData { value: i }),
|
||||
data: serde_json::json!({"value": i}),
|
||||
ts: (i * 100) as u64,
|
||||
});
|
||||
namespace.cache(event, &cache_fn);
|
||||
};
|
||||
namespace.cache(&event, &cache_fn);
|
||||
|
||||
let should_have_events = min(i / 10, 11);
|
||||
assert_eq!(
|
||||
|
||||
@@ -17,8 +17,8 @@ impl Serialize for NamespaceId {
|
||||
S: Serializer,
|
||||
{
|
||||
match self {
|
||||
Self::Main => serializer.serialize_str("/main"),
|
||||
Self::Machine(id) => {
|
||||
NamespaceId::Main => serializer.serialize_str("/main"),
|
||||
NamespaceId::Machine(id) => {
|
||||
let path = format!(
|
||||
"/machine/{}/{}/{}",
|
||||
id.machine_identification.vendor, id.machine_identification.machine, id.serial
|
||||
@@ -36,7 +36,7 @@ impl<'de> Deserialize<'de> for NamespaceId {
|
||||
{
|
||||
struct NamespaceIdVisitor;
|
||||
|
||||
impl Visitor<'_> for NamespaceIdVisitor {
|
||||
impl<'de> Visitor<'de> for NamespaceIdVisitor {
|
||||
type Value = NamespaceId;
|
||||
|
||||
fn expecting(&self, formatter: &mut fmt::Formatter) -> fmt::Result {
|
||||
@@ -85,7 +85,7 @@ impl FromStr for NamespaceId {
|
||||
|
||||
fn from_str(s: &str) -> Result<Self, Self::Err> {
|
||||
if s == "/main" {
|
||||
return Ok(Self::Main);
|
||||
return Ok(NamespaceId::Main);
|
||||
}
|
||||
|
||||
if let Some(machine_path) = s.strip_prefix("/machine/") {
|
||||
@@ -101,7 +101,7 @@ impl FromStr for NamespaceId {
|
||||
.parse::<u16>()
|
||||
.map_err(|_| "Invalid serial id".to_string())?;
|
||||
|
||||
return Ok(Self::Machine(MachineIdentificationUnique {
|
||||
return Ok(NamespaceId::Machine(MachineIdentificationUnique {
|
||||
machine_identification: MachineIdentification { vendor, machine },
|
||||
serial,
|
||||
}));
|
||||
@@ -116,8 +116,8 @@ impl FromStr for NamespaceId {
|
||||
impl fmt::Display for NamespaceId {
|
||||
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
|
||||
match self {
|
||||
Self::Main => write!(f, "/main"),
|
||||
Self::Machine(id) => {
|
||||
NamespaceId::Main => write!(f, "/main"),
|
||||
NamespaceId::Machine(id) => {
|
||||
write!(
|
||||
f,
|
||||
"/machine/{}/{}/{}",
|
||||
|
||||
@@ -0,0 +1,120 @@
|
||||
use std::{
|
||||
collections::VecDeque,
|
||||
sync::{
|
||||
Arc, Mutex,
|
||||
atomic::{AtomicBool, Ordering},
|
||||
},
|
||||
time::Duration,
|
||||
};
|
||||
|
||||
use socketioxide::extract::SocketRef;
|
||||
use tracing::{debug_span, instrument};
|
||||
|
||||
use super::event::GenericEvent;
|
||||
|
||||
/// A queue for managing events for a specific socket
|
||||
#[derive(Debug)]
|
||||
pub struct SocketQueue {
|
||||
queue: Arc<Mutex<VecDeque<GenericEvent>>>,
|
||||
is_flushing: Arc<AtomicBool>,
|
||||
}
|
||||
|
||||
impl SocketQueue {
|
||||
/// Create a new socket queue for the given socket ID
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
queue: Arc::new(Mutex::new(VecDeque::new())),
|
||||
is_flushing: Arc::new(AtomicBool::new(false)),
|
||||
}
|
||||
}
|
||||
|
||||
/// Add an event to the queue
|
||||
#[instrument(skip_all)]
|
||||
pub fn push(&self, event: GenericEvent) {
|
||||
if let Ok(mut queue) = self.queue.lock() {
|
||||
queue.push_back(event);
|
||||
}
|
||||
}
|
||||
|
||||
/// Force flush events asynchronously (assumes flushing flag is already set)
|
||||
#[instrument(skip_all)]
|
||||
pub fn flush(&self, socket: SocketRef) {
|
||||
// Check if we're already flushing
|
||||
if self
|
||||
.is_flushing
|
||||
.compare_exchange(false, true, Ordering::SeqCst, Ordering::SeqCst)
|
||||
.is_err()
|
||||
{
|
||||
// Already flushing, return early
|
||||
return;
|
||||
}
|
||||
|
||||
let queue = Arc::clone(&self.queue);
|
||||
let is_flushing = Arc::clone(&self.is_flushing);
|
||||
|
||||
// Spawn a smol task to flush the queue
|
||||
smol::spawn(async move {
|
||||
let span = debug_span!("socket_queue_flush", socket_id = ?socket.id);
|
||||
let _enter = span.enter();
|
||||
|
||||
// Process events directly from the queue without collecting first
|
||||
loop {
|
||||
let event = {
|
||||
if let Ok(mut queue_guard) = queue.lock() {
|
||||
queue_guard.pop_front()
|
||||
} else {
|
||||
break; // Exit if we can't lock the queue
|
||||
}
|
||||
};
|
||||
|
||||
let Some(event) = event else {
|
||||
break; // No more events in queue
|
||||
};
|
||||
|
||||
// retry loop for each event
|
||||
loop {
|
||||
// check if socket is still connected
|
||||
if !socket.connected() {
|
||||
break; // Exit the loop if the socket is not connected
|
||||
}
|
||||
|
||||
let span = debug_span!(
|
||||
"socket_queue_flush_event",
|
||||
socket_id = ?socket.id,
|
||||
event_name = %event.name,
|
||||
);
|
||||
let _enter = span.enter();
|
||||
|
||||
match socket.emit("event", &event) {
|
||||
Ok(_) => break, // Successfully emitted, exit loop
|
||||
Err(e) => match e {
|
||||
socketioxide::SendError::Serialize(_) => {
|
||||
// no reason in retrying serialization errors
|
||||
break;
|
||||
}
|
||||
socketioxide::SendError::Socket(socket_error) => match socket_error {
|
||||
socketioxide::SocketError::InternalChannelFull => {
|
||||
// wait 10ms before retrying
|
||||
tracing::warn!(
|
||||
"Socket {} internal channel full, retrying in 10ms",
|
||||
socket.id,
|
||||
);
|
||||
smol::Timer::after(Duration::from_millis(10)).await;
|
||||
continue; // Retry sending the event
|
||||
}
|
||||
socketioxide::SocketError::Closed => {
|
||||
// Socket is closed, no point in retrying
|
||||
break;
|
||||
}
|
||||
},
|
||||
},
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Reset the flushing flag
|
||||
is_flushing.store(false, Ordering::SeqCst);
|
||||
})
|
||||
.detach();
|
||||
}
|
||||
}
|
||||
@@ -1,27 +0,0 @@
|
||||
use crate::transmission::Transmission;
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct FixedTransmission {
|
||||
ratio: f64,
|
||||
}
|
||||
|
||||
impl Transmission for FixedTransmission {
|
||||
fn get_ratio(&self) -> f64 {
|
||||
self.ratio
|
||||
}
|
||||
}
|
||||
|
||||
impl FixedTransmission {
|
||||
/// Creates a new FixedTransmission with the specified ratio.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `ratio` - The transmission ratio (output over input).
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// A new instance of FixedTransmission.
|
||||
pub const fn new(ratio: f64) -> Self {
|
||||
Self { ratio }
|
||||
}
|
||||
}
|
||||
@@ -1,95 +0,0 @@
|
||||
use uom::si::{
|
||||
angle::radian,
|
||||
angular_velocity::radian_per_second,
|
||||
f64::{Angle, AngularVelocity, Length, Velocity},
|
||||
length::meter,
|
||||
velocity::meter_per_second,
|
||||
};
|
||||
|
||||
pub mod fixed;
|
||||
|
||||
/// A trait representing a mechanical transmission system that converts input to output
|
||||
/// with a specific gear ratio.
|
||||
///
|
||||
/// This trait provides methods for working with transmission ratios and converting
|
||||
/// between input and output values based on the transmission's gear ratio.
|
||||
pub trait Transmission {
|
||||
/// Returns the transmission ratio of this transmission system.
|
||||
///
|
||||
/// The ratio represents the relationship between input and output speeds/torques.
|
||||
/// A ratio greater than 1.0 indicates speed reduction (torque multiplication),
|
||||
/// while a ratio less than 1.0 indicates speed increase (torque reduction).
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The transmission ratio as a floating-point number.
|
||||
fn get_ratio(&self) -> f64;
|
||||
|
||||
/// Converts an input value to the corresponding output value using the transmission ratio.
|
||||
///
|
||||
/// This method multiplies the input by the transmission ratio to calculate the output.
|
||||
/// Typically used for converting input speed to output speed in a transmission system.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `input` - The input value to be converted
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The output value after applying the transmission ratio.
|
||||
fn calculate_output(&self, input: f64) -> f64 {
|
||||
input * self.get_ratio()
|
||||
}
|
||||
|
||||
/// Converts an output value to the corresponding input value using the transmission ratio.
|
||||
///
|
||||
/// This method divides the output by the transmission ratio to calculate the required input.
|
||||
/// Typically used for determining what input is needed to achieve a desired output.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `output` - The output value to be converted back to input
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// The input value required to produce the given output.
|
||||
fn calculate_input(&self, output: f64) -> f64 {
|
||||
output / self.get_ratio()
|
||||
}
|
||||
|
||||
fn calculate_angular_output(&self, input: Angle) -> Angle {
|
||||
Angle::new::<radian>(self.calculate_output(input.get::<radian>()))
|
||||
}
|
||||
|
||||
fn calculate_angular_input(&self, output: Angle) -> Angle {
|
||||
Angle::new::<radian>(self.calculate_input(output.get::<radian>()))
|
||||
}
|
||||
|
||||
fn calculate_angular_velocity_output(&self, input: AngularVelocity) -> AngularVelocity {
|
||||
AngularVelocity::new::<radian_per_second>(
|
||||
self.calculate_output(input.get::<radian_per_second>()),
|
||||
)
|
||||
}
|
||||
|
||||
fn calculate_angular_velocity_input(&self, output: AngularVelocity) -> AngularVelocity {
|
||||
AngularVelocity::new::<radian_per_second>(
|
||||
self.calculate_input(output.get::<radian_per_second>()),
|
||||
)
|
||||
}
|
||||
|
||||
fn calculate_linear_output(&self, input: Length) -> Length {
|
||||
Length::new::<meter>(self.calculate_output(input.get::<meter>()))
|
||||
}
|
||||
|
||||
fn calculate_linear_input(&self, output: Length) -> Length {
|
||||
Length::new::<meter>(self.calculate_input(output.get::<meter>()))
|
||||
}
|
||||
|
||||
fn calculate_linear_velocity_output(&self, input: Velocity) -> Velocity {
|
||||
Velocity::new::<meter_per_second>(self.calculate_output(input.get::<meter_per_second>()))
|
||||
}
|
||||
|
||||
fn calculate_linear_velocity_input(&self, output: Velocity) -> Velocity {
|
||||
Velocity::new::<meter_per_second>(self.calculate_input(output.get::<meter_per_second>()))
|
||||
}
|
||||
}
|
||||
@@ -1,208 +0,0 @@
|
||||
use anyhow::Error;
|
||||
use std::collections::HashMap;
|
||||
use tokio::sync::broadcast;
|
||||
|
||||
/// Video stream manager that handles multiple video streams per machine with true broadcasting
|
||||
/// Uses tokio broadcast channels to ensure all subscribers receive all messages without stealing
|
||||
#[derive(Debug)]
|
||||
pub struct VideoStreamManager {
|
||||
/// Each stream has its own broadcast transmitter and template receiver
|
||||
streams: HashMap<String, (broadcast::Sender<Vec<u8>>, broadcast::Receiver<Vec<u8>>)>,
|
||||
}
|
||||
|
||||
impl VideoStreamManager {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
streams: HashMap::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// Send a frame to all subscribers of a stream
|
||||
/// Returns Ok if the frame was sent, Err if the stream doesn't exist
|
||||
pub fn send_frame(&self, stream_id: &str, frame: Vec<u8>) -> Result<(), Error> {
|
||||
if let Some((tx, _rx)) = self.streams.get(stream_id) {
|
||||
match tx.send(frame) {
|
||||
Ok(_) => Ok(()),
|
||||
Err(_) => Ok(()), // No receivers, but that's not an error
|
||||
}
|
||||
} else {
|
||||
Err(anyhow::anyhow!("Stream '{}' not found", stream_id))
|
||||
}
|
||||
}
|
||||
|
||||
/// Register a new video stream with the given ID
|
||||
pub fn add_stream(&mut self, stream_id: String) {
|
||||
let (tx, rx) = broadcast::channel::<Vec<u8>>(256); // 256 frame buffer capacity
|
||||
self.streams.insert(stream_id, (tx, rx));
|
||||
}
|
||||
|
||||
/// Get list of available stream IDs
|
||||
pub fn get_streams(&self) -> Vec<String> {
|
||||
self.streams.keys().cloned().collect()
|
||||
}
|
||||
|
||||
/// Check if a stream exists
|
||||
pub fn has_stream(&self, stream_id: &str) -> bool {
|
||||
self.streams.contains_key(stream_id)
|
||||
}
|
||||
|
||||
/// Remove a stream and all its subscribers
|
||||
pub fn remove_stream(&mut self, stream_id: &str) -> bool {
|
||||
self.streams.remove(stream_id).is_some()
|
||||
}
|
||||
|
||||
/// Subscribe to a video stream and get a receiver
|
||||
/// Returns the receiver for the new subscription
|
||||
pub fn subscribe_to_stream(&self, stream_id: &str) -> Option<broadcast::Receiver<Vec<u8>>> {
|
||||
self.streams
|
||||
.get(stream_id)
|
||||
.map(|(_tx, rx)| rx.resubscribe()) // Resubscribe to create a new independent receiver
|
||||
}
|
||||
|
||||
/// Get subscriber count for a stream
|
||||
pub fn get_subscriber_count(&self, stream_id: &str) -> usize {
|
||||
self.streams
|
||||
.get(stream_id)
|
||||
.map_or(0, |(tx, _rx)| tx.receiver_count())
|
||||
}
|
||||
|
||||
/// Get a stream transmitter for sending frames (used by machines to send video)
|
||||
/// Returns None if the stream doesn't exist
|
||||
pub fn get_sender(&self, stream_id: &str) -> Option<broadcast::Sender<Vec<u8>>> {
|
||||
self.streams.get(stream_id).map(|(tx, _rx)| tx.clone())
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for VideoStreamManager {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[tokio::test]
|
||||
async fn test_video_stream_manager() {
|
||||
let mut manager = VideoStreamManager::new();
|
||||
|
||||
// Register streams
|
||||
manager.register_stream("camera1".to_string());
|
||||
manager.register_stream("camera2".to_string());
|
||||
|
||||
assert_eq!(manager.get_stream_ids().len(), 2);
|
||||
assert!(manager.has_stream("camera1"));
|
||||
assert!(manager.has_stream("camera2"));
|
||||
assert!(!manager.has_stream("camera3"));
|
||||
|
||||
// Subscribe to streams
|
||||
let mut rx1 = manager.subscribe_to_stream("camera1").unwrap();
|
||||
let mut rx2 = manager.subscribe_to_stream("camera1").unwrap();
|
||||
let mut rx3 = manager.subscribe_to_stream("camera2").unwrap();
|
||||
|
||||
assert_eq!(manager.get_subscriber_count("camera1"), 3); // Original + 2 clones
|
||||
assert_eq!(manager.get_subscriber_count("camera2"), 2); // Original + 1 clone
|
||||
assert_eq!(manager.get_subscriber_count("nonexistent"), 0);
|
||||
|
||||
// Send frames
|
||||
let frame1 = vec![1, 2, 3, 4];
|
||||
let frame2 = vec![5, 6, 7, 8];
|
||||
|
||||
assert!(manager.send_single_frame("camera1", frame1.clone()));
|
||||
assert!(manager.send_single_frame("camera2", frame2.clone()));
|
||||
assert!(!manager.send_single_frame("nonexistent", vec![]));
|
||||
|
||||
// Both camera1 subscribers should receive the frame
|
||||
assert_eq!(rx1.recv().await.unwrap(), frame1);
|
||||
assert_eq!(rx2.recv().await.unwrap(), frame1);
|
||||
|
||||
// Camera2 subscriber should receive its frame
|
||||
assert_eq!(rx3.recv().await.unwrap(), frame2);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn test_video_frame_broadcasting() {
|
||||
let mut manager = VideoStreamManager::new();
|
||||
manager.register_stream("main_camera".to_string());
|
||||
|
||||
// Multiple clients subscribe to the same camera
|
||||
let mut rx1 = manager.subscribe_to_stream("main_camera").unwrap();
|
||||
let mut rx2 = manager.subscribe_to_stream("main_camera").unwrap();
|
||||
let mut rx3 = manager.subscribe_to_stream("main_camera").unwrap();
|
||||
|
||||
// Simulate video frames
|
||||
let frames = vec![
|
||||
vec![0xFF, 0xD8, 0xFF, 0xE0], // JPEG header
|
||||
vec![0x00, 0x01, 0x02, 0x03], // Frame 1
|
||||
vec![0x04, 0x05, 0x06, 0x07], // Frame 2
|
||||
];
|
||||
|
||||
for frame in &frames {
|
||||
assert!(manager.send_single_frame("main_camera", frame.clone()));
|
||||
}
|
||||
|
||||
// All clients should receive all frames in order
|
||||
for frame in &frames {
|
||||
assert_eq!(rx1.recv().await.unwrap(), *frame);
|
||||
assert_eq!(rx2.recv().await.unwrap(), *frame);
|
||||
assert_eq!(rx3.recv().await.unwrap(), *frame);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_stream_management() {
|
||||
let mut manager = VideoStreamManager::new();
|
||||
|
||||
// Initially no streams
|
||||
assert_eq!(manager.get_stream_ids().len(), 0);
|
||||
|
||||
// Add streams
|
||||
manager.register_stream("front_camera".to_string());
|
||||
manager.register_stream("rear_camera".to_string());
|
||||
manager.register_stream("side_camera".to_string());
|
||||
|
||||
let mut stream_ids = manager.get_stream_ids();
|
||||
stream_ids.sort(); // Sort for consistent testing
|
||||
assert_eq!(
|
||||
stream_ids,
|
||||
vec!["front_camera", "rear_camera", "side_camera"]
|
||||
);
|
||||
|
||||
// Remove a stream
|
||||
assert!(manager.remove_stream("rear_camera"));
|
||||
assert!(!manager.remove_stream("rear_camera")); // Already removed
|
||||
assert!(!manager.has_stream("rear_camera"));
|
||||
|
||||
assert_eq!(manager.get_stream_ids().len(), 2);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn test_send_frame_results() {
|
||||
let mut manager = VideoStreamManager::new();
|
||||
manager.register_stream("test_camera".to_string());
|
||||
|
||||
// One initial receiver (the original one created in register_stream)
|
||||
let result = manager.send_frame("test_camera", vec![1, 2, 3]).unwrap();
|
||||
assert_eq!(result, 1); // Number of subscribers
|
||||
|
||||
// Add subscribers by cloning the receiver
|
||||
let _rx1 = manager.subscribe_to_stream("test_camera").unwrap();
|
||||
let _rx2 = manager.subscribe_to_stream("test_camera").unwrap();
|
||||
|
||||
// Send frame to multiple subscribers (original + 2 clones = 3 total)
|
||||
let result = manager.send_frame("test_camera", vec![4, 5, 6]).unwrap();
|
||||
assert_eq!(result, 3); // Number of subscribers
|
||||
|
||||
// Try sending to non-existent stream
|
||||
let result = manager.send_frame("nonexistent", vec![7, 8, 9]);
|
||||
assert!(result.is_err());
|
||||
assert_eq!(result.unwrap_err(), "Stream 'nonexistent' not found");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn test_default_video_stream_manager() {
|
||||
let manager = VideoStreamManager::default();
|
||||
assert_eq!(manager.get_stream_ids().len(), 0);
|
||||
}
|
||||
}
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 7.2 KiB |
@@ -1,145 +0,0 @@
|
||||
# How can I add a new Machine?
|
||||
|
||||
Adding a new machine follows a common structure in `server/machines/`.
|
||||
|
||||
|
||||
|
||||
# TLDR
|
||||
|
||||
Summary
|
||||
|
||||
When adding a new machine:
|
||||
|
||||
- Create Folder server/machines/YOUR_MACHINE_NAME/.
|
||||
- Add the 4 files (mod.rs, new.rs, act.rs, api.rs).
|
||||
- Register it in machines/mod.rs with a unique constant.
|
||||
- mod.rs: define the struct + emit functions.
|
||||
- new.rs: assemble devices/controllers into the machine.
|
||||
- act.rs: implement the machine loop (MachineAct).
|
||||
- api.rs: define request (mutations) and response (events) types.
|
||||
|
||||
Tip: Start simple (copy MockMachine) and grow complexity as needed (add EtherCAT devices, controllers, etc.).
|
||||
|
||||
---
|
||||
|
||||
## 1. Create the folder
|
||||
|
||||
Create a new directory:
|
||||
server/machines/YOUR_MACHINE_NAME
|
||||
|
||||
At a minimum, this folder needs the following files:
|
||||
|
||||
- `mod.rs`
|
||||
- `new.rs`
|
||||
- `act.rs`
|
||||
- `api.rs`
|
||||
|
||||
---
|
||||
|
||||
## 2. Register the module
|
||||
|
||||
Add your machine to `server/machines/mod.rs`:
|
||||
|
||||
```rust
|
||||
// mod.rs
|
||||
pub mod buffer1;
|
||||
pub mod extruder1;
|
||||
pub mod laser;
|
||||
pub mod mock;
|
||||
pub mod registry;
|
||||
pub mod winder2;
|
||||
pub mod YOUR_MACHINE;
|
||||
|
||||
pub const VENDOR_QITECH: u16 = 0x0001;
|
||||
pub const MACHINE_WINDER_V1: u16 = 0x0002;
|
||||
pub const MACHINE_EXTRUDER_V1: u16 = 0x0004;
|
||||
pub const MACHINE_LASER_V1: u16 = 0x0006;
|
||||
pub const MACHINE_MOCK: u16 = 0x0007;
|
||||
pub const MACHINE_BUFFER_V1: u16 = 0x0008;
|
||||
pub const YOUR_MACHINE_V1: u16 = 0xffff; // pick a unique ID to avoid collisions also DO NOT touch any already defined ids!!
|
||||
```
|
||||
|
||||
## 3. mod.rs
|
||||
|
||||
This file defines your machine struct and the required event emitters.
|
||||
|
||||
Look at MockMachine’s mod.rs for the simplest possible version.
|
||||
|
||||
At a minimum, you need:
|
||||
- emit_state
|
||||
- emit_live_values
|
||||
- A machine struct for state(e.g. MockMachine, Extruder, Laser, Winder, …)
|
||||
|
||||
More complex machines (e.g. Extruder, Winder) encapsulate controllers that in turn manage devices like EtherCAT terminals.
|
||||
|
||||
## 4 new.rs
|
||||
|
||||
This file implements MachineNewTrait for your machine.
|
||||
Here you assemble your machine from its parts — devices, controllers, and any state.
|
||||
|
||||
Minimal example: see MockMachine → basically no devices, just returns a struct.
|
||||
|
||||
Medium complexity: see Laser → wraps a single serial USB device.
|
||||
|
||||
High complexity: see Extruder → configures multiple EtherCAT devices inside controllers.
|
||||
|
||||
Think of new.rs as the factory:
|
||||
This is the place to declare which devices make up your machine and how they’re configured.
|
||||
|
||||
## 5. act.rs
|
||||
|
||||
This file implements the machine control loop using MachineAct.
|
||||
The act() method is called repeatedly by the control runtime.
|
||||
|
||||
```rs
|
||||
impl MachineAct for YourMachine {
|
||||
fn act(&mut self, _now_ts: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
let now = Instant::now();
|
||||
|
||||
// Update state, e.g. step a controller
|
||||
self.some_controller.update();
|
||||
|
||||
// Emit live values ~30 Hz if machine is in Running mode
|
||||
if now.duration_since(self.last_measurement_emit) > Duration::from_secs_f64(1.0 / 30.0) {
|
||||
self.maybe_emit_state_event();
|
||||
self.emit_live_values();
|
||||
self.last_measurement_emit = now;
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## 6. api.rs
|
||||
|
||||
Defines the frontend-facing API for your machine:
|
||||
|
||||
Mutations → requests that the machine accepts
|
||||
(e.g. start/stop commands, setpoints, configuration updates).
|
||||
|
||||
Events → responses emitted to the frontend
|
||||
(e.g. live sensor data, alarms, machine state changes).
|
||||
|
||||
Look at MockMachine’s api.rs for a minimal version.
|
||||
|
||||
```rs
|
||||
/// Mutations are API requests to control the machine
|
||||
#[derive(Debug, Clone, serde::Deserialize)]
|
||||
#[serde(tag = "type", rename_all = "snake_case")]
|
||||
pub enum YourMachineMutation {
|
||||
Start,
|
||||
Stop,
|
||||
SetSpeed { rpm: f64 },
|
||||
}
|
||||
|
||||
/// Events are responses sent to the frontend
|
||||
#[derive(Debug, Clone, serde::Serialize)]
|
||||
#[serde(tag = "type", rename_all = "snake_case")]
|
||||
pub enum YourMachineEvent {
|
||||
StateChanged { state: String },
|
||||
LiveValues { rpm: f64, torque: f64 },
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
@@ -1,94 +0,0 @@
|
||||
# Code Style
|
||||
|
||||
# General Code styling
|
||||
|
||||
Try to adhere to the rust style guide:
|
||||
https://doc.rust-lang.org/nightly/style-guide/
|
||||
|
||||
# Beckhoff terminals and Machines
|
||||
|
||||
For Beckhoff terminals you should first implement the basic functionality, then an Interpreter of the device/data is built on top.
|
||||
A good example of how to do this is by looking at the following code:
|
||||
el3021.rs , AnalogInputDevice
|
||||
|
||||
El3021 implements a so called AnalogInputDevice, which acts like an interface.
|
||||
This allows us to use AnalogInputDevice instead of having to always specify which device should be used.
|
||||
In Essence any Device that implements AnalogInputDevice can be used.
|
||||
|
||||
AnalogInputDevice is then used for creating an AnalogInput, which is plug and play for machines.
|
||||
|
||||
Then a further layer of Abstraction is used on top,
|
||||
where the data is interpreted and incorporated into a sort of control loop for certain functions, for example:
|
||||
|
||||
```rust
|
||||
// ScrewSpeedController controlls the speed (frequency) and direction of the motor
|
||||
// AnalogInput is used to collect measurements from the pressure sensor.
|
||||
// screw_speed_controller.rs
|
||||
#[derive(Debug)]
|
||||
pub struct ScrewSpeedController {
|
||||
pub pid: ClampingTimeagnosticPidController,
|
||||
pub target_pressure: Pressure,
|
||||
pub target_rpm: AngularVelocity,
|
||||
pub inverter: MitsubishiCS80,
|
||||
pressure_sensor: AnalogInput,
|
||||
last_update: Instant,
|
||||
uses_rpm: bool,
|
||||
forward_rotation: bool,
|
||||
transmission_converter: TransmissionConverter,
|
||||
frequency: Frequency,
|
||||
maximum_frequency: Frequency,
|
||||
minimum_frequency: Frequency,
|
||||
motor_on: bool,
|
||||
nozzle_pressure_limit: Pressure,
|
||||
nozzle_pressure_limit_enabled: bool,
|
||||
}
|
||||
|
||||
// Under the hood for ExtruderV2 a El3021 is used:
|
||||
// (from new.rs of extruder)
|
||||
let pressure_sensor = AnalogInput::new(el3021, EL3021Port::AI1);
|
||||
|
||||
// But the extruder does not directly use the AnalogInput, instead the logic is decoupled in the screw_speed_controller:
|
||||
let mut extruder: ExtruderV2 = Self {
|
||||
namespace: ExtruderV2Namespace::new(params.socket_queue_tx.clone()),
|
||||
last_measurement_emit: Instant::now(),
|
||||
mode: ExtruderV2Mode::Standby,
|
||||
temperature_controller_front: temperature_controller_front,
|
||||
temperature_controller_middle: temperature_controller_middle,
|
||||
temperature_controller_back: temperature_controller_back,
|
||||
temperature_controller_nozzle: temperature_controller_nozzle,
|
||||
screw_speed_controller: screw_speed_controller,
|
||||
emitted_default_state: false,
|
||||
};
|
||||
|
||||
// Then in act.rs
|
||||
// The extruder simply calls the update function of the screw_speed_controller,
|
||||
// which contains "all" of the logic needed to properly drive the motor
|
||||
self.screw_speed_controller.update(now_ts, true);
|
||||
|
||||
|
||||
```
|
||||
|
||||
This is desired behaviour for all implemented devices, the machines should stay as modular as possible.
|
||||
|
||||
# Representing physical Units (Volt,Ampere,Hz,RPM etc)
|
||||
|
||||
To represent physical units like voltage ampere etc... we use uom (units of measurement), which acts as a wrapper for units and supplies all conversions between certain units.
|
||||
For example:
|
||||
|
||||
```rust
|
||||
// Lets say a device measures voltage, you just got the raw value
|
||||
let volts : ElectricPotential = ElectricPotential::new::<volt>(raw_value_here);
|
||||
// Then you can do this to get milli_volts
|
||||
// In api responses we always use the RAW float value (64bit):
|
||||
let milli_volts : f64 = volts.get::<millivolt>();
|
||||
```
|
||||
|
||||
# General Advice
|
||||
|
||||
- Try to avoid duplicate code (DRY principle)
|
||||
- Try to avoid lifetimes if at all possible
|
||||
- Avoid excessive abstractions
|
||||
- Avoid async code unless it is required
|
||||
- Avoid taking Ownership of values, instead borrow the value
|
||||
- When implementing a Trait for a struct, do it in the same file as the struct definition
|
||||
- Split up large impl blocks into smaller impl blocks
|
||||
@@ -1,84 +0,0 @@
|
||||
# Environment Setup
|
||||
|
||||
Working on this repository requires you to use cargo and npm. In the following document the installation process will be shown
|
||||
|
||||
## Ubuntu/Debian Installation
|
||||
|
||||
```bash
|
||||
# Just press enter when prompted
|
||||
curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh
|
||||
apt install npm
|
||||
apt install nodejs
|
||||
# assuming you dont have git already, Git setup not included in here
|
||||
apt install git
|
||||
git clone git@github.com:qitechgmbh/control.git
|
||||
cd control
|
||||
cd electron
|
||||
npm install
|
||||
```
|
||||
|
||||
# Running Backend and Frontend Linux
|
||||
|
||||
## Backend
|
||||
|
||||
To Compile backend code and run it on Linux:
|
||||
|
||||
```bash
|
||||
./cargo_run_linux.sh
|
||||
```
|
||||
|
||||
The script sets capabilities on the compiled binary like raw socket access.
|
||||
Here it is required that you are connected to atleast one machine that is communicated with over ethercat or usb/serial.
|
||||
The Most minimal working setup would be to connect over ethernet to an ek1100 beckhoff terminal.
|
||||
|
||||
## Mock-Machine
|
||||
|
||||
The Mock-Machine can be used to test code that does not require an actual machine connection.
|
||||
Like Frontend Code for example.
|
||||
|
||||
Mock machines can be used if the compile feature mock-machine is enabled.
|
||||
|
||||
```bash
|
||||
# in root git folder
|
||||
./cargo_run_linux.sh mock-machine
|
||||
```
|
||||
|
||||
## Frontend
|
||||
|
||||
To run the Frontend Code:
|
||||
|
||||
```bash
|
||||
# need to be in the electron folder and an additional terminal
|
||||
cd electron
|
||||
npm run start
|
||||
```
|
||||
|
||||
# Contributing
|
||||
|
||||
Generally you contribute to the codebase by:
|
||||
|
||||
1. Opening or Choosing an existing issue (Bug,Feature,Task etc)
|
||||
2. Work on it locally
|
||||
3. Commit your changes locally
|
||||
4. Before pushing changes, if there were changes on the master branch, rebase your branch like so:
|
||||
|
||||
```bash
|
||||
git fetch
|
||||
git rebase origin/master
|
||||
```
|
||||
|
||||
5. Push changes
|
||||
|
||||
```bash
|
||||
# IF you needed to rebase
|
||||
git push --force-with-lease
|
||||
# ELSE you just push
|
||||
git push
|
||||
```
|
||||
|
||||
6. Open a Pull Request on Github and link the issue by writing fix #issue_number, after the pull request is merged into master the branch is closed automatically
|
||||
7. Request a Review and hope for the best :)
|
||||
|
||||
# Recommended Editor Setup
|
||||
|
||||
We recommend you to use an Editor with rust-analyzer support like VSCode to speed up development and detect errors before compiling.
|
||||
@@ -1,128 +0,0 @@
|
||||
# Performance/Stability Tweaks
|
||||
## EtherCrab Config
|
||||
We set ethercrab to default settings for everything except for RetryBehaviour, so that one unlikely failure doesnt destroy the EtherCAT network.
|
||||
|
||||
```rust
|
||||
let maindevice = MainDevice::new(
|
||||
pdu,
|
||||
Timeouts {
|
||||
// Default 5000ms
|
||||
state_transition: Duration::from_millis(5000),
|
||||
// Default 30_000us
|
||||
pdu: Duration::from_micros(30_000),
|
||||
// Default 10ms
|
||||
eeprom: Duration::from_millis(10),
|
||||
// Default 0ms
|
||||
wait_loop_delay: Duration::from_millis(0),
|
||||
// Default 100ms
|
||||
mailbox_echo: Duration::from_millis(100),
|
||||
// Default 1000ms
|
||||
mailbox_response: Duration::from_millis(1000),
|
||||
},
|
||||
MainDeviceConfig {
|
||||
// Default RetryBehaviour::None
|
||||
retry_behaviour: RetryBehaviour::Count(5),
|
||||
// Default 10_000
|
||||
dc_static_sync_iterations: 10_000,
|
||||
},
|
||||
);
|
||||
|
||||
```
|
||||
wait_loop_delay CAN in theory also decrease CPU usage if set exactly as the loop_delay we set, however this setting has caused us some issues, so we just leave it as is.
|
||||
|
||||
## Realtime Kernel
|
||||
In our nixOs Config we use the latest realtime kernel and isolate TWO cores out of the Four available for the machines we use.
|
||||
|
||||
```nix
|
||||
#From configuration.nix
|
||||
boot.loader.efi.canTouchEfiVariables = true;
|
||||
boot.kernelPackages = pkgs.linuxPackages-rt_latest;
|
||||
boot.kernelModules = [ "i915" ];
|
||||
```
|
||||
|
||||
Notice how we hardcode the intel gpu kernel module. This is because only the GPU driver gets blacklisted for some reason. By specifying it manually it loads like before.
|
||||
|
||||
One Core is used for the EtherCrab TXRX Thread and one is used for the machine Thread.
|
||||
```
|
||||
"isolcpus=2,3" # Isolate cpus 2 and 3 from scheduler for better latency, 2 runs ethercatthread and 3 runs server control-loop
|
||||
"nohz_full=2,3" # In this mode, the periodic scheduler tick is stopped when only one task is running, reducing kernel interruptions on those CPUs.
|
||||
"rcu_nocbs=2,3" # Moves RCU (Read-Copy Update) callback processing away from CPUs 2 and 3.
|
||||
```
|
||||
|
||||
Essentially we completely disable acces from the operating system to these two cpus, which saves scheduler overhead, which would introduce noticable latency.
|
||||
Its not entirely clear yet if what we did here is overkill. Maybe we could use only one core and get away with that?
|
||||
Processor load normally hovers around 18% for one of the cores and like 9% for the other.
|
||||
|
||||
We also have some minor cpu tweaks in configuration.nix
|
||||
```
|
||||
# Graphical
|
||||
"logo.nologo" # Remove kernel logo during boot
|
||||
|
||||
# Performance
|
||||
|
||||
# Specific Vulnerabilities Addressed by Mitigations:
|
||||
# - Spectre variants (V1, V2, V4, SWAPGS, SpectreRSB, etc.)
|
||||
# - Meltdown (Rogue Data Cache Load)
|
||||
# - Foreshadow/L1TF (L1 Terminal Fault)
|
||||
# - Microarchitectural Data Sampling (MDS, RIDL, Fallout, ZombieLoad)
|
||||
# - SRBDS (Special Register Buffer Data Sampling)
|
||||
# - TSX Asynchronous Abort (TAA)
|
||||
# - iTLB Multihit
|
||||
# - And others as they're discovered and mitigated
|
||||
#
|
||||
# With mitigations=off
|
||||
# - PROS: Maximum performance, equivalent to pre-2018 behavior
|
||||
# - CONS: Vulnerable to Spectre, Meltdown, Foreshadow, ZombieLoad, etc.
|
||||
# Should ONLY be used in completely trusted environments
|
||||
# - Improves performance by 7-43%
|
||||
"mitigation=off"
|
||||
"intel_pstate=performance" # Intel CPU-specific performance mode (if applicable)
|
||||
|
||||
# Memory Management
|
||||
"transparent_hugepage=always" # Use larger memory pages for memory intense applications
|
||||
"nmi_watchdog=0" # Disable NMI watchdog for reduced CPU overhead and realtime execution
|
||||
|
||||
# High-throughput ethernet parameters
|
||||
"pcie_aspm=off" # Disable PCIe power management for NICs
|
||||
"intel_iommu=off" # Disable IOMMU (performance gain)
|
||||
|
||||
# Reliability
|
||||
"panic=10" # Auto-reboot 10 seconds after kernel panic
|
||||
"oops=panic" # Treat kernel oops as panic for auto-recovery
|
||||
"usbcore.autosuspend=-1" # Possibly fixes dre disconnect issue?
|
||||
```
|
||||
|
||||
We should check if its really required to turn mitigations off as well.
|
||||
|
||||
## Rust Code Optimizations
|
||||
|
||||
In General we try to avoid memory allocations in our loop as much as possible, as it is not deterministic
|
||||
|
||||
One problem we found, is that due to the realtime kernel our ethernet driver caused us quite large spikes in our tx_rx cycle time for EtherCAT.
|
||||
These spikes happen when CPU Load is very high on the two usable cores, causing the interrupt handler for Ethernet to stall.
|
||||
|
||||
Which is why we reassign the interrupt handler on startup to the same core that the EtherCatTxRx Thread runs on:
|
||||
|
||||
```rust
|
||||
#[cfg(target_os = "linux")]
|
||||
match set_irq_affinity(&interface, 3) {
|
||||
Ok(_) => tracing::info!("ethernet interrupt handler now runs on cpu:{}", 3),
|
||||
Err(e) => tracing::error!("set_irq_affinity failed: {:?}", e),
|
||||
}
|
||||
// Set core affinity to 4th core
|
||||
let _ = set_core_affinity(3);
|
||||
|
||||
// Set the thread to real-time priority
|
||||
let _ = set_realtime_priority();
|
||||
```
|
||||
This means that the Interrupt Handler ALWAYS has enough ressources to cope with our demand for realtime communication with EtherCAT
|
||||
|
||||
|
||||
One Problem we have at the moment (we think) is the locking and async code blocking our server loop.
|
||||
Note that this doesnt affect the EtherCAT communication at all.
|
||||
|
||||
## Electron Optimizations (Not Yet Implemented)
|
||||
|
||||
Currently we are struggling with the fact, that apparently Electron doesnt really have good support for HW acceleration on Linux.
|
||||
Electron runs with the software-renderer, which is VERY slow compared to a gpu.
|
||||
This would eliminate the weird stuttering seen on Graphs and make them run buttery smooth, while also alleviating load off of the CPU.
|
||||
@@ -1,166 +0,0 @@
|
||||
# Presets
|
||||
|
||||
To allow the user to save the current configuration of a machine, they can create a preset in the presets page. Presets are presited in electron's local storage.
|
||||
|
||||
Presets are all handled by the `PresetsPage`. To use the `PresetsPage` for a single machine, one must implement the following.
|
||||
|
||||
Create a zod schema for the data, the presets contain.
|
||||
```tsx
|
||||
const mock1PresetDataSchema = z
|
||||
.object({
|
||||
frequency1: z.number(),
|
||||
frequency2: z.number(),
|
||||
frequency3: z.number(),
|
||||
})
|
||||
.partial();
|
||||
|
||||
type Mock1 = typeof mock1PresetDataSchema;
|
||||
```
|
||||
Here, intensionally, `Mock1` is the schema type itself. It can be used as a type argument everything that uses presets.
|
||||
|
||||
Next, create a map containing all schema versions currently supported (the new schema, to be precise).
|
||||
```tsx
|
||||
const schemas = new Map([[1, mock1PresetDataSchema]]);
|
||||
```
|
||||
New, the schema will be used to parse version `1`. To create new versions, see migrations below.
|
||||
|
||||
One also needs to define a table, how the preset data should be shown to the user.
|
||||
```tsx
|
||||
const previewEntries: PresetPreviewEntries<Mock1> = [
|
||||
{
|
||||
name: "Frequency 1",
|
||||
unit: "mHz",
|
||||
renderValue: (data: PresetData<Mock1>) => data?.frequency1?.toFixed(3),
|
||||
},
|
||||
{
|
||||
name: "Frequency 2",
|
||||
unit: "mHz",
|
||||
renderValue: (data: PresetData<Mock1>) => data?.frequency2?.toFixed(3),
|
||||
},
|
||||
{
|
||||
name: "Frequency 3",
|
||||
unit: "mHz",
|
||||
renderValue: (data: PresetData<Mock1>) => data?.frequency3?.toFixed(3),
|
||||
},
|
||||
];
|
||||
```
|
||||
`renderValue` gets the data itself and renders the corresponding part of it. Returning undefined will automatically be rendered as the value is not set, showing "N/A".
|
||||
|
||||
Create a function for the new react component and use the appropreate machine.
|
||||
```tsx
|
||||
export function Mock1PresetsPage() {
|
||||
const { setFrequency1, setFrequency2, setFrequency3, defaultState, state } =
|
||||
useMock1();
|
||||
// ...
|
||||
}
|
||||
```
|
||||
|
||||
Here, two more functions need to be created. const toPresetData = (s: typeof state): PresetData<Mock1> => ({
|
||||
frequency1: s?.frequency1 ?? defaultState?.frequency1,
|
||||
frequency2: s?.frequency2 ?? defaultState?.frequency2,
|
||||
frequency3: s?.frequency3 ?? defaultState?.frequency3,The first takes in a preset and applies it on the machine. If data is missing, use the default defined by the backend.
|
||||
```tsx
|
||||
export function Mock1PresetsPage() {
|
||||
const { setFrequency1, setFrequency2, setFrequency3, defaultState, state } =
|
||||
useMock1();
|
||||
|
||||
const applyPreset = (preset: Preset<Mock1>) => {
|
||||
const frequency1 = preset.data?.frequency1 ?? 100;
|
||||
const frequency2 = preset.data?.frequency2 ?? 200;
|
||||
const frequency3 = preset.data?.frequency3 ?? 500;
|
||||
|
||||
setFrequency1(frequency1);
|
||||
setFrequency2(frequency2);
|
||||
setFrequency3(frequency3);
|
||||
};
|
||||
// ...
|
||||
}
|
||||
```
|
||||
The later does the opposite, it turns a machine state, comming from the backend, into the preset data
|
||||
```tsx
|
||||
export function Mock1PresetsPage() {
|
||||
const { setFrequency1, setFrequency2, setFrequency3, defaultState, state } =
|
||||
useMock1();
|
||||
// ...
|
||||
const toPresetData = (s: typeof state): PresetData<Mock1> => ({
|
||||
frequency1: s?.frequency1 ?? defaultState?.frequency1,
|
||||
frequency2: s?.frequency2 ?? defaultState?.frequency2,
|
||||
frequency3: s?.frequency3 ?? defaultState?.frequency3,
|
||||
});
|
||||
// ...
|
||||
}
|
||||
```
|
||||
|
||||
Having all this, we can simple pass it to the `PresetsPage`
|
||||
```tsx
|
||||
export function Mock1PresetsPage() {
|
||||
// ...
|
||||
return (
|
||||
<PresetsPage
|
||||
machine_identification={mock1.machine_identification}
|
||||
currentState={toPresetData(state)}
|
||||
schemas={schemas}
|
||||
schemaVersion={1}
|
||||
applyPreset={applyPreset}
|
||||
previewEntries={previewEntries}
|
||||
defaultState={toPresetData(defaultState)}
|
||||
/>
|
||||
);
|
||||
}
|
||||
```
|
||||
Also mind the machine identification, that has to match the machine we are using and the schema versionen. Here we pass `1`, as we also used `1` in the schemas map.
|
||||
|
||||
## Migrations
|
||||
|
||||
Migrations are handled by the `PresetsPage`, however, one still has to define what to migrate.
|
||||
To do so, first create the new schema.
|
||||
```tsx
|
||||
const fancyNewMock1PresetDataSchema = z
|
||||
.object({
|
||||
frequency1_now_with_ai: z.number(),
|
||||
frequency2: z.number(),
|
||||
frequency3: z.number(),
|
||||
frequency4: z.number(),
|
||||
})
|
||||
.partial();
|
||||
|
||||
type Mock1 = typeof fancyNewMock1PresetDataSchema;
|
||||
```
|
||||
and then, also create a migration that uses the new schema, but can handle the old format.
|
||||
```tsx
|
||||
const mock1PresetDataSchemaMigration: PresetSchema = z.preprocess(
|
||||
(obj: any): PresetData<Mock1> => ({
|
||||
frequency1_now_with_ai: obj?.frequency1, // Here, a field have been renamed
|
||||
frequency2: obj?.frequency2,
|
||||
frequency3: obj?.frequency3,
|
||||
frequency4: 666.0, // A new field is added. Initialize it with default the value
|
||||
}),
|
||||
fancyNewMock1PresetDataSchema,
|
||||
);
|
||||
```
|
||||
Usings zods preprocessing step,
|
||||
we can first transform the data as needed and then validate using the new schema accordingly.
|
||||
|
||||
We have to register both schemas in our schema map.
|
||||
```tsx
|
||||
const schemas = new Map<number, Mock1>([
|
||||
[1, mock1PresetDataSchemaMigration as Mock1],
|
||||
[2, fancyNewMock1PresetDataSchema],
|
||||
]);
|
||||
```
|
||||
Mind the cast, which is ok, because `mock1PresetDataSchemaMigration` parses `fancyNewMock1PresetDataSchema` by construction.
|
||||
|
||||
Lastly, simply change the schema version being passed to the `PresetsPage`.
|
||||
```diff
|
||||
@@ -62,7 +77,7 @@ export function Mock1PresetsPage() {
|
||||
machine_identification={mock1.machine_identification}
|
||||
currentState={toPresetData(state)}
|
||||
schemas={schemas}
|
||||
- schemaVersion={1}
|
||||
+ schemaVersion={2}
|
||||
applyPreset={applyPreset}
|
||||
previewEntries={previewEntries}
|
||||
defaultState={toPresetData(defaultState)}
|
||||
```
|
||||
Make sure, that all (exsiting and new migrations) return a schema version 2 conform format.
|
||||
Of corse, one can chain migrations of needed - just the end result must be complient.
|
||||
@@ -1,27 +0,0 @@
|
||||
# Testing Ram and Cpu (Hardware)
|
||||
|
||||
There were instability problems on one of the hmi panel pc's related to ram.
|
||||
|
||||
To test Ram and Cpu for stability problems we use memtest86.
|
||||
|
||||
You can download it here: https://www.memtest86.com/download.htm
|
||||
Then in the unzipped folder you have to flash the .img file to a USB drive with atleast 2GB of space.
|
||||
We already have USB-Sticks flashed with memtest though.
|
||||
|
||||
1. When starting up the Pc mash Del (Entf in german) on an external keyboard and enter the bios
|
||||
|
||||
2. Select the Memtest86 usb stick in the Bios
|
||||
|
||||
3. Save changes and exit
|
||||
|
||||
4. Now memtest boots up, just let it do its thing from here
|
||||
|
||||
It works if the screen looks something like this:
|
||||
|
||||
|
||||
|
||||
Simply let it do one Pass ~20 minutes and shutdown and unplug usb stick afterwards
|
||||
|
||||
If it freezes completely -> unstable
|
||||
|
||||
If it shows errors the ram should be swapped out and the test repeated
|
||||
-127
@@ -115,130 +115,3 @@ pub fn (
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
|
||||
# Device Implementation Guide
|
||||
|
||||
This section describes how to implement EtherCAT devices in the framework used by the QiTech Control project (via `ethercat-hal`, `server`, etc.). We include a running example of a simple digital output device (EL2004) and discuss how it fits into the larger architecture.
|
||||
|
||||
## Architecture & Context
|
||||
|
||||
The QiTech Control system is built as:
|
||||
|
||||
- `ethercat-hal` / `ethercat-hal-derive`: Hardware abstraction, PDO / CoE handling, device traits.
|
||||
- `server`: Glue code that reads device identities, instantiates the correct device types, performs CoE configuration, and runs cyclic process data.
|
||||
- The `docs` directory documents APIs and device patterns.
|
||||
:contentReference[oaicite:0]{index=0}
|
||||
|
||||
In this architecture:
|
||||
|
||||
1. During initialization, subdevice identity tuples (Vendor ID, Product ID, Revision) are read.
|
||||
2. The `server` matches identity tuples to concrete `Device` types and constructs them (via `NewEthercatDevice`).
|
||||
3. The device instance then handles PDO exchange, and optional CoE (configuration) writes.
|
||||
4. Higher-level application code interacts with devices via traits like `DigitalOutputDevice`, `DigitalInputDevice`, etc.
|
||||
|
||||
---
|
||||
|
||||
## Example: Simple Digital Output Device (EL2004)
|
||||
|
||||
Below is a minimal example of a digital-output-only EtherCAT device. It shows how to map PDOs, implement traits, and define identity constants.
|
||||
|
||||
```rust
|
||||
use super::{EthercatDeviceProcessing, NewEthercatDevice, SubDeviceIdentityTuple};
|
||||
use crate::helpers::ethercrab_types::EthercrabSubDevicePreoperational;
|
||||
use crate::io::digital_output::{DigitalOutputDevice, DigitalOutputOutput};
|
||||
use crate::pdo::{RxPdo, basic::BoolPdoObject};
|
||||
use ethercat_hal_derive::{EthercatDevice, RxPdo};
|
||||
|
||||
/// EL2004 4-channel digital output device — 24 V DC, 0.5 A per channel
|
||||
#[derive(EthercatDevice)]
|
||||
pub struct EL2004 {
|
||||
/// The Rx PDO where output bits are written by the master
|
||||
pub rxpdo: EL2004RxPdo,
|
||||
is_used: bool,
|
||||
}
|
||||
|
||||
impl EthercatDeviceProcessing for EL2004 {}
|
||||
|
||||
impl std::fmt::Debug for EL2004 {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
write!(f, "EL2004")
|
||||
}
|
||||
}
|
||||
|
||||
impl NewEthercatDevice for EL2004 {
|
||||
fn new() -> Self {
|
||||
Self {
|
||||
rxpdo: EL2004RxPdo::default(),
|
||||
is_used: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl DigitalOutputDevice<EL2004Port> for EL2004 {
|
||||
fn set_output(&mut self, port: EL2004Port, value: DigitalOutputOutput) {
|
||||
let expect_text = "All channels should be Some(_)";
|
||||
match port {
|
||||
EL2004Port::DO1 => {
|
||||
self.rxpdo.channel1.as_mut().expect(expect_text).value = value.into()
|
||||
}
|
||||
EL2004Port::DO2 => {
|
||||
self.rxpdo.channel2.as_mut().expect(expect_text).value = value.into()
|
||||
}
|
||||
EL2004Port::DO3 => {
|
||||
self.rxpdo.channel3.as_mut().expect(expect_text).value = value.into()
|
||||
}
|
||||
EL2004Port::DO4 => {
|
||||
self.rxpdo.channel4.as_mut().expect(expect_text).value = value.into()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn get_output(&self, port: EL2004Port) -> DigitalOutputOutput {
|
||||
let expect_text = "All channels should be Some(_)";
|
||||
DigitalOutputOutput(match port {
|
||||
EL2004Port::DO1 => self.rxpdo.channel1.as_ref().expect(expect_text).value,
|
||||
EL2004Port::DO2 => self.rxpdo.channel2.as_ref().expect(expect_text).value,
|
||||
EL2004Port::DO3 => self.rxpdo.channel3.as_ref().expect(expect_text).value,
|
||||
EL2004Port::DO4 => self.rxpdo.channel4.as_ref().expect(expect_text).value,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum EL2004Port {
|
||||
DO1,
|
||||
DO2,
|
||||
DO3,
|
||||
DO4,
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, RxPdo)]
|
||||
pub struct EL2004RxPdo {
|
||||
#[pdo_object_index(0x1600)]
|
||||
pub channel1: Option<BoolPdoObject>,
|
||||
#[pdo_object_index(0x1601)]
|
||||
pub channel2: Option<BoolPdoObject>,
|
||||
#[pdo_object_index(0x1602)]
|
||||
pub channel3: Option<BoolPdoObject>,
|
||||
#[pdo_object_index(0x1603)]
|
||||
pub channel4: Option<BoolPdoObject>,
|
||||
}
|
||||
|
||||
impl Default for EL2004RxPdo {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
channel1: Some(BoolPdoObject::default()),
|
||||
channel2: Some(BoolPdoObject::default()),
|
||||
channel3: Some(BoolPdoObject::default()),
|
||||
channel4: Some(BoolPdoObject::default()),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Identity constants for EL2004
|
||||
pub const EL2004_VENDOR_ID: u32 = 0x2;
|
||||
pub const EL2004_PRODUCT_ID: u32 = 131346514;
|
||||
pub const EL2004_REVISION_A: u32 = 1179648;
|
||||
pub const EL2004_IDENTITY_A: SubDeviceIdentityTuple =
|
||||
(EL2004_VENDOR_ID, EL2004_PRODUCT_ID, EL2004_REVISION_A);
|
||||
@@ -1,13 +0,0 @@
|
||||
# Electrical Diagrams
|
||||
|
||||
This directory contains electrical wiring diagrams for various machines and systems supported by the control system.
|
||||
|
||||
## Available Diagrams
|
||||
|
||||
### Extruder
|
||||
- **Location**: [`extruder/`](extruder/)
|
||||
- **File**: [QiTech Pro Extruder Electrical Diagram 2025.pdf](extruder/QiTech_Pro_Extruder_Electrical_Diagram_2025.pdf)
|
||||
|
||||
### Winder
|
||||
- **Location**: [`winder/`](winder/)
|
||||
- **File**: [QiTech Pro Winder Electrical Diagram.pdf](winder/QiTech_Pro_Winder_Electrical_Diagram.pdf)
|
||||
Binary file not shown.
Binary file not shown.
@@ -27,13 +27,13 @@ We need to know what the purpose of each device in a machine is:
|
||||
|
||||
How and where do we save the identification values? A machine does not have a central storage medium. Saving the network configuration in the application is bad when changing the EtherCAT master device. Additionally, the serial number in the EEPROM is always `0`. Not all devices have the CoE key value store.
|
||||
|
||||
The solution: Persisting the values in empty spaces inside each device's EEPROM. As seen in the [ESC Access](https://infosys.beckhoff.com/english.php?content=../content/1033/tc3_io_intro/1358008331.html&id=) documentation, there are reserved spaces. With a hex dump of the EEPROM, we can confirm that these spaces are likely not used. The EEPROM is very small, so we can fit the values as four `u16` (Big Endian) values. In case we need to customize the position of the values, we customize the addresses for each device.
|
||||
The solution: Persisting the values in empty spaces inside each device's EEPROM. As seen in the [ESC Access](https://infosys.beckhoff.com/english.php?content=../content/1033/tc3_io_intro/1358008331.html&id=) documentation, there are reserved spaces. With a hex dump of the EEPROM, we can confirm that these spaces are likely not used. The EEPROM is very small, so we can fit the values as four `u32` (Big Endian) values. In case we need to customize the position of the values, we customize the addresses for each device.
|
||||
|
||||
The default word (not byte) addresses are:
|
||||
- Vendor: `0x0028`
|
||||
- Machine: `0x0029`
|
||||
- Serial: `0x002A`
|
||||
- Role: `0x002B`
|
||||
- Machine: `0x002C`
|
||||
- Role: `0x002E`
|
||||
|
||||
## Writing values
|
||||
1. For each new device, we need to identify free space in the EEPROM and add them to `get_identification_addresses` in `control-core/src/identification/mod.rs`.
|
||||
|
||||
+75
-144
@@ -11,8 +11,6 @@ A functionality can be virtually wired to any device that matches the IO type. E
|
||||
|
||||
Functionalities of different machines should not be wired to the same devices because of possibly conflicting configuration and machine identification issues (which devices belong to what machine).
|
||||
|
||||
The IO types provide direct synchronous methods for interacting with hardware, eliminating the need for actor patterns while maintaining clean abstractions.
|
||||
|
||||
# Implemented IO types
|
||||
|
||||
- Digital Output (DO)
|
||||
@@ -37,13 +35,13 @@ pub enum EL2002Port {
|
||||
|
||||
Then the device must implement the respective device trait like `DigitalOutputDevice`.
|
||||
|
||||
The `set_output` method is used to set the desired level of the output of a given port.
|
||||
The `digital_output_write` is used to set the desired level of the output of a given port.
|
||||
|
||||
The `get_output` method is used to read the current state of the output of a given port.
|
||||
The `digital_output_state` is used to read the current state of the output of a given port.
|
||||
|
||||
```rust
|
||||
impl DigitalOutputDevice<EL2002Port> for EL2002 {
|
||||
fn set_output(&mut self, port: EL2002Port, value: DigitalOutputOutput) {
|
||||
fn digital_output_write(&mut self, port: EL2002Port, value: DigitalOutputOutput) {
|
||||
let expect_text = "All channels should be Some(_)";
|
||||
match port {
|
||||
EL2002Port::DO1 => self.rxpdo.channel1.as_mut().expect(&expect_text).value = value.into(),
|
||||
@@ -51,95 +49,84 @@ impl DigitalOutputDevice<EL2002Port> for EL2002 {
|
||||
}
|
||||
}
|
||||
|
||||
fn get_output(&self, port: EL2002Port) -> DigitalOutputOutput {
|
||||
fn digital_output_state(&self, port: EL2002Port) -> DigitalOutputState {
|
||||
let expect_text = "All channels should be Some(_)";
|
||||
DigitalOutputOutput(match port {
|
||||
EL2002Port::DO1 => self.rxpdo.channel1.as_ref().expect(&expect_text).value,
|
||||
EL2002Port::DO2 => self.rxpdo.channel2.as_ref().expect(&expect_text).value,
|
||||
})
|
||||
DigitalOutputState {
|
||||
output: DigitalOutputOutput(match port {
|
||||
EL2002Port::DO1 => self.rxpdo.channel1.as_ref().expect(&expect_text).value,
|
||||
EL2002Port::DO2 => self.rxpdo.channel2.as_ref().expect(&expect_text).value,
|
||||
}),
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Using IO types in machine logic
|
||||
## Implementing for an `Actor`
|
||||
|
||||
IO types now provide direct synchronous methods for reading and writing values, making them easy to use in machine logic without async/await patterns.
|
||||
The `act` function of an `Actor` will be called after the inputs are read from the device and before the outputs are written for the next EtherCAT frame.
|
||||
|
||||
The actor needs to be constructed with the `DigitalOutput` and hold it in its struct.
|
||||
```rust
|
||||
#[derive(Debug)]
|
||||
pub struct DigitalOutputToggler {
|
||||
output: DigitalOutput,
|
||||
}
|
||||
|
||||
impl DigitalOutputToggler {
|
||||
pub fn new(output: DigitalOutput) -> Self {
|
||||
Self { output }
|
||||
}
|
||||
|
||||
pub fn toggle(&mut self) {
|
||||
let current_state = self.output.get();
|
||||
self.output.set(!current_state);
|
||||
}
|
||||
|
||||
pub fn turn_on(&mut self) {
|
||||
self.output.set(true);
|
||||
}
|
||||
|
||||
pub fn turn_off(&mut self) {
|
||||
self.output.set(false);
|
||||
}
|
||||
```
|
||||
|
||||
Implementing the `act` function which will toggle the output every cycle.
|
||||
|
||||
The `state` and `write` values are callbacks and must be wrapped with parentheses to be called.
|
||||
```rust
|
||||
fn act(&mut self, _now: Instant) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
|
||||
Box::pin(async move {
|
||||
{
|
||||
let state = (self.output.state)().await;
|
||||
match state.output.into() {
|
||||
true => (self.output.write)(false.into()).await,
|
||||
false => (self.output.write)(true.into()).await,
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
```
|
||||
|
||||
## Getting `DigitalOutput` from a `Device`
|
||||
With the `new` function of the `DigitalOutput` struct, you can create a new `DigitalOutput` instance from a device and its port. The IO type provides direct synchronous methods for interaction.
|
||||
|
||||
With the `new` function of the `DigitalOutput` struct, you can create a new `DigitalOutput` instance from a device and its port. We can then provide this `DigitalOutput` instance to the `Actor` struct.
|
||||
```rust
|
||||
let el2002 = Arc::new(RwLock::new(EL2002::new()));
|
||||
let el2002 = EL2002::new();
|
||||
let digital_output = DigitalOutput::new(el2002, EL2002Port::DO1);
|
||||
let mut toggler = DigitalOutputToggler::new(digital_output);
|
||||
|
||||
// Direct usage
|
||||
toggler.turn_on();
|
||||
toggler.toggle();
|
||||
let actor = DigitalOutputToggler::new(digital_output);
|
||||
```
|
||||
|
||||
# Creating a new IO type
|
||||
To create a new IO type, we must know if it's writable (output) or readable (input), or combined (input/output) type.
|
||||
|
||||
For output types, we define the struct with sync closures for setting and getting values:
|
||||
We first define the `DigitalOutput` struct with the `write` and `state` fields. Every IO needs a `state` field but only writable types need a `write` field.
|
||||
|
||||
The `state` field is an async callback which returns the `DigitalOutputState` struct.
|
||||
|
||||
The `write` field is an async callback which takes the value.
|
||||
|
||||
```rust
|
||||
pub struct DigitalOutput {
|
||||
set_output: Box<dyn Fn(DigitalOutputOutput) -> () + Send + Sync>,
|
||||
get_output: Box<dyn Fn() -> DigitalOutputOutput + Send + Sync>,
|
||||
pub write: Box<dyn Fn(DigitalOutputOutput) -> Pin<Box<dyn Future<Output = ()> + Send>> + Send + Sync>,
|
||||
pub state:
|
||||
Box<dyn Fn() -> Pin<Box<dyn Future<Output = DigitalOutputState> + Send>> + Send + Sync>,
|
||||
}
|
||||
```
|
||||
|
||||
For input types, we only need the getter:
|
||||
Next, we need to implement the `DigitalOutputState` and `DigitalOutputOutput` structs.
|
||||
|
||||
The `DigitalOutputState` contains the timestamps of the output (and inputs if applicable) and the `DigitalOutputOutput` struct (and the input struct if applicable). This is the return type of `DigitalOutput::state`.
|
||||
|
||||
The `DigitalOutputOutput` is both contained in the `DigitalOutputState` and is the input type of the `DigitalOutput::write` function. It implements the `From` trait in both directions to `bool` to allow easy access to the inner value on this simple type (as seen in the `act` example above).
|
||||
```rust
|
||||
pub struct DigitalInput {
|
||||
get_input: Box<dyn Fn() -> Result<DigitalInputInput, anyhow::Error> + Send + Sync>,
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct DigitalOutputState {
|
||||
pub output: DigitalOutputOutput,
|
||||
}
|
||||
```
|
||||
|
||||
For combined input/output types, we have separate getters for input and output:
|
||||
|
||||
```rust
|
||||
pub struct StepperVelocityEL70x1 {
|
||||
set_output: Box<dyn Fn(StepperVelocityEL70x1Output) -> Result<(), Error> + Send + Sync>,
|
||||
get_output: Box<dyn Fn() -> Result<StepperVelocityEL70x1Output, Error> + Send + Sync>,
|
||||
get_input: Box<dyn Fn() -> Result<StepperVelocityEL70x1Input, Error> + Send + Sync>,
|
||||
// ... additional closures as needed
|
||||
}
|
||||
```
|
||||
|
||||
Next, we need to implement the input and output structs separately.
|
||||
|
||||
For a digital output, we only need the output struct:
|
||||
|
||||
```rust
|
||||
/// Output value
|
||||
/// true: high
|
||||
/// false: low
|
||||
@@ -159,35 +146,8 @@ impl From<DigitalOutputOutput> for bool {
|
||||
}
|
||||
```
|
||||
|
||||
For input types, we define input structs:
|
||||
|
||||
```rust
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct DigitalInputInput {
|
||||
pub value: bool,
|
||||
}
|
||||
```
|
||||
|
||||
For combined input/output types, we define both structs separately, avoiding the need for state containers.
|
||||
|
||||
```rust
|
||||
pub struct DigitalOutputOutput(pub bool);
|
||||
|
||||
impl From<bool> for DigitalOutputOutput {
|
||||
fn from(value: bool) -> Self {
|
||||
DigitalOutputOutput(value)
|
||||
}
|
||||
}
|
||||
|
||||
impl From<DigitalOutputOutput> for bool {
|
||||
fn from(value: DigitalOutputOutput) -> Self {
|
||||
value.0
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
Now we implement the constructor for the IO type. The `new` function takes a device and port and returns the IO instance with sync closures:
|
||||
|
||||
Now we need to implement the constructor for a `DigitalOutput` instance. The `new` function takes a device and a port and returns a `DigitalOutput` instance.
|
||||
We need to construct the async closures which is a bit of type madness since we also need to move the device `Arc` into them.
|
||||
```rust
|
||||
impl DigitalOutput {
|
||||
pub fn new<PORT>(
|
||||
@@ -197,75 +157,46 @@ impl DigitalOutput {
|
||||
where
|
||||
PORT: Clone + Send + Sync + 'static,
|
||||
{
|
||||
// build sync write closure
|
||||
// build async write closure
|
||||
let port1 = port.clone();
|
||||
let device1 = device.clone();
|
||||
let set_output = Box::new(move |value: DigitalOutputOutput| {
|
||||
let mut device = block_on(device1.write());
|
||||
device.set_output(port1.clone(), value);
|
||||
});
|
||||
let write = Box::new(
|
||||
move |value: DigitalOutputOutput| -> Pin<Box<dyn Future<Output = ()> + Send>> {
|
||||
let device_clone = device1.clone();
|
||||
let port_clone = port1.clone();
|
||||
Box::pin(async move {
|
||||
let mut device = device_clone.write().await;
|
||||
device.digital_output_write(port_clone, value);
|
||||
})
|
||||
},
|
||||
);
|
||||
|
||||
// build sync get closure
|
||||
// build async get closure
|
||||
let port2 = port.clone();
|
||||
let device2 = device.clone();
|
||||
let get_output = Box::new(move || -> DigitalOutputOutput {
|
||||
let device = block_on(device2.read());
|
||||
device.get_output(port2.clone())
|
||||
});
|
||||
|
||||
DigitalOutput {
|
||||
set_output,
|
||||
get_output,
|
||||
}
|
||||
}
|
||||
|
||||
/// Set the digital output value
|
||||
pub fn set(&self, enabled: bool) {
|
||||
(self.set_output)(enabled.into());
|
||||
}
|
||||
|
||||
/// Get the current output value
|
||||
pub fn get(&self) -> bool {
|
||||
let output = (self.get_output)();
|
||||
output.into()
|
||||
let state = Box::new(
|
||||
move || -> Pin<Box<dyn Future<Output = DigitalOutputState> + Send>> {
|
||||
let device2 = device2.clone();
|
||||
let port_clone = port2.clone();
|
||||
Box::pin(async move {
|
||||
let device = device2.read().await;
|
||||
device.digital_output_state(port_clone)
|
||||
})
|
||||
},
|
||||
);
|
||||
DigitalOutput { write, state }
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
Lastly, we create the device trait that needs to be implemented by devices. The trait methods are simplified and separated by input/output:
|
||||
|
||||
Lastly, we can create the `DigitalOutputDevice` trait which needs to be implemented by the device.
|
||||
```rust
|
||||
pub trait DigitalOutputDevice<PORT>: Send + Sync
|
||||
where
|
||||
PORT: Clone,
|
||||
{
|
||||
fn set_output(&mut self, port: PORT, value: DigitalOutputOutput);
|
||||
fn get_output(&self, port: PORT) -> DigitalOutputOutput;
|
||||
fn digital_output_write(&mut self, port: PORT, value: DigitalOutputOutput);
|
||||
fn digital_output_state(&self, port: PORT) -> DigitalOutputState;
|
||||
}
|
||||
```
|
||||
|
||||
For input devices:
|
||||
|
||||
```rust
|
||||
pub trait DigitalInputDevice<PORT>: Send + Sync
|
||||
where
|
||||
PORT: Clone,
|
||||
{
|
||||
fn get_input(&self, port: PORT) -> Result<DigitalInputInput, anyhow::Error>;
|
||||
}
|
||||
```
|
||||
|
||||
For combined input/output devices:
|
||||
|
||||
```rust
|
||||
pub trait StepperVelocityEL70x1Device<PORT>: Send + Sync
|
||||
where
|
||||
PORT: Clone,
|
||||
{
|
||||
fn set_output(&mut self, port: PORT, value: StepperVelocityEL70x1Output) -> Result<(), Error>;
|
||||
fn get_output(&self, port: PORT) -> Result<StepperVelocityEL70x1Output, Error>;
|
||||
fn get_input(&self, port: PORT) -> Result<StepperVelocityEL70x1Input, Error>;
|
||||
// Additional methods as needed for specific device functionality
|
||||
fn get_speed_range(&self, port: PORT) -> SpeedRange;
|
||||
}
|
||||
```
|
||||
+5
-22
@@ -308,32 +308,15 @@ pub fn my_function() {
|
||||
|
||||
### Structured Logging
|
||||
|
||||
**Important**: When logging to journald (systemd's logging service), key-value pairs in event logs are not properly captured. Instead, include structured data using string formatting within the message itself.
|
||||
|
||||
```rust
|
||||
use tracing::{info, instrument};
|
||||
|
||||
// ❌ Don't use key-value pairs in event logs (won't work with journald)
|
||||
info!(
|
||||
user_id = %user.id,
|
||||
email = %user.email,
|
||||
"Updating user profile"
|
||||
);
|
||||
|
||||
// ✅ Use string formatting instead (works with journald)
|
||||
info!(
|
||||
"Updating user profile user_id={} email={}",
|
||||
user.id,
|
||||
user.email
|
||||
);
|
||||
|
||||
// ✅ Spans can still use structured fields
|
||||
#[instrument(fields(user_id = %user.id, operation = "update"))]
|
||||
pub fn update_user(user: &User) {
|
||||
info!(
|
||||
"Updating user profile user_id={} email={}",
|
||||
user.id,
|
||||
user.email
|
||||
user_id = %user.id,
|
||||
email = %user.email,
|
||||
"Updating user profile"
|
||||
);
|
||||
}
|
||||
```
|
||||
@@ -370,11 +353,11 @@ use anyhow::Result;
|
||||
pub fn operation_with_error_handling() -> Result<()> {
|
||||
match risky_operation() {
|
||||
Ok(result) => {
|
||||
info!("Operation completed successfully result={:?}", result);
|
||||
info!(result = ?result, "Operation completed successfully");
|
||||
Ok(())
|
||||
}
|
||||
Err(e) => {
|
||||
error!("Operation failed error={}", e);
|
||||
error!(error = %e, "Operation failed");
|
||||
Err(e)
|
||||
}
|
||||
}
|
||||
|
||||
Loaded 100 of 547 files, more files were not shown because too many files have changed in this diff.
Show more
Reference in new issue
Block a user