This commit is contained in:
Icedsoda committed 2025-02-11 18:08:56 +01:00
1 parent e85d90816b
commit eecdc6332c
15 files changed
+577 -216

No files matched your search

Generated
+41
View File
@@ -282,6 +282,18 @@ version = "2.8.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "8f68f53c83ab957f72c32642f3868eec03eb974d1fb82e453128456482613d36"
[[package]]
name = "bitvec"
version = "1.0.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1bc2832c24239b0141d5674bb9174f9d68a8b5b3f2753311927c172ca46f7e9c"
dependencies = [
"funty",
"radium",
"tap",
"wyz",
]
[[package]]
name = "block-buffer"
version = "0.10.4"
@@ -745,6 +757,7 @@ name = "ethercat_hal"
version = "0.1.0"
dependencies = [
"anyhow",
"bitvec",
"crossbeam-utils",
"ethercat_hal_derive",
"ethercrab",
@@ -852,6 +865,12 @@ dependencies = [
"percent-encoding",
]
[[package]]
name = "funty"
version = "2.0.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "e6d5a32815ae3f33302d95fdcb2ce17862f8c65363dcfd29360480ba1001fc9c"
[[package]]
name = "futures"
version = "0.3.31"
@@ -1627,6 +1646,12 @@ dependencies = [
"proc-macro2",
]
[[package]]
name = "radium"
version = "0.7.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "dc33ff2d4973d518d823d61aa239014831e521c75da58e3df4840d3f47749d09"
[[package]]
name = "rand"
version = "0.8.5"
@@ -1813,6 +1838,7 @@ version = "0.1.0"
dependencies = [
"anyhow",
"axum",
"bitvec",
"chrono",
"env_logger",
"ethercat_hal",
@@ -2034,6 +2060,12 @@ version = "1.0.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "0bf256ce5efdfa370213c1dabab5935a12e49f2c58d15e9eac2870d3b4f27263"
[[package]]
name = "tap"
version = "1.0.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "55937e1799185b12863d447f42597ed69d9928686b8d88a1df17376a097d8369"
[[package]]
name = "thiserror"
version = "1.0.69"
@@ -2637,6 +2669,15 @@ dependencies = [
"memchr",
]
[[package]]
name = "wyz"
version = "0.5.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "05f360fc0b24296329c78fda852a1e9ae82de9cf7b27dae4b7f62f118f77b9ed"
dependencies = [
"tap",
]
[[package]]
name = "zerocopy"
version = "0.7.35"
+90 -11
View File
@@ -1,6 +1,6 @@
use proc_macro::TokenStream;
use quote::quote;
use syn::{Data, DeriveInput};
use syn::{parse_macro_input, Data, DeriveInput};
#[derive(deluxe::ExtractAttributes)]
#[deluxe(attributes(pdo_object_index))]
@@ -36,7 +36,7 @@ fn rxpdo_derive2(item: proc_macro2::TokenStream) -> deluxe::Result<proc_macro2::
let (impl_generics, ty_generics, where_clause) = ast.generics.split_for_impl();
let expanded = quote! {
impl #impl_generics Configuration for #ident #ty_generics #where_clause {
impl #impl_generics crate::coe::Configuration for #ident #ty_generics #where_clause {
async fn write_config<'a>(
&self,
device: &'a EthercrabSubDevice<'a>,
@@ -57,10 +57,10 @@ fn rxpdo_derive2(item: proc_macro2::TokenStream) -> deluxe::Result<proc_macro2::
}
impl #impl_generics RxPdo for #ident #ty_generics #where_clause {
fn get_objects(&self) -> &[Option<&dyn RxPdoObject>] {
fn get_objects(&self) -> &[Option<&dyn crate::pdo::RxPdoObject>] {
let objs = vec![
#(
self.#field_name.as_ref().map(|o| o as &dyn RxPdoObject),
self.#field_name.as_ref().map(|o| o as &dyn crate::pdo::RxPdoObject),
)*
];
Box::leak(objs.into_boxed_slice())
@@ -86,7 +86,7 @@ fn txpdo_derive2(item: proc_macro2::TokenStream) -> deluxe::Result<proc_macro2::
let (impl_generics, ty_generics, where_clause) = ast.generics.split_for_impl();
let expanded = quote! {
impl #impl_generics Configuration for #ident #ty_generics #where_clause {
impl #impl_generics crate::coe::Configuration for #ident #ty_generics #where_clause {
async fn write_config<'a>(
&self,
device: &'a EthercrabSubDevice<'a>,
@@ -106,20 +106,20 @@ fn txpdo_derive2(item: proc_macro2::TokenStream) -> deluxe::Result<proc_macro2::
}
}
impl #impl_generics TxPdo for #ident #ty_generics #where_clause {
fn get_objects(&self) -> &[Option<&dyn TxPdoObject>] {
impl #impl_generics crate::pdo::TxPdo for #ident #ty_generics #where_clause {
fn get_objects(&self) -> &[Option<&dyn crate::pdo::TxPdoObject>] {
let objs = vec![
#(
self.#field_name.as_ref().map(|o| o as &dyn TxPdoObject),
self.#field_name.as_ref().map(|o| o as &dyn crate::pdo::TxPdoObject),
)*
];
Box::leak(objs.into_boxed_slice())
}
fn get_objects_mut(&mut self) -> &mut [Option<&mut dyn TxPdoObject>] {
fn get_objects_mut(&mut self) -> &mut [Option<&mut dyn crate::pdo::TxPdoObject>] {
let objs = vec![
#(
self.#field_name.as_mut().map(|o| o as &mut dyn TxPdoObject),
self.#field_name.as_mut().map(|o| o as &mut dyn crate::pdo::TxPdoObject),
)*
];
Box::leak(objs.into_boxed_slice())
@@ -150,7 +150,7 @@ fn pdo_object_derive2(item: proc_macro2::TokenStream) -> deluxe::Result<proc_mac
let (impl_generics, ty_generics, where_clause) = ast.generics.split_for_impl();
let expanded = quote! {
impl #impl_generics PdoObject for #ident #ty_generics #where_clause {
impl #impl_generics crate::pdo::PdoObject for #ident #ty_generics #where_clause {
fn size(&self) -> usize {
#bits
}
@@ -164,3 +164,82 @@ fn pdo_object_derive2(item: proc_macro2::TokenStream) -> deluxe::Result<proc_mac
pub fn pdo_object_derive(item: TokenStream) -> TokenStream {
pdo_object_derive2(item.into()).unwrap().into()
}
extern crate proc_macro;
#[proc_macro_derive(Device)]
pub fn device_derive(input: TokenStream) -> TokenStream {
let input = parse_macro_input!(input as DeriveInput);
let name = &input.ident;
let mut output_impl = quote! {};
let mut input_impl = quote! {};
let mut ts_impl = quote! {};
let mut has_rxpdo = false;
let mut has_txpdo = false;
let mut self_output_ts = quote! {};
let mut self_input_ts = quote! {};
if let Data::Struct(data_struct) = input.data {
for field in data_struct.fields.iter() {
if let Some(ident) = &field.ident {
if ident == "rxpdo" {
has_rxpdo = true;
}
if ident == "txpdo" {
has_txpdo = true;
}
}
}
}
if has_rxpdo {
output_impl = quote! {
fn output(&self, output: &mut bitvec::prelude::BitSlice<u8, bitvec::prelude::Lsb0>) {
self.rxpdo.write(output);
}
fn output_len(&self) -> usize {
self.rxpdo.size()
}
};
self_output_ts = quote! {
self.output_ts = output_ts;
}
}
if has_txpdo {
input_impl = quote! {
fn input(&mut self, input: & bitvec::prelude::BitSlice<u8, bitvec::prelude::Lsb0>) {
self.txpdo.read(input);
}
fn input_len(&self) -> usize {
self.txpdo.size()
}
};
self_input_ts = quote! {
self.input_ts = input_ts;
}
}
// Generate the ts function based on the presence of rxpdo and txpdo
ts_impl = quote! {
fn ts(&mut self, input_ts: u64, output_ts: u64) {
#self_output_ts
#self_input_ts
}
};
let expanded = quote! {
impl crate::devices::Device for #name {
#output_impl
#input_impl
#ts_impl
fn as_any(&self) -> &dyn std::any::Any {
self
}
}
};
TokenStream::from(expanded)
}
+1
View File
@@ -12,3 +12,4 @@ log = "0.4.25"
stepper_driver = { version = "0.1.0", path = "../stepper-driver" }
tokio = { version = "1.43.0", features = ["full"] }
uom = "0.36.0"
bitvec = { version = "1.0.1", features = ["alloc"] }
@@ -1,16 +1,28 @@
use super::Actor;
use crate::io::digital_output::DigitalOutput;
use crate::io::pulse_train_output::{
PulseTrainOutput, PulseTrainOutputInput, PulseTrainOutputOutput,
};
use std::{future::Future, pin::Pin, sync::Arc};
use tokio::sync::RwLock;
/// Set a digital output high and low with a given interval
pub struct StepperDriverPulseTrain {
pulse: DigitalOutput,
pulse: PulseTrainOutput,
// track position
counter_cleared_last: bool,
last_target_position: u32,
position: i64,
}
impl StepperDriverPulseTrain {
pub fn new(output: DigitalOutput) -> Self {
Self { pulse: output }
pub fn new(output: PulseTrainOutput) -> Self {
Self {
pulse: output,
counter_cleared_last: false,
last_target_position: 0,
position: 0,
}
}
}
@@ -18,14 +30,42 @@ impl Actor for StepperDriverPulseTrain {
fn act(&mut self, _now_ts: u64) -> Pin<Box<dyn Future<Output = ()> + Send + '_>> {
Box::pin(async move {
let state = (self.pulse.state)().await;
match state.value {
true => (self.pulse.write)(false).await,
false => (self.pulse.write)(true).await,
}
let mut output = state.output.clone();
log::info!("state: {:#?}", state);
log::info!("position: {:#?}", self.position);
// self.counter_to_position(&state.input, &mut output);
output.frequency_value = 6000;
(self.pulse.write)(output).await;
})
}
}
impl StepperDriverPulseTrain {
/// since the internal counter of 32bit is a bit small, we use this counter instead
fn counter_to_position(
&mut self,
input: &PulseTrainOutputInput,
output: &mut PulseTrainOutputOutput,
) {
self.position += input.counter_value as i64;
match self.counter_cleared_last {
true => {
output.set_counter = false;
self.counter_cleared_last = false;
}
false => {
output.set_counter_value = 0;
output.set_counter = true;
// reduce target positon by the amount since
self.counter_cleared_last = true;
}
}
}
}
impl From<StepperDriverPulseTrain> for Arc<RwLock<StepperDriverPulseTrain>> {
fn from(actor: StepperDriverPulseTrain) -> Self {
Arc::new(RwLock::new(actor))
+113 -52
View File
@@ -1,28 +1,30 @@
use super::Device;
use crate::{
coe::{Configuration, RX_PDO_ASSIGNMENT_REG, TX_PDO_ASSIGNMENT_REG},
coe::Configuration,
io::pulse_train_output::{
PulseTrainOutputDevice, PulseTrainOutputState, PulseTrainOutputWrite,
PulseTrainOutputDevice, PulseTrainOutputInput, PulseTrainOutputOutput,
PulseTrainOutputState,
},
pdo::{
PdoObject, PdoPreset, RxPdo, RxPdoObject, TxPdo, TxPdoObject,
EL252X::{EncControl, EncStatus, PtoControl, PtoStatus, PtoTarget},
el252x::{EncControl, EncStatus, PtoControl, PtoStatus, PtoTarget},
PdoPreset, RxPdo, RxPdoObject, TxPdo, TxPdoObject,
},
types::EthercrabSubDevice,
};
use anyhow::Ok;
use ethercat_hal_derive::{RxPdo, TxPdo};
use std::any::Any;
use bitvec::prelude::*;
use ethercat_hal_derive::{Device, RxPdo, TxPdo};
/// EL2521 8-channel digital output device
///
/// 24V DC, 0.5A per channel
#[derive(Debug)]
#[derive(Debug, Device)]
pub struct EL2521 {
pub configuration: EL2521Configuration,
pub txpdo: EL2521TxPdo,
pub rxpdo: EL2521RxPdo,
pub output_ts: u64,
pub input_ts: u64,
}
impl EL2521 {
@@ -36,6 +38,7 @@ impl EL2521 {
txpdo,
rxpdo,
output_ts: 0,
input_ts: 0,
}
}
@@ -46,48 +49,38 @@ impl EL2521 {
}
}
impl Device for EL2521 {
fn output(&self, output: &mut [u8]) {
// log::info!("EL2521 output {:?}", self.output_pdus);
self.rxpdo.write(output);
}
fn output_len(&self) -> usize {
self.rxpdo.size()
}
fn input(&mut self, _input: &[u8]) {
log::info!("EL2521 input {:?}", _input);
self.txpdo.read(_input);
}
fn input_len(&self) -> usize {
self.txpdo.size()
}
fn ts(&mut self, _input_ts: u64, output_ts: u64) {
self.output_ts = output_ts;
}
fn as_any(&self) -> &dyn Any {
self
}
}
impl PulseTrainOutputDevice<EL2521Port> for EL2521 {
fn pulse_train_output_write(&mut self, port: EL2521Port, value: PulseTrainOutputWrite) {
match self.configuration.pdo_assignment {
EL2521PdoPreset::EnhancedOperatingMode32Bit => {}
_ => {
panic!("Only EnhancedOperatingMode32Bit is supported");
}
}
fn pulse_train_output_write(&mut self, port: EL2521Port, value: PulseTrainOutputOutput) {
self.rxpdo.pto_control.as_mut().unwrap().disble_ramp = value.disble_ramp;
self.rxpdo.pto_control.as_mut().unwrap().frequency_value = value.frequency_value;
self.rxpdo.pto_target.as_mut().unwrap().target_counter_value = value.target_counter_value;
self.rxpdo.enc_control.as_mut().unwrap().set_counter = value.set_counter;
self.rxpdo.enc_control.as_mut().unwrap().set_counter_value = value.set_counter_value;
}
fn pulse_train_output_state(&self, port: EL2521Port) -> PulseTrainOutputState {
match self.configuration.pdo_assignment {
EL2521PdoPreset::EnhancedOperatingMode32Bit => {}
_ => {
panic!("Only EnhancedOperatingMode32Bit is supported");
}
PulseTrainOutputState {
output_ts: self.output_ts,
input: PulseTrainOutputInput {
select_end_counter: self.txpdo.pto_status.as_ref().unwrap().select_end_counter,
ramp_active: self.txpdo.pto_status.as_ref().unwrap().ramp_active,
input_t: self.txpdo.pto_status.as_ref().unwrap().input_t,
input_z: self.txpdo.pto_status.as_ref().unwrap().input_z,
error: self.txpdo.pto_status.as_ref().unwrap().error,
sync_error: self.txpdo.pto_status.as_ref().unwrap().sync_error,
counter_underflow: self.txpdo.enc_status.as_ref().unwrap().counter_underflow,
counter_overflow: self.txpdo.enc_status.as_ref().unwrap().counter_overflow,
counter_value: self.txpdo.enc_status.as_ref().unwrap().counter_value,
set_counter_done: self.txpdo.enc_status.as_ref().unwrap().set_counter_done,
},
output: PulseTrainOutputOutput {
disble_ramp: self.rxpdo.pto_control.as_ref().unwrap().disble_ramp,
frequency_value: self.rxpdo.pto_control.as_ref().unwrap().frequency_value,
target_counter_value: self.rxpdo.pto_target.as_ref().unwrap().target_counter_value,
set_counter: self.rxpdo.enc_control.as_ref().unwrap().set_counter,
set_counter_value: self.rxpdo.enc_control.as_ref().unwrap().set_counter_value,
},
}
unimplemented!()
}
}
@@ -365,28 +358,96 @@ impl PdoPreset<EL2521TxPdo, EL2521RxPdo> for EL2521PdoPreset {
}
}
#[derive(Debug, Clone, TxPdo)]
#[derive(Debug, Clone, TxPdo, Default)]
struct EL2521TxPdo {
/// # `0x1A01` PTO Status
#[pdo_object_index(0x1A01)]
pub pto_status: Option<PtoStatus>,
/// # `0x1A02` Encoder Status
#[pdo_object_index(0x1A02)]
#[pdo_object_index(0x1A05)]
pub enc_status: Option<EncStatus>,
}
#[derive(Debug, Clone, RxPdo)]
#[derive(Debug, Clone, RxPdo, Default)]
struct EL2521RxPdo {
/// # `0x1601` PTO Control
#[pdo_object_index(0x1601)]
pub pto_control: Option<PtoControl>,
/// # `0x1607` PTO Target
#[pdo_object_index(0x1607)]
pub pto_target: Option<PtoTarget>,
/// # `0x1605` Encoder Control
#[pdo_object_index(0x1605)]
pub enc_control: Option<EncControl>,
}
#[cfg(test)]
mod tests {
use super::*;
use bitvec::prelude::*;
#[test]
fn test_rx_pdo() {
let mut buffer = [0u8; 14];
let rxpdo = EL2521RxPdo {
pto_control: Some(PtoControl {
frequency_select: true,
disble_ramp: true,
go_counter: true,
frequency_value: 1000,
}),
pto_target: Some(PtoTarget {
target_counter_value: 1000000,
}),
enc_control: Some(EncControl {
set_counter: true,
set_counter_value: 1000000,
}),
};
let bits = buffer.view_bits_mut::<Lsb0>();
rxpdo.write(bits);
assert_eq!(buffer[0], 0b0000_0111);
assert_eq!(u16::from_be_bytes([buffer[2], buffer[3]]), 1000);
assert_eq!(
u32::from_be_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]),
1000000
);
assert_eq!(buffer[8], 0b0000_0100,);
assert_eq!(
u32::from_be_bytes([buffer[10], buffer[11], buffer[12], buffer[13]]),
1000000
);
}
#[test]
fn text_tx_pdo() {
let buffer = [
0b0000_0111,
0,
0b0000_0100,
0,
0,
3,
232,
0,
0,
0,
0,
3,
232,
0,
];
let bits = buffer.view_bits::<Lsb0>();
let mut txpdo = EL2521PdoPreset::EnhancedOperatingMode32Bit.txpdo_assignment();
txpdo.read(bits);
assert_eq!(txpdo.pto_status.as_ref().unwrap().select_end_counter, true);
// assert_eq!(txpdo.pto_status.as_ref().unwrap().ramp_active, true);
// assert_eq!(txpdo.pto_status.as_ref().unwrap().input_t, true);
// assert_eq!(txpdo.pto_status.as_ref().unwrap().input_z, false);
// assert_eq!(txpdo.pto_status.as_ref().unwrap().error, false);
// assert_eq!(txpdo.pto_status.as_ref().unwrap().sync_error, false);
// assert_eq!(txpdo.pto_status.as_ref().unwrap().txpdo_toggle, false);
// assert_eq!(txpdo.enc_status.as_ref().unwrap().counter_underflow, false);
// assert_eq!(txpdo.enc_status.as_ref().unwrap().counter_overflow, true);
// assert_eq!(txpdo.enc_status.as_ref().unwrap().counter_value, 1000);
// assert_eq!(txpdo.enc_status.as_ref().unwrap().set_counter_done, false);
}
}
+14 -14
View File
@@ -35,32 +35,32 @@ impl TemperatureInputDevice<EL3204Port> for EL3204 {
let byte_1 = self.input_pdu[byte_offset + 1];
// subindex 01
let status_undervoltage = byte_0 & 0b0000_0001 != 0;
let undervoltage = byte_0 & 0b0000_0001 != 0;
//subindex 02
let status_overvoltage = byte_0 & 0b0000_0010 != 0;
let overvoltage = byte_0 & 0b0000_0010 != 0;
// subindex 03/04
let status_limit_1 = TemperatureInputLimit::new((byte_0 & 0b0000_1100) >> 2 as u8);
let limit_1 = TemperatureInputLimit::new((byte_0 & 0b0000_1100) >> 2 as u8);
// subindex 05/06
let status_limit_2: TemperatureInputLimit =
let limit_2: TemperatureInputLimit =
TemperatureInputLimit::new((byte_0 & 0b0011_0000) >> 4 as u8);
// subindex 07
let status_error = byte_0 & 0b1000_0000 != 0;
let error = byte_0 & 0b1000_0000 != 0;
// subindex 0F 0b0100_0000
let status_valid = TemperatureInputValid::new(byte_1 & 0b0100_0000 >> 6);
let valid = TemperatureInputValid::new(byte_1 & 0b0100_0000 >> 6);
// subindex 10 0b1000_0000
let status_toggle = byte_1 & 0b1000_0000 != 0;
let toggle = byte_1 & 0b1000_0000 != 0;
let temperature = (value as f32) / 10.0;
TemperatureInputState {
input_ts: self.inputs_ts,
value: temperature,
status_undervoltage,
status_overvoltage,
status_limit_1,
status_limit_2,
status_error,
status_valid,
status_toggle,
undervoltage,
overvoltage,
limit_1,
limit_2,
error,
valid,
toggle,
}
}
}
+23 -22
View File
@@ -1,19 +1,20 @@
// pub mod el1008;
pub mod el1008;
pub mod el2008;
pub mod el2024;
// pub mod el1008;
// pub mod el2008;
// pub mod el2024;
pub mod el2521;
pub mod el2634;
pub mod el2809;
pub mod el3204;
pub mod el4008;
// pub mod el2634;
// pub mod el2809;
// pub mod el3204;
// pub mod el4008;
use super::devices::{
el1008::EL1008, el2008::EL2008, el2024::EL2024, el2634::EL2634, el2809::EL2809, el3204::EL3204,
el4008::EL4008,
};
// use super::devices::{
// el1008::EL1008, el2008::EL2008, el2024::EL2024, el2634::EL2634, el2809::EL2809, el3204::EL3204,
// el4008::EL4008,
// };
use crate::devices::el2521::EL2521;
use anyhow::anyhow;
use bitvec::{order::Lsb0, slice::BitSlice};
use ethercrab::{subdevice_group::Op, MainDevice, SubDeviceGroup, SubDevicePdi, SubDeviceRef};
use std::{any::Any, sync::Arc};
use tokio::sync::RwLock;
@@ -21,7 +22,7 @@ use tokio::sync::RwLock;
pub trait Device: Any + Send + Sync {
/// Input data from the last cycle
/// `ts` is the timestamp when the input data was sent by the device
fn input(&mut self, _input: &[u8]) {
fn input(&mut self, _input: &BitSlice<u8, Lsb0>) {
()
}
@@ -31,7 +32,7 @@ pub trait Device: Any + Send + Sync {
}
/// automatically validate input length, then calls input
fn input_checked(&mut self, input: &[u8]) -> Result<(), anyhow::Error> {
fn input_checked(&mut self, input: &BitSlice<u8, Lsb0>) -> Result<(), anyhow::Error> {
// validate input has correct length
let input_len = self.input_len();
if input.len() != input_len {
@@ -49,7 +50,7 @@ pub trait Device: Any + Send + Sync {
/// Output data for the next cycle
/// `ts` is the timestamp when the output data is predicted to be received by the device
fn output(&self, _output: &mut [u8]) {
fn output(&self, _output: &mut BitSlice<u8, Lsb0>) {
()
}
@@ -58,7 +59,7 @@ pub trait Device: Any + Send + Sync {
0
}
fn output_checked(&self, output: &mut [u8]) -> Result<(), anyhow::Error> {
fn output_checked(&self, output: &mut BitSlice<u8, Lsb0>) -> Result<(), anyhow::Error> {
self.output(output);
// validate input has correct length
@@ -108,13 +109,13 @@ fn device_from_subdevice<'maindevice, 'group, const PDI_LEN: usize>(
) -> Result<Arc<RwLock<dyn Device>>, anyhow::Error> {
let name = subdevice.name();
match name {
"EL2008" => Ok(Arc::new(RwLock::new(EL2008::new()))),
"EL2809" => Ok(Arc::new(RwLock::new(EL2809::new()))),
"EL2634" => Ok(Arc::new(RwLock::new(EL2634::new()))),
"EL4008" => Ok(Arc::new(RwLock::new(EL4008::new()))),
"EL1008" => Ok(Arc::new(RwLock::new(EL1008::new()))),
"EL3204" => Ok(Arc::new(RwLock::new(EL3204::new()))),
"EL2024" => Ok(Arc::new(RwLock::new(EL2024::new()))),
// "EL2008" => Ok(Arc::new(RwLock::new(EL2008::new()))),
// "EL2809" => Ok(Arc::new(RwLock::new(EL2809::new()))),
// "EL2634" => Ok(Arc::new(RwLock::new(EL2634::new()))),
// "EL4008" => Ok(Arc::new(RwLock::new(EL4008::new()))),
// "EL1008" => Ok(Arc::new(RwLock::new(EL1008::new()))),
// "EL3204" => Ok(Arc::new(RwLock::new(EL3204::new()))),
// "EL2024" => Ok(Arc::new(RwLock::new(EL2024::new()))),
"EL2521" => Ok(Arc::new(RwLock::new(EL2521::new()))),
_ => Err(anyhow::anyhow!("No Driver: {}", name)),
}
+18 -26
View File
@@ -4,7 +4,7 @@ use tokio::sync::RwLock;
pub struct PulseTrainOutput {
pub write: Box<
dyn Fn(PulseTrainOutputWrite) -> Pin<Box<dyn Future<Output = ()> + Send>> + Send + Sync,
dyn Fn(PulseTrainOutputOutput) -> Pin<Box<dyn Future<Output = ()> + Send>> + Send + Sync,
>,
pub state:
Box<dyn Fn() -> Pin<Box<dyn Future<Output = PulseTrainOutputState> + Send>> + Send + Sync>,
@@ -22,7 +22,7 @@ impl PulseTrainOutput {
let port1 = port.clone();
let device1 = device.clone();
let write = Box::new(
move |value: PulseTrainOutputWrite| -> Pin<Box<dyn Future<Output = ()> + Send>> {
move |value: PulseTrainOutputOutput| -> Pin<Box<dyn Future<Output = ()> + Send>> {
let device_clone = device1.clone();
let port_clone = port1.clone();
Box::pin(async move {
@@ -54,45 +54,37 @@ impl PulseTrainOutput {
pub struct PulseTrainOutputState {
/// Nanosecond timestamp
pub output_ts: u64,
pub input: PulseTrainOutputRead,
pub output: PulseTrainOutputWrite,
pub input: PulseTrainOutputInput,
pub output: PulseTrainOutputOutput,
}
#[derive(Debug, Clone)]
pub struct PulseTrainOutputRead {
// 0x1A01
pub status_select_end_counter: bool,
pub status_ramp_active: bool,
pub status_input_t: bool,
pub status_input_z: bool,
pub status_error: bool,
pub status_sync_error: bool,
pub status_txpdo_toggle: bool,
// 0x1A05
pub set_counter_done: bool,
pub struct PulseTrainOutputInput {
pub select_end_counter: bool,
pub ramp_active: bool,
pub input_t: bool,
pub input_z: bool,
pub error: bool,
pub sync_error: bool,
pub counter_underflow: bool,
pub counter_overflow: bool,
pub counter_value: u64,
pub counter_value: u32,
pub set_counter_done: bool,
}
#[derive(Debug, Clone)]
pub struct PulseTrainOutputWrite {
// 0x1601
pub frequency_select: bool,
pub struct PulseTrainOutputOutput {
pub disble_ramp: bool,
pub go_counter: bool,
pub frequency_value: u32,
// 0x1607 UDINT
pub target_counter_value: u64,
// 0x1605
pub frequency_value: i32,
pub target_counter_value: u32,
pub set_counter: bool,
pub set_counter_value: u64,
pub set_counter_value: u32,
}
pub trait PulseTrainOutputDevice<PORT>: Send + Sync
where
PORT: Clone,
{
fn pulse_train_output_write(&mut self, port: PORT, value: PulseTrainOutputWrite);
fn pulse_train_output_write(&mut self, port: PORT, value: PulseTrainOutputOutput);
fn pulse_train_output_state(&self, port: PORT) -> PulseTrainOutputState;
}
+7 -7
View File
@@ -40,19 +40,19 @@ pub struct TemperatureInputState {
/// Temperature in degrees Celsius (°C) with a resolution of 0.1°C
pub value: Value,
/// Under-voltage error
pub status_undervoltage: bool,
pub undervoltage: bool,
/// Over-voltage error
pub status_overvoltage: bool,
pub overvoltage: bool,
/// Configured limit 1
pub status_limit_1: TemperatureInputLimit,
pub limit_1: TemperatureInputLimit,
/// Configured limit 2
pub status_limit_2: TemperatureInputLimit,
pub limit_2: TemperatureInputLimit,
/// Error flag
pub status_error: bool,
pub error: bool,
/// if the TxPdu sstate is valid
pub status_valid: TemperatureInputValid,
pub valid: TemperatureInputValid,
/// if the TxPdu is toggled
pub status_toggle: bool,
pub toggle: bool,
}
#[derive(Debug, Clone)]
+142 -51
View File
@@ -1,73 +1,70 @@
use super::{RxPdoObject, TxPdoObject};
use bitvec::{field::BitField, order::Lsb0, slice::BitSlice};
use ethercat_hal_derive::PdoObject;
use super::{PdoObject, RxPdoObject, TxPdoObject};
#[derive(Debug, Clone, Default, PdoObject)]
#[derive(Debug, Clone, Default, PdoObject, PartialEq)]
#[pdo_object(bits = 16)]
pub struct PtoStatus {
pub status_select_end_counter: bool,
pub status_ramp_active: bool,
pub status_input_t: bool,
pub status_input_z: bool,
pub status_error: bool,
pub status_sync_error: bool,
pub status_txpdo_toggle: bool,
pub select_end_counter: bool,
pub ramp_active: bool,
pub input_t: bool,
pub input_z: bool,
pub error: bool,
pub sync_error: bool,
pub txpdo_toggle: bool,
}
impl TxPdoObject for PtoStatus {
fn read(&mut self, buffer: &[u8]) {
self.status_select_end_counter = buffer[0] & 0b0000_0001 != 0;
self.status_ramp_active = buffer[0] & 0b0000_0010 != 0;
self.status_input_t = buffer[0] & 0b0001_0000 != 0;
self.status_input_z = buffer[0] & 0b0010_0000 != 0;
self.status_error = buffer[0] & 0b0100_0000 != 0;
fn read(&mut self, buffer: &BitSlice<u8, Lsb0>) {
self.select_end_counter = buffer[0];
self.ramp_active = buffer[1];
self.input_t = buffer[4];
self.input_z = buffer[5];
self.error = buffer[6];
self.status_sync_error = buffer[1] & 0b0010_0000 != 0;
self.status_txpdo_toggle = buffer[1] & 0b1000_0000 != 0;
self.sync_error = buffer[8 + 5];
self.txpdo_toggle = buffer[8 + 7];
}
}
#[derive(Debug, Clone, Default, PdoObject)]
#[pdo_object(bits = 64)]
#[derive(Debug, Clone, Default, PdoObject, PartialEq)]
#[pdo_object(bits = 48)]
pub struct EncStatus {
pub status_set_counter_done: bool,
pub status_counter_underflow: bool,
pub status_counter_overflow: bool,
pub status_sync_error: bool,
pub status_txpdo_toggle: bool,
pub set_counter_done: bool,
pub counter_underflow: bool,
pub counter_overflow: bool,
pub sync_error: bool,
pub txpdo_toggle: bool,
pub counter_value: u32,
}
impl TxPdoObject for EncStatus {
fn read(&mut self, buffer: &[u8]) {
self.status_set_counter_done = buffer[0] & 0b0000_0100 != 0;
self.status_counter_underflow = buffer[0] & 0b0000_100 != 0;
self.status_counter_overflow = buffer[0] & 0b0001_0000 != 0;
self.status_sync_error = buffer[1] & 0b0010_0000 != 0;
self.status_txpdo_toggle = buffer[1] & 0b1000_0000 != 0;
self.counter_value = u32::from_be_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]);
fn read(&mut self, bits: &BitSlice<u8, Lsb0>) {
self.set_counter_done = bits[2];
self.counter_underflow = bits[3];
self.counter_overflow = bits[4];
self.sync_error = bits[8 + 5];
self.txpdo_toggle = bits[8 + 7];
self.counter_value = bits[16..16 + 32].load_le();
}
}
#[derive(Debug, Clone, Default, PdoObject)]
#[pdo_object(bits = 32)]
pub struct PtoControl {
pub control_frequency_select: bool,
pub control_disble_ramp: bool,
pub control_go_counter: bool,
pub frequency_value: u32,
pub frequency_select: bool,
pub disble_ramp: bool,
pub go_counter: bool,
pub frequency_value: i32,
}
impl RxPdoObject for PtoControl {
fn write(&self, buffer: &mut [u8]) {
buffer[0] = 0u8
| (self.control_frequency_select as u8)
| (self.control_disble_ramp as u8) << 1
| (self.control_go_counter as u8) << 2;
fn write(&self, buffer: &mut BitSlice<u8, Lsb0>) {
buffer.set(0, self.frequency_select);
buffer.set(1, self.disble_ramp);
buffer.set(2, self.go_counter);
buffer[2..=3].copy_from_slice(&self.frequency_value.to_be_bytes());
buffer[16..16 + 16].store_le(self.frequency_value);
}
}
@@ -78,22 +75,116 @@ pub struct PtoTarget {
}
impl RxPdoObject for PtoTarget {
fn write(&self, buffer: &mut [u8]) {
buffer[4..=7].copy_from_slice(&self.target_counter_value.to_be_bytes());
fn write(&self, buffer: &mut BitSlice<u8, Lsb0>) {
buffer[0..32].store_le(self.target_counter_value);
}
}
#[derive(Debug, Clone, Default, PdoObject)]
#[pdo_object(bits = 32)]
#[pdo_object(bits = 48)]
pub struct EncControl {
pub control_set_counter: bool,
pub set_counter: bool,
pub set_counter_value: u32,
}
impl RxPdoObject for EncControl {
fn write(&self, buffer: &mut [u8]) {
buffer[8] = 0u8 | (self.control_set_counter as u8) << 2;
fn write(&self, buffer: &mut BitSlice<u8, Lsb0>) {
buffer.set(2, self.set_counter);
buffer[10..=13].copy_from_slice(&self.set_counter_value.to_be_bytes());
buffer[16..16 + 32].store_le(self.set_counter_value);
}
}
#[cfg(test)]
mod tests {
use super::*;
use bitvec::prelude::*;
#[test]
fn test_pto_status() {
// set all flags
let buffer = vec![0b0111_0011u8, 0b1010_0000u8];
let bits = buffer.view_bits::<Lsb0>();
let mut pdo_status = PtoStatus::default();
pdo_status.read(&bits);
assert_eq!(
pdo_status,
PtoStatus {
select_end_counter: true,
ramp_active: true,
input_t: true,
input_z: true,
error: true,
sync_error: true,
txpdo_toggle: true
}
)
}
#[test]
fn test_enc_status() {
// set all flags
// set counter value to 0x12345678
let buffer = vec![0b0001_1100u8, 0b1010_0000u8, 0x12u8, 0x34u8, 0x56u8, 0x78u8];
let bits = buffer.view_bits::<Lsb0>();
let mut enc_status = EncStatus::default();
enc_status.read(&bits);
assert_eq!(
enc_status,
EncStatus {
set_counter_done: true,
counter_underflow: true,
counter_overflow: true,
sync_error: true,
txpdo_toggle: true,
counter_value: 0x12345678
}
)
}
#[test]
fn test_pto_control() {
// set all flags
// set frequency value to 0x1234
let mut buffer = vec![0u8; 4];
let mut bits = buffer.view_bits_mut::<Lsb0>();
let pto_control = PtoControl {
frequency_select: true,
disble_ramp: true,
go_counter: true,
frequency_value: 0x1234,
};
pto_control.write(&mut bits);
assert_eq!(buffer, vec![0b0000_0111u8, 0u8, 0x12u8, 0x34u8])
}
#[test]
fn test_pto_target() {
// set target counter value to 0x12345678
let mut buffer = vec![0u8; 4];
let mut bits = buffer.view_bits_mut::<Lsb0>();
let pto_target = PtoTarget {
target_counter_value: 0x12345678,
};
pto_target.write(&mut bits);
assert_eq!(buffer, vec![0x12u8, 0x34u8, 0x56u8, 0x78u8])
}
#[test]
fn test_enc_control() {
// set all flags
// set counter value to 0x12345678
let mut buffer = vec![0u8; 6];
let mut bits = buffer.view_bits_mut::<Lsb0>();
let enc_control = EncControl {
set_counter: true,
set_counter_value: 0x12345678,
};
enc_control.write(&mut bits);
assert_eq!(
buffer,
vec![0b0000_0100u8, 0u8, 0x12u8, 0x34u8, 0x56u8, 0x78u8]
)
}
}
+43
View File
@@ -0,0 +1,43 @@
use bitvec::{
order::{Lsb0, Msb0},
slice::BitSlice,
};
use ethercat_hal_derive::PdoObject;
use super::{RxPdoObject, TxPdoObject};
#[derive(Debug, Clone, Default, PdoObject)]
#[pdo_object(bits = 1)]
pub struct BoolPdoObject {
pub value: bool,
}
impl TxPdoObject for BoolPdoObject {
fn read(&mut self, buffer: &BitSlice<u8, Lsb0>) {
self.value = buffer[0].into();
}
}
impl RxPdoObject for BoolPdoObject {
fn write(&self, buffer: &mut BitSlice<u8, Lsb0>) {
buffer.set(0, self.value);
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_bool_pdo_object() {
let mut buffer = [0u8];
let mut object = BoolPdoObject::default();
object.value = true;
let mut bits = BitSlice::<_, Lsb0>::from_slice_mut(&mut buffer);
object.write(&mut bits);
assert_eq!(buffer[0], 1);
}
}
View File
Whitespace-only changes.
+13 -12
View File
@@ -1,7 +1,10 @@
use bitvec::prelude::*;
use crate::coe::Configuration;
#[allow(non_snake_case)]
pub mod EL252X;
mod EL252X;
pub mod basic;
pub mod el252x;
pub trait PdoObject {
/// size in bits
@@ -9,11 +12,11 @@ pub trait PdoObject {
}
pub trait TxPdoObject: PdoObject {
fn read(&mut self, buffer: &[u8]);
fn read(&mut self, buffer: &BitSlice<u8, Lsb0>);
}
pub trait RxPdoObject: PdoObject {
fn write(&self, buffer: &mut [u8]);
fn write(&self, buffer: &mut BitSlice<u8, Lsb0>);
}
pub trait PdoPreset<TXPDOA, RXPDOA>
@@ -35,13 +38,12 @@ pub trait RxPdo: Configuration {
.sum::<usize>()
}
fn write(&self, buffer: &mut [u8]) {
fn write(&self, buffer: &mut BitSlice<u8, Lsb0>) {
let mut bit_offset = 0;
for object in self.get_objects() {
if let Some(object) = object {
let start_byte_index = bit_offset / 8;
let end_byte_index = (bit_offset + object.size()) / 8;
object.write(&mut buffer[start_byte_index..=end_byte_index]);
let end_bit_index = bit_offset + object.size();
object.write(&mut buffer[bit_offset..end_bit_index]);
bit_offset += object.size();
}
}
@@ -60,14 +62,13 @@ pub trait TxPdo: Configuration {
.sum::<usize>()
}
fn read(&mut self, buffer: &[u8]) {
fn read(&mut self, buffer: &BitSlice<u8, Lsb0>) {
let mut bit_offset = 0;
for object in self.get_objects_mut().iter_mut() {
if let Some(object) = object {
let start_byte_index = bit_offset / 8;
let end_byte_index = (bit_offset + object.size()) / 8;
let end_bit_index = bit_offset + object.size();
object.read(&buffer[bit_offset..end_bit_index]);
bit_offset += object.size();
object.read(&buffer[start_byte_index..=end_byte_index]);
}
}
}
+1
View File
@@ -38,3 +38,4 @@ tokio = { version = "1.43.0", features = ["rt-multi-thread"] }
include_dir = "0.7.4"
mime_guess = "2.0.5"
open = "5.3.2"
bitvec = "1.0.1"
+23 -13
View File
@@ -14,18 +14,21 @@ use ethercat_hal::actors::analog_function_generator::{
};
use ethercat_hal::actors::digital_input_logger::DigitalInputLogger;
use ethercat_hal::actors::stepper_driver_max_speed::StepperDriverMaxSpeed;
use ethercat_hal::actors::stepper_driver_pulse_train::StepperDriverPulseTrain;
use ethercat_hal::actors::temperature_input_logger::TemperatureInputLogger;
use ethercat_hal::actors::Actor;
use ethercat_hal::coe::Configuration;
use ethercat_hal::devices::el1008::{EL1008Port, EL1008};
use ethercat_hal::devices::el2008::{EL2008Port, EL2008};
use ethercat_hal::devices::el2521::{EL2521Configuration, EL2521OperatingMode};
use ethercat_hal::devices::el3204::{EL3204Port, EL3204};
use ethercat_hal::devices::el4008::{EL4008Port, EL4008};
use ethercat_hal::devices::{devices_from_subdevice_group, Device};
// use ethercat_hal::devices::el1008::{EL1008Port, EL1008};
// use ethercat_hal::devices::el2008::{EL2008Port, EL2008};
use ethercat_hal::devices::el2521::{EL2521Configuration, EL2521OperatingMode, EL2521Port, EL2521};
// use ethercat_hal::devices::el3204::{EL3204Port, EL3204};
// use ethercat_hal::devices::el4008::{EL4008Port, EL4008};
use bitvec::prelude::*;
use ethercat_hal::devices::{devices_from_subdevice_group, get_device, Device};
use ethercat_hal::io::analog_output::AnalogOutput;
use ethercat_hal::io::digital_input::DigitalInput;
use ethercat_hal::io::digital_output::DigitalOutput;
use ethercat_hal::io::pulse_train_output::PulseTrainOutput;
use ethercat_hal::io::temperature_input::TemperatureInput;
use ethercrab::std::{ethercat_now, tx_rx_task};
use std::{sync::Arc, time::Duration};
@@ -124,11 +127,18 @@ pub async fn setup_loop(interface: &str, app_state: Arc<AppState>) -> Result<(),
let config = EL2521Configuration {
operating_mode: EL2521OperatingMode::PulseDirectionSpecification,
direct_input_mode: true,
ramp_function_active: false,
..EL2521Configuration::default()
};
config.write_config(&subdevice_el2521).await?;
let rxpdo_mapping = subdevice_el2521.sdo_read_array::<u16, 8>(0x1C12).await?;
log::info!("RXPDO mapping: {:?}", rxpdo_mapping);
let txpdo_mapping = subdevice_el2521.sdo_read_array::<u16, 8>(0x1C13).await?;
log::info!("TXPDO mapping: {:?}", txpdo_mapping);
// for subdevice in group.iter(&maindevice) {
// log::info!(
// "Subdevice: config: {}, alias: {}",
@@ -177,6 +187,9 @@ pub async fn setup_loop(interface: &str, app_state: Arc<AppState>) -> Result<(),
// TemperatureInput::new(get_device::<EL3204>(&devices, 4).await?, EL3204Port::T1),
// )
// .to_arc_rwlock(),
Arc::new(RwLock::new(StepperDriverPulseTrain::new(
PulseTrainOutput::new(get_device::<EL2521>(&devices, 5).await?, EL2521Port::PTO1),
))),
];
// set all setup data
@@ -223,9 +236,7 @@ pub async fn loop_once<'maindevice>(
let input_ts = ethercat_now();
// TX/RX cycle
log::info!("TX/RX cycle");
setup.group.tx_rx(&setup.maindevice).await?;
log::info!("TX/RX cycle done");
// Prediction when the next TX/RX cycle starts
let output_ts = input_ts + *average_nanos;
@@ -238,7 +249,8 @@ pub async fn loop_once<'maindevice>(
};
device.ts(input_ts, output_ts);
let input = subdevice.inputs_raw();
device.input_checked(input.as_ref())?;
let input_bits = input.view_bits::<Lsb0>();
device.input_checked(input_bits)?;
}
// execute actors
@@ -254,10 +266,8 @@ pub async fn loop_once<'maindevice>(
None => continue,
};
let mut output = subdevice.outputs_raw_mut();
device.output_checked(output.as_mut())?;
let output_bits = output.view_bits_mut::<Lsb0>();
device.output_checked(output_bits)?;
}
tokio::time::sleep(Duration::from_secs(1)).await;
Ok(())
}