aquapath now works with all the other machines connected (#1103)
* aquapath now works with all the other machines connected * fixed formatting * removed unused import * fixed formatting AGAIN
This commit is contained in:
1 parent
78c7667d96
commit
08ce5a37bd
11 files changed
+281
-126
No files matched your search
Generated
+10
-6
@@ -968,8 +968,7 @@ dependencies = [
|
||||
[[package]]
|
||||
name = "ethercrab"
|
||||
version = "0.6.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "fbb13becfa15713adb09b27fdb74468b827cdc78aa99aea638a89c1b0db3ce94"
|
||||
source = "git+https://github.com/v-morlock/ethercrab?branch=main#d297d13389d85a4edb4e69cc98f9dd7f12626cac"
|
||||
dependencies = [
|
||||
"async-io",
|
||||
"atomic-waker",
|
||||
@@ -998,8 +997,7 @@ dependencies = [
|
||||
[[package]]
|
||||
name = "ethercrab-wire"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d15828cb020fb44c92ca67e1ee3c122c9621c2e00d383d066e488f0619023ba2"
|
||||
source = "git+https://github.com/v-morlock/ethercrab?branch=main#d297d13389d85a4edb4e69cc98f9dd7f12626cac"
|
||||
dependencies = [
|
||||
"ethercrab-wire-derive",
|
||||
"heapless",
|
||||
@@ -1008,8 +1006,7 @@ dependencies = [
|
||||
[[package]]
|
||||
name = "ethercrab-wire-derive"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0e790b8011211ed70babfcacbd4277b91ee21402ad5ba6a2c293db02e2f12919"
|
||||
source = "git+https://github.com/v-morlock/ethercrab?branch=main#d297d13389d85a4edb4e69cc98f9dd7f12626cac"
|
||||
dependencies = [
|
||||
"criterion",
|
||||
"proc-macro2",
|
||||
@@ -2332,6 +2329,7 @@ dependencies = [
|
||||
"smol",
|
||||
"socketioxide",
|
||||
"spin_sleep",
|
||||
"ta",
|
||||
"textplots",
|
||||
"tokio",
|
||||
"tower-http",
|
||||
@@ -2588,6 +2586,12 @@ version = "1.0.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0bf256ce5efdfa370213c1dabab5935a12e49f2c58d15e9eac2870d3b4f27263"
|
||||
|
||||
[[package]]
|
||||
name = "ta"
|
||||
version = "0.5.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "609409d472a0a7d8d4dd9e19891bbdef546b9dce670c3057d0e02192dc541226"
|
||||
|
||||
[[package]]
|
||||
name = "tap"
|
||||
version = "1.0.1"
|
||||
|
||||
@@ -13,7 +13,7 @@ axum = "0.8.6"
|
||||
|
||||
ethercat_hal = { path = "../ethercat-hal" }
|
||||
units = { path = "../units" }
|
||||
ethercrab = "0.6"
|
||||
ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" }
|
||||
interfaces = "0.0.9"
|
||||
serde = "1.0.219"
|
||||
serialport = "4.7.3"
|
||||
|
||||
@@ -8,7 +8,7 @@ workspace = true
|
||||
|
||||
[dependencies]
|
||||
anyhow = "1.0.100"
|
||||
ethercrab = "0.6"
|
||||
ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" }
|
||||
ethercat_hal_derive = { version = "0.1.0", path = "../ethercat-hal-derive" }
|
||||
units = { path = "../units" }
|
||||
bitvec = { version = "1.0.1", features = ["alloc"] }
|
||||
|
||||
@@ -180,7 +180,6 @@ impl Configuration for EL5152Configuration {
|
||||
) -> Result<(), anyhow::Error> {
|
||||
// Configure channel 1
|
||||
self.channel1.write_channel_config(device, 0x8000).await?;
|
||||
|
||||
// Configure channel 2
|
||||
self.channel2.write_channel_config(device, 0x8010).await?;
|
||||
// Write PDO assignments
|
||||
|
||||
+1
-1
@@ -4,7 +4,7 @@ version = "0.1.0"
|
||||
edition = "2024"
|
||||
|
||||
[dependencies]
|
||||
ethercrab = "0.6"
|
||||
ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" }
|
||||
ethercat_hal = { path = "../ethercat-hal" }
|
||||
control_core = { path = "../control-core" }
|
||||
control_core_derive = { path = "../control-core-derive" }
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
use crate::{
|
||||
MachineNewHardware, MachineNewParams, MachineNewTrait, get_ethercat_device,
|
||||
get_subdevice_by_index, validate_no_role_dublicates,
|
||||
validate_same_machine_identification_unique,
|
||||
validate_no_role_dublicates, validate_same_machine_identification_unique,
|
||||
};
|
||||
|
||||
use super::{
|
||||
@@ -92,7 +91,12 @@ impl MachineNewTrait for AquaPathV1 {
|
||||
pdo_assignment: EL5152PredefinedPdoAssignment::Frequency,
|
||||
..Default::default()
|
||||
};
|
||||
let subdevice = get_subdevice_by_index(hardware.subdevices, 4)?;
|
||||
|
||||
let subdevice =
|
||||
get_ethercat_device::<EL5152>(hardware, params, 4, [EL5152_IDENTITY_A].to_vec())
|
||||
.await?
|
||||
.1;
|
||||
|
||||
el5152
|
||||
.write()
|
||||
.await
|
||||
|
||||
+2
-1
@@ -9,7 +9,7 @@ workspace = true
|
||||
|
||||
|
||||
[dependencies]
|
||||
ethercrab = "0.6"
|
||||
ethercrab = { git = "https://github.com/v-morlock/ethercrab", branch = "main" }
|
||||
ethercat_hal = { path = "../ethercat-hal" }
|
||||
control_core = { path = "../control-core" }
|
||||
|
||||
@@ -47,6 +47,7 @@ utils = { version = "0.1.0", path="../utils" }
|
||||
spin_sleep = "1.3.3"
|
||||
dhat = { version = "0.3.3", optional = true }
|
||||
ctrlc = { version = "3.3", optional = true }
|
||||
ta = "0.5.0"
|
||||
|
||||
|
||||
[dev-dependencies]
|
||||
|
||||
@@ -7,6 +7,7 @@ use anyhow::{Result, bail};
|
||||
use control_core::socketio::event::GenericEvent;
|
||||
use ethercat_hal::devices::EthercatDevice;
|
||||
use ethercrab::SubDeviceRef;
|
||||
use ethercrab::subdevice_group::HasDc;
|
||||
use ethercrab::{MainDevice, SubDeviceGroup, subdevice_group::Op};
|
||||
use machines::machine_identification::{DeviceIdentification, MachineIdentificationUnique};
|
||||
use machines::serial::registry::SERIAL_DEVICE_REGISTRY;
|
||||
@@ -102,22 +103,25 @@ pub struct EthercatSetup {
|
||||
/// All Ethercat devices
|
||||
/// Generic interface for all devices
|
||||
/// Needed to interface with the devices on an Ethercat level
|
||||
pub group: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, Op>,
|
||||
pub group: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, Op, HasDc>,
|
||||
/// The Ethercat main device
|
||||
/// Needed to interface with the devices
|
||||
pub maindevice: MainDevice<'static>,
|
||||
pub all_operational: bool,
|
||||
}
|
||||
|
||||
impl EthercatSetup {
|
||||
pub fn new(
|
||||
devices: Vec<(DeviceIdentification, Arc<RwLock<dyn EthercatDevice>>)>,
|
||||
group: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, Op>,
|
||||
group: SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, Op, HasDc>,
|
||||
maindevice: MainDevice<'static>,
|
||||
all_operational: bool,
|
||||
) -> Self {
|
||||
Self {
|
||||
devices,
|
||||
group,
|
||||
maindevice,
|
||||
all_operational,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+161
-68
@@ -18,10 +18,18 @@ use ethercat_hal::devices::wago_750_354::{
|
||||
use ethercat_hal::devices::wago_modules::ip20_ec_di8_do8::{
|
||||
IP20_EC_DI8_DO8_PRODUCT_ID, IP20_EC_DI8_DO8_VENDOR_ID, IP20EcDi8Do8,
|
||||
};
|
||||
use ethercrab::error::Error;
|
||||
use ethercrab::subdevice_group::{DcConfiguration, HasDc, PreOpPdi, SafeOp};
|
||||
use smol::stream::StreamExt;
|
||||
use ta::Next;
|
||||
use ta::indicators::ExponentialMovingAverage;
|
||||
|
||||
use crate::utils::{start_dnsmasq, stop_dnsmasq};
|
||||
use crate::utils::stop_dnsmasq;
|
||||
use ethercrab::std::ethercat_now;
|
||||
use ethercrab::{MainDevice, MainDeviceConfig, PduStorage, RetryBehaviour, Timeouts};
|
||||
use ethercrab::{
|
||||
DcSync, MainDevice, MainDeviceConfig, PduStorage, RegisterAddress, RetryBehaviour,
|
||||
SubDeviceGroup, Timeouts,
|
||||
};
|
||||
use machines::machine_identification::{
|
||||
DeviceHardwareIdentification, DeviceHardwareIdentificationEthercat, DeviceIdentification,
|
||||
DeviceIdentificationIdentified, MachineIdentificationUnique, read_device_identifications,
|
||||
@@ -30,6 +38,7 @@ use machines::registry::{MACHINE_REGISTRY, MachineRegistry};
|
||||
use machines::{Machine, MachineNewHardware, MachineNewHardwareEthercat, MachineNewParams};
|
||||
use smol::channel::Sender;
|
||||
use socketioxide::extract::SocketRef;
|
||||
use std::time::Instant;
|
||||
use std::{sync::Arc, time::Duration};
|
||||
|
||||
/// Structure to hold the result of grouping devices by identification
|
||||
@@ -172,7 +181,7 @@ pub async fn setup_loop(
|
||||
app_state: Arc<SharedState>,
|
||||
) -> Result<EthercatSetup, anyhow::Error> {
|
||||
tracing::info!("Starting Ethercat PDU loop");
|
||||
|
||||
const TICK_INTERVAL: Duration = Duration::from_micros(700);
|
||||
let res = stop_dnsmasq();
|
||||
match res {
|
||||
Ok(_) => (),
|
||||
@@ -185,7 +194,6 @@ pub async fn setup_loop(
|
||||
let pdu_storage = Box::leak(Box::new(PduStorage::<MAX_FRAMES, MAX_PDU_DATA>::new()));
|
||||
let (tx, rx, pdu) = pdu_storage.try_split().expect("can only split once");
|
||||
let interface = interface.to_string();
|
||||
let mut has_dc = false;
|
||||
|
||||
std::thread::Builder::new()
|
||||
.name("EthercatTxRxThread".to_owned())
|
||||
@@ -278,6 +286,45 @@ pub async fn setup_loop(
|
||||
))?,
|
||||
};
|
||||
|
||||
for mut subdevice in group_preop.iter_mut(&maindevice) {
|
||||
println!(
|
||||
"subdevice {} {:X?}",
|
||||
subdevice.name(),
|
||||
subdevice.configured_address()
|
||||
);
|
||||
if subdevice.name() == "EL4002" {
|
||||
// Sync mode 01 = SM Synchronous, says it does dc but actually doesnt, thx?
|
||||
subdevice
|
||||
.sdo_write(0x1c32, 1, 1u16)
|
||||
.await
|
||||
.expect("Set sync mode");
|
||||
}
|
||||
// Configure SYNC0 AND SYNC1 for EL4102
|
||||
else if subdevice.name() == "EL5152" {
|
||||
// Sync mode 02 = SYNC0
|
||||
subdevice
|
||||
.sdo_write(0x1c32, 1, 2u16)
|
||||
.await
|
||||
.expect("Set sync mode");
|
||||
|
||||
subdevice
|
||||
.sdo_write(0x1c32, 0x02, TICK_INTERVAL.as_nanos() as u32)
|
||||
.await
|
||||
.expect("Set cycle time");
|
||||
|
||||
subdevice.set_dc_sync(DcSync::Sync01 {
|
||||
// EL4102 ESI specifies SYNC1 with an offset of 100k ns
|
||||
sync1_period: Duration::from_nanos(100_000),
|
||||
});
|
||||
} else {
|
||||
match subdevice.dc_support() {
|
||||
ethercrab::DcSupport::None => (),
|
||||
ethercrab::DcSupport::RefOnly => (),
|
||||
ethercrab::DcSupport::Bits64 => subdevice.set_dc_sync(DcSync::Sync0),
|
||||
ethercrab::DcSupport::Bits32 => subdevice.set_dc_sync(DcSync::Sync0),
|
||||
}
|
||||
}
|
||||
}
|
||||
// create devices
|
||||
let devices =
|
||||
devices_from_subdevices::<MAX_SUBDEVICES, PDI_LEN>(&mut group_preop, &maindevice)?;
|
||||
@@ -298,6 +345,7 @@ pub async fn setup_loop(
|
||||
},
|
||||
)
|
||||
.collect::<Vec<_>>();
|
||||
|
||||
let devices = device_identifications
|
||||
.into_iter()
|
||||
.zip(devices)
|
||||
@@ -356,7 +404,6 @@ pub async fn setup_loop(
|
||||
*/
|
||||
match (coupler.identity().vendor_id, coupler.identity().product_id) {
|
||||
(WAGO_750_354_VENDOR_ID, WAGO_750_354_PRODUCT_ID) => {
|
||||
has_dc = true;
|
||||
let r = Wago750_354::initialize_modules(coupler).await?;
|
||||
for module in r {
|
||||
if coupler.configured_address() == module.belongs_to_addr {
|
||||
@@ -380,7 +427,6 @@ pub async fn setup_loop(
|
||||
}
|
||||
}
|
||||
(IP20_EC_DI8_DO8_VENDOR_ID, IP20_EC_DI8_DO8_PRODUCT_ID) => {
|
||||
has_dc = true;
|
||||
let r = IP20EcDi8Do8::initialize_modules(coupler).await?;
|
||||
for module in r {
|
||||
if coupler.configured_address() == module.belongs_to_addr {
|
||||
@@ -407,19 +453,11 @@ pub async fn setup_loop(
|
||||
};
|
||||
drop(ethercat_meta_devices);
|
||||
|
||||
for subdevice in subdevices.iter() {
|
||||
// Hack so El5152 goes into OP
|
||||
if subdevice.name() == "EL5152" {
|
||||
has_dc = true;
|
||||
}
|
||||
}
|
||||
|
||||
// remove subdevice from devices tuple
|
||||
let devices = devices
|
||||
.iter()
|
||||
.map(|(device_identification, device, _)| (device_identification.clone(), device.clone()))
|
||||
.collect::<Vec<_>>();
|
||||
// Notify client via socketio
|
||||
|
||||
set_ethercat_devices::<MAX_SUBDEVICES, PDI_LEN>(
|
||||
&identified_device_identifications,
|
||||
@@ -433,72 +471,127 @@ pub async fn setup_loop(
|
||||
)
|
||||
.await?;
|
||||
|
||||
let group_safe = match group_preop.into_safe_op(&maindevice).await {
|
||||
Ok(group_op) => {
|
||||
tracing::info!("Group in Safe-OP state");
|
||||
group_op
|
||||
}
|
||||
Err(err) => Err(anyhow::anyhow!(
|
||||
"[{}::setup_loop] Failed to put group in Safe-OP state: {:?}",
|
||||
module_path!(),
|
||||
err
|
||||
))?,
|
||||
};
|
||||
// Avoid potential Sdo write Contention on the bus, by just waiting a bit
|
||||
smol::Timer::after(Duration::from_millis(4000)).await;
|
||||
let mut now = Instant::now();
|
||||
let start = Instant::now();
|
||||
let mut averages = Vec::new();
|
||||
for _ in 0..group_preop.len() {
|
||||
averages.push(ExponentialMovingAverage::new(64).unwrap());
|
||||
}
|
||||
let mut tick_interval = smol::Timer::interval(TICK_INTERVAL);
|
||||
|
||||
// TODO Make a more extensive init for the case of DC-Sync
|
||||
// Maybe we need multiple groups? like one DC group and one non dc sync group?
|
||||
// For now we just check if we use wago coupler or IP20
|
||||
if has_dc {
|
||||
for _ in 1..1000 {
|
||||
let res = group_safe.tx_rx_sync_system_time(&maindevice).await;
|
||||
match res {
|
||||
Ok(_) => (),
|
||||
Err(e) => tracing::error!(
|
||||
"[{}::setup_loop] Failed to sync dc time: {:?}",
|
||||
e,
|
||||
module_path!()
|
||||
),
|
||||
tracing::info!("Moving into PRE-OP with PDI");
|
||||
let group = group_preop.into_pre_op_pdi(&maindevice).await?;
|
||||
tracing::info!("Done. PDI available. Waiting for SubDevices to align");
|
||||
|
||||
loop {
|
||||
group
|
||||
.tx_rx_sync_system_time(&maindevice)
|
||||
.await
|
||||
.expect("TX/RX");
|
||||
|
||||
if now.elapsed() >= Duration::from_millis(25) {
|
||||
now = Instant::now();
|
||||
let mut max_deviation = 0;
|
||||
for (s1, ema) in group.iter(&maindevice).zip(averages.iter_mut()) {
|
||||
let diff = match s1
|
||||
.register_read::<u32>(RegisterAddress::DcSystemTimeDifference)
|
||||
.await
|
||||
{
|
||||
Ok(value) =>
|
||||
// The returned value is NOT in two's compliment, rather the upper bit specifies
|
||||
// whether the number in the remaining bits is odd or even, so we convert the
|
||||
// value to `i32` using that logic here.
|
||||
{
|
||||
let flag = 0b1u32 << 31;
|
||||
if value >= flag {
|
||||
// Strip off negative flag bit and negate value as normal
|
||||
-((value & !flag) as i32)
|
||||
} else {
|
||||
value as i32
|
||||
}
|
||||
}
|
||||
Err(Error::WorkingCounter { .. }) => 0,
|
||||
Err(e) => {
|
||||
return Err(anyhow::anyhow!("Failed to read DC system time: {:?}", e));
|
||||
}
|
||||
};
|
||||
|
||||
let ema_next = ema.next(diff as f64);
|
||||
max_deviation = max_deviation.max(ema_next.abs() as u32);
|
||||
}
|
||||
if max_deviation < 100 {
|
||||
tracing::info!("Clocks settled after {} ms", start.elapsed().as_millis());
|
||||
break;
|
||||
}
|
||||
}
|
||||
tick_interval.next().await;
|
||||
}
|
||||
|
||||
// Put group in operational state
|
||||
let group_op = match group_safe.into_op(&maindevice).await {
|
||||
Ok(group_op) => {
|
||||
tracing::info!("Group in OP state");
|
||||
group_op
|
||||
// SubDevice clocks are aligned. We can turn DC on now.
|
||||
let group = group
|
||||
.configure_dc_sync(
|
||||
&maindevice,
|
||||
DcConfiguration {
|
||||
// Start SYNC0 100ms in the future
|
||||
start_delay: Duration::from_millis(100),
|
||||
// SYNC0 period should be the same as the process data loop in most cases
|
||||
sync0_period: TICK_INTERVAL,
|
||||
// Send process data half way through cycle
|
||||
sync0_shift: TICK_INTERVAL / 2,
|
||||
},
|
||||
)
|
||||
.await?;
|
||||
|
||||
// State machine to handle transition to SafeOp with process data
|
||||
enum GroupState {
|
||||
PreOp(SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, PreOpPdi, HasDc>),
|
||||
SafeOp(SubDeviceGroup<MAX_SUBDEVICES, PDI_LEN, SafeOp, HasDc>),
|
||||
}
|
||||
|
||||
let mut group_container = Some(GroupState::PreOp(group));
|
||||
let mut tick = 0;
|
||||
|
||||
let group = loop {
|
||||
let now = Instant::now();
|
||||
match group_container.take().unwrap() {
|
||||
GroupState::PreOp(group) => {
|
||||
let res = group.tx_rx_dc(&maindevice).await.expect("TX/RX");
|
||||
if tick > 300 {
|
||||
let group = group.request_into_safe_op(&maindevice).await?;
|
||||
group_container = Some(GroupState::SafeOp(group));
|
||||
tracing::info!("Requested SAFE-OP");
|
||||
} else {
|
||||
group_container = Some(GroupState::PreOp(group));
|
||||
}
|
||||
|
||||
smol::Timer::at(now + res.extra.next_cycle_wait).await;
|
||||
}
|
||||
GroupState::SafeOp(group) => {
|
||||
let res = group.tx_rx_dc(&maindevice).await.expect("TX/RX");
|
||||
if res.all_safe_op() {
|
||||
tracing::info!("SAFE-OP");
|
||||
break group;
|
||||
} else {
|
||||
group_container = Some(GroupState::SafeOp(group));
|
||||
}
|
||||
smol::Timer::at(now + res.extra.next_cycle_wait).await;
|
||||
}
|
||||
}
|
||||
Err(err) => Err(anyhow::anyhow!(
|
||||
"[{}::setup_loop] Failed to put group in OP state: {:?}",
|
||||
module_path!(),
|
||||
err
|
||||
))?,
|
||||
tick += 1;
|
||||
};
|
||||
|
||||
{
|
||||
// Notify client via socketio
|
||||
let app_state_clone = app_state.clone();
|
||||
let main_namespace = &mut app_state_clone
|
||||
.socketio_setup
|
||||
.namespaces
|
||||
.write()
|
||||
.await
|
||||
.main_namespace;
|
||||
let event = EthercatDevicesEventBuilder()
|
||||
.build(app_state_clone.clone())
|
||||
.await;
|
||||
main_namespace.emit(MainNamespaceEvents::EthercatDevicesEvent(event));
|
||||
}
|
||||
|
||||
let res = start_dnsmasq();
|
||||
match res {
|
||||
Ok(o) => o,
|
||||
Err(e) => tracing::error!("Failed to start dnsmasq: {:?}", e),
|
||||
};
|
||||
let group_op = group
|
||||
.request_into_op(&maindevice)
|
||||
.await
|
||||
.expect("SAFE-OP -> OP");
|
||||
tracing::info!("Started Transition to OP");
|
||||
|
||||
Ok(EthercatSetup {
|
||||
devices,
|
||||
group: group_op,
|
||||
maindevice,
|
||||
all_operational: false,
|
||||
})
|
||||
}
|
||||
+67
-41
@@ -4,6 +4,8 @@ use bitvec::prelude::*;
|
||||
use control_core::realtime::set_core_affinity;
|
||||
#[cfg(not(feature = "development-build"))]
|
||||
use control_core::realtime::set_realtime_priority;
|
||||
use ethercrab::TxRxResponse;
|
||||
use ethercrab::subdevice_group::CycleInfo;
|
||||
use machines::Machine;
|
||||
use machines::machine_identification::write_machine_device_identification;
|
||||
use smol::channel::Receiver;
|
||||
@@ -159,55 +161,79 @@ pub fn start_loop_thread(
|
||||
}
|
||||
|
||||
pub async fn copy_ethercat_inputs(
|
||||
ethercat_setup: Option<&EthercatSetup>,
|
||||
ethercat_setup: Option<&mut EthercatSetup>,
|
||||
) -> Result<(), anyhow::Error> {
|
||||
// only if we have an ethercat setup
|
||||
// - tx/rx cycle
|
||||
// - copy inputs to devices
|
||||
if let Some(ethercat_setup) = ethercat_setup {
|
||||
ethercat_setup
|
||||
.group
|
||||
.tx_rx(ðercat_setup.maindevice)
|
||||
.await?;
|
||||
if ethercat_setup.all_operational {
|
||||
ethercat_setup
|
||||
.group
|
||||
.tx_rx_dc(ðercat_setup.maindevice)
|
||||
.await?;
|
||||
|
||||
// copy inputs to devices
|
||||
for (i, subdevice) in ethercat_setup
|
||||
.group
|
||||
.iter(ðercat_setup.maindevice)
|
||||
.enumerate()
|
||||
{
|
||||
// retrieve inputs
|
||||
let input = subdevice.inputs_raw();
|
||||
let input_bits = input.view_bits::<Lsb0>();
|
||||
// copy inputs to devices
|
||||
for (i, subdevice) in ethercat_setup
|
||||
.group
|
||||
.iter(ðercat_setup.maindevice)
|
||||
.enumerate()
|
||||
{
|
||||
// retrieve inputs
|
||||
let input = subdevice.inputs_raw();
|
||||
let input_bits = input.view_bits::<Lsb0>();
|
||||
|
||||
// get device
|
||||
let mut device = ethercat_setup.devices[i].1.as_ref().write().await;
|
||||
// get device
|
||||
let mut device = ethercat_setup.devices[i].1.as_ref().write().await;
|
||||
|
||||
// check if the device is used
|
||||
if !device.is_used() {
|
||||
// if the device is not used, we skip it
|
||||
continue;
|
||||
// check if the device is used
|
||||
if !device.is_used() {
|
||||
// if the device is not used, we skip it
|
||||
continue;
|
||||
}
|
||||
|
||||
// put inputs into device
|
||||
device.input_checked(input_bits).map_err(|e| {
|
||||
anyhow::anyhow!(
|
||||
"[{}::loop_once] SubDevice with index {} failed to copy inputs\n{:?}",
|
||||
module_path!(),
|
||||
i,
|
||||
e
|
||||
)
|
||||
})?;
|
||||
|
||||
// post process inputs
|
||||
device.input_post_process().map_err(|e| {
|
||||
anyhow::anyhow!(
|
||||
"[{}::loop_once] SubDevice with index {} failed to copy post_process\n{:?}",
|
||||
module_path!(),
|
||||
i,
|
||||
e
|
||||
)
|
||||
})?;
|
||||
}
|
||||
return Ok(());
|
||||
} else {
|
||||
loop {
|
||||
let now = Instant::now();
|
||||
let response @ TxRxResponse {
|
||||
working_counter: _wkc,
|
||||
extra:
|
||||
CycleInfo {
|
||||
next_cycle_wait, ..
|
||||
},
|
||||
..
|
||||
} = ethercat_setup
|
||||
.group
|
||||
.tx_rx_dc(ðercat_setup.maindevice)
|
||||
.await
|
||||
.expect("TX/RX");
|
||||
if response.all_op() {
|
||||
ethercat_setup.all_operational = true;
|
||||
break;
|
||||
}
|
||||
smol::Timer::at(now + next_cycle_wait).await;
|
||||
}
|
||||
|
||||
// put inputs into device
|
||||
device.input_checked(input_bits).map_err(|e| {
|
||||
anyhow::anyhow!(
|
||||
"[{}::loop_once] SubDevice with index {} failed to copy inputs\n{:?}",
|
||||
module_path!(),
|
||||
i,
|
||||
e
|
||||
)
|
||||
})?;
|
||||
|
||||
// post process inputs
|
||||
device.input_post_process().map_err(|e| {
|
||||
anyhow::anyhow!(
|
||||
"[{}::loop_once] SubDevice with index {} failed to copy post_process\n{:?}",
|
||||
module_path!(),
|
||||
i,
|
||||
e
|
||||
)
|
||||
})?;
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
@@ -276,7 +302,7 @@ pub fn loop_once<'maindevice>(inputs: &mut RtLoopInputs<'_>) -> Result<(), anyho
|
||||
.unwrap()
|
||||
.cycle_start();
|
||||
|
||||
let res = smol::block_on(copy_ethercat_inputs(inputs.ethercat_setup.as_deref()));
|
||||
let res = smol::block_on(copy_ethercat_inputs(inputs.ethercat_setup.as_deref_mut()));
|
||||
match res {
|
||||
Ok(_) => (),
|
||||
Err(e) => {
|
||||
|
||||
+25
-1
@@ -2,6 +2,7 @@ use crate::{
|
||||
metrics::collector::{RuntimeMetricsConfig, spawn_runtime_metrics_sampler},
|
||||
socketio::main_namespace::machines_event::MachineObj,
|
||||
};
|
||||
use control_core::socketio::namespace::NamespaceCacheingLogic;
|
||||
use machines::{
|
||||
AsyncThreadMessage, MachineConnection, MachineNewHardware, MachineNewHardwareSerial,
|
||||
MachineNewParams, SerialDevice, SerialDeviceIdentification, SerialDeviceNew,
|
||||
@@ -14,6 +15,9 @@ use machines::{
|
||||
serial::{devices::laser::Laser, init::SerialDetection},
|
||||
winder2::api::GenericEvent,
|
||||
};
|
||||
use socketio::main_namespace::{
|
||||
MainNamespaceEvents, ethercat_devices_event::EthercatDevicesEventBuilder,
|
||||
};
|
||||
#[cfg(feature = "development-build")]
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use utils::start_dnsmasq;
|
||||
@@ -145,8 +149,28 @@ pub async fn start_interface_discovery(
|
||||
Ok(setup) => {
|
||||
let _ = sender.send(HotThreadMessage::AddEtherCatSetup(setup)).await;
|
||||
tracing::info!("Successfully initialized EtherCAT devices");
|
||||
}
|
||||
|
||||
{
|
||||
// Notify client via socketio
|
||||
let app_state_clone = app_state.clone();
|
||||
let main_namespace = &mut app_state_clone
|
||||
.socketio_setup
|
||||
.namespaces
|
||||
.write()
|
||||
.await
|
||||
.main_namespace;
|
||||
let event = EthercatDevicesEventBuilder()
|
||||
.build(app_state_clone.clone())
|
||||
.await;
|
||||
main_namespace.emit(MainNamespaceEvents::EthercatDevicesEvent(event));
|
||||
}
|
||||
|
||||
let res = start_dnsmasq();
|
||||
match res {
|
||||
Ok(o) => o,
|
||||
Err(e) => tracing::error!("Failed to start dnsmasq: {:?}", e),
|
||||
};
|
||||
}
|
||||
Err(e) => {
|
||||
tracing::error!(
|
||||
"[{}::main] Failed to initialize EtherCAT network \n{:?}",
|
||||
|
||||
Reference in new issue
Block a user