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>
|
||||
+1
-126
@@ -1,126 +1 @@
|
||||
# `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
+294
-311
File diff suppressed because it is too large.
Load diff
+1
-1
@@ -4,7 +4,7 @@ members = [
|
||||
"ethercat-hal",
|
||||
"ethercat-hal-derive",
|
||||
"control-core",
|
||||
"ethercat-eeprom-dump", "control-core-derive",
|
||||
"ethercat-eeprom-dump",
|
||||
]
|
||||
exclude = []
|
||||
resolver = "2"
|
||||
@@ -4,9 +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.*
|
||||
|
||||
# Repo Structure
|
||||
|
||||
Frontend
|
||||
@@ -44,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/))
|
||||
@@ -68,9 +63,10 @@ We could combine the code of the frontend and backend using [Doxius](https://dio
|
||||
| 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 | Not Yet | |
|
||||
| Waterway | V1 | 2025 | Filament Water Cooling | - | 1 (Qitech Industries GmbH) | 5 | Not Yet | |
|
||||
| Laser | V1 | ??? | Diameter Measuring Laser | - | 1 (Qitech Industries GmbH) | 6 | 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
|
||||
|
||||
- [x] [Architecture & Data Flow](./docs/architecture-overview.md)
|
||||
@@ -94,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:$USER" /dev/ttyUSB*
|
||||
fi
|
||||
|
||||
# set cap for server executable
|
||||
sudo setcap cap_net_raw=eip ./target/debug/server
|
||||
# run
|
||||
./target/debug/server
|
||||
@@ -1,12 +0,0 @@
|
||||
[package]
|
||||
name = "control_core_derive"
|
||||
version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[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,31 +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)
|
||||
}
|
||||
+5
-14
@@ -4,29 +4,20 @@ version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[dependencies]
|
||||
anyhow = "1.0.99"
|
||||
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"
|
||||
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.6"
|
||||
core_affinity = "0.8.3"
|
||||
control_core_derive = { version = "0.1.0", path = "../control-core-derive" }
|
||||
|
||||
|
||||
[target.'cfg(unix)'.dependencies]
|
||||
libc = "0.2.175"
|
||||
|
||||
[dev-dependencies]
|
||||
approx = "0.5.1"
|
||||
textplots = "0.8"
|
||||
@@ -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,241 +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 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 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 fn get_kp(&self) -> f64 {
|
||||
self.kp
|
||||
}
|
||||
|
||||
pub fn get_ki(&self) -> f64 {
|
||||
self.ki
|
||||
}
|
||||
|
||||
pub fn get_kd(&self) -> f64 {
|
||||
self.kd
|
||||
}
|
||||
|
||||
pub fn configure(&mut self, ki: f64, kp: f64, kd: f64) {
|
||||
self.reset();
|
||||
self.kp = kp;
|
||||
self.ki = ki;
|
||||
self.kd = kd;
|
||||
}
|
||||
|
||||
pub 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 {
|
||||
let signal = match self.last {
|
||||
// First update
|
||||
None => {
|
||||
// Calculate error
|
||||
let ep = error;
|
||||
|
||||
// Calculate signal
|
||||
let signal = self.kp * ep;
|
||||
let clamped_signal = ClampingTimeagnosticPidController::optional_clamp(
|
||||
signal,
|
||||
self.min_signal,
|
||||
self.max_signal,
|
||||
);
|
||||
|
||||
// Set values
|
||||
self.ep =
|
||||
ClampingTimeagnosticPidController::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 = ClampingTimeagnosticPidController::optional_clamp(
|
||||
error,
|
||||
self.min_ep,
|
||||
self.max_ep,
|
||||
);
|
||||
|
||||
let ei = ClampingTimeagnosticPidController::optional_clamp(
|
||||
self.ei + ep * dt,
|
||||
self.min_ei,
|
||||
self.max_ei,
|
||||
);
|
||||
|
||||
let ed = ClampingTimeagnosticPidController::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 = kp * ep + ki * ei + kd * ed;
|
||||
let clamped_signal = ClampingTimeagnosticPidController::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
|
||||
}
|
||||
};
|
||||
|
||||
signal
|
||||
}
|
||||
|
||||
pub 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);
|
||||
}
|
||||
}
|
||||