21b2d3bf99
Re-implementation of rustbootd's ecat_el6695_* examples as a single binary with fixes found in review and on hardware: - DC-follow PLL hardened against period-2 hunting: slew-limited anchor (+/-50us/cycle), bistable-trap snap re-anchor, re-prime on stale deadline - drift-free absolute-grid ticker mode; probe mode for timestamp forensics - bounded-memory online stats (histograms), graceful SIGINT/SIGTERM shutdown with full report, error-streak abort - timestamp plausibility filter comparing against the previous raw sample (avoids the deadlock after a startup outlier) - XFC scope waveform options: --el2202 (with --el2202-dual), --el2262, --el1252 latch timestamp readback with per-channel edge statistics - register access unified in regs.rs: named bit constants everywhere, read-modify-write for enable/activation bytes - vendored patched ethercrab 0.7.1 (sdo_write_complete, send_raw_coe) Verified on J1900 (PREEMPT_RT 6.6.135): 600k cycles/600s exact 1 kHz, tx/rx zero errors, phase_err p50=164us std=5us; EL2202<->EL1252 loopback edge interval mean 2000.24us std=24.65us.
338 lines
10 KiB
Plaintext
338 lines
10 KiB
Plaintext
//! Configure a Nanotec C5-E EtherCAT stepper drive.
|
|
//!
|
|
//! Motor used for testing is AS5918M2804-E with 500PPR encoder.
|
|
|
|
use anyhow::Context;
|
|
use env_logger::Env;
|
|
use ethercrab::{
|
|
ds402::{Ds402, Ds402Sm, StatusWord},
|
|
std::{ethercat_now, tx_rx_task},
|
|
MainDevice, MainDeviceConfig, PduStorage, Timeouts,
|
|
};
|
|
use std::{
|
|
array::from_ref,
|
|
sync::{
|
|
atomic::{AtomicBool, Ordering},
|
|
Arc,
|
|
},
|
|
time::Duration,
|
|
};
|
|
use tokio::time::MissedTickBehavior;
|
|
|
|
/// Maximum number of SubDevices that can be stored.
|
|
const MAX_SUBDEVICES: usize = 16;
|
|
/// Maximum PDU data payload size - set this to the max PDI size or higher.
|
|
const MAX_PDU_DATA: usize = PduStorage::element_size(1100);
|
|
/// Maximum number of EtherCAT frames that can be in flight at any one time.
|
|
const MAX_FRAMES: usize = 16;
|
|
/// Maximum total PDI length.
|
|
const PDI_LEN: usize = 64;
|
|
|
|
static PDU_STORAGE: PduStorage<MAX_FRAMES, MAX_PDU_DATA> = PduStorage::new();
|
|
|
|
/// C5-E manual page 127 "Error number"
|
|
#[derive(ethercrab::EtherCrabWireRead, Debug)]
|
|
#[allow(unused)]
|
|
#[wire(bytes = 4)]
|
|
struct C5Error {
|
|
#[wire(bytes = 2)]
|
|
pub code: u16,
|
|
#[wire(bytes = 1)]
|
|
pub class: u8,
|
|
#[wire(bytes = 1)]
|
|
pub number: u8,
|
|
}
|
|
|
|
#[tokio::main]
|
|
async fn main() -> anyhow::Result<()> {
|
|
env_logger::Builder::from_env(Env::default().default_filter_or("info")).init();
|
|
|
|
let interface = std::env::args()
|
|
.nth(1)
|
|
.expect("Provide network interface as first argument.");
|
|
|
|
log::info!("Starting C5-E demo...");
|
|
log::info!("Run with RUST_LOG=ethercrab=debug or =trace for debug information");
|
|
|
|
let (tx, rx, pdu_loop) = PDU_STORAGE.try_split().expect("can only split once");
|
|
|
|
let maindevice = Arc::new(MainDevice::new(
|
|
pdu_loop,
|
|
Timeouts {
|
|
wait_loop_delay: Duration::from_millis(2),
|
|
mailbox_response: Duration::from_millis(1000),
|
|
..Default::default()
|
|
},
|
|
MainDeviceConfig::default(),
|
|
));
|
|
|
|
tokio::spawn(tx_rx_task(&interface, tx, rx).expect("spawn TX/RX task"));
|
|
|
|
let mut group = maindevice
|
|
.init_single_group::<MAX_SUBDEVICES, PDI_LEN>(ethercat_now)
|
|
.await
|
|
.expect("Init");
|
|
|
|
for subdevice in group.iter(&maindevice) {
|
|
// Assuming all connected SubDevices are C5-Es here
|
|
|
|
// Manual section 4.8 Setting the motor data
|
|
// 1.8deg step, so 50 pole pairs
|
|
subdevice
|
|
.sdo_write(0x2030, 0, 50u32)
|
|
.await
|
|
.context("pole pairs")?;
|
|
// Max motor current in mA.
|
|
subdevice
|
|
.sdo_write(0x2031, 0, 1000u32)
|
|
.await
|
|
.context("max current")?;
|
|
// Rated motor current in mA
|
|
subdevice
|
|
.sdo_write(0x6075, 0, 2820u32)
|
|
.await
|
|
.context("rated currnet")?;
|
|
// Max motor current, % of rated current in milli-percent, i.e. 1000 is 100%
|
|
subdevice
|
|
.sdo_write(0x6073, 0, 1000u16)
|
|
.await
|
|
.context("current %")?;
|
|
// Max motor current max duration in ms
|
|
subdevice
|
|
.sdo_write(0x203b, 02, 100u32)
|
|
.await
|
|
.context("max current duration")?;
|
|
// Motor type: stepper
|
|
subdevice
|
|
.sdo_write(0x3202, 00, 0x08u32)
|
|
.await
|
|
.context("set motor type")?;
|
|
// Test motor has 500ppr incremental encoder, differential
|
|
subdevice
|
|
.sdo_write(0x2059, 00, 0x0u32)
|
|
.await
|
|
.context("encoder kind")?;
|
|
// Set velocity unit to RPM (factory default)
|
|
subdevice
|
|
.sdo_write(0x60a9, 00, 0x00B44700u32)
|
|
.await
|
|
.context("velocity unit RPM")?;
|
|
|
|
// CSV described a bit better in section 7.6.2.2 Related Objects of the manual
|
|
subdevice
|
|
.sdo_write_array(
|
|
0x1600,
|
|
&[
|
|
// Control word, u16
|
|
// NOTE: The lower word specifies the field length
|
|
0x6040_0010u32,
|
|
// Target velocity, i32
|
|
0x60ff_0020,
|
|
],
|
|
)
|
|
.await?;
|
|
|
|
// The above code is equivalent to:
|
|
// subdevice.sdo_write(0x1600, 0, 0u8).await?;
|
|
// subdevice.sdo_write(0x1600, 1, 0x6040_0010u32).await?;
|
|
// subdevice.sdo_write(0x1600, 2, 0x60ff_0020u32).await?;
|
|
// subdevice.sdo_write(0x1600, 0, 2u8).await?;
|
|
|
|
subdevice
|
|
.sdo_write_array(
|
|
0x1a00,
|
|
&[
|
|
// Status word, u16
|
|
0x6041_0010u32,
|
|
// Actual position, i32
|
|
0x6064_0020,
|
|
// Actual velocity, i32
|
|
0x606c_0020,
|
|
],
|
|
)
|
|
.await?;
|
|
|
|
// Above code is equivalent to:
|
|
// subdevice.sdo_write(0x1a00, 0, 0u8).await?;
|
|
// subdevice.sdo_write(0x1a00, 1, 0x6041_0010u32).await?;
|
|
// subdevice.sdo_write(0x1a00, 2, 0x6064_0020u32).await?;
|
|
// subdevice.sdo_write(0x1a00, 3, 0x606c_0020u32).await?;
|
|
// subdevice.sdo_write(0x1a00, 0, 0x03u8).await?;
|
|
|
|
subdevice
|
|
.sdo_write_array(0x1c12, from_ref(&0x1600u16))
|
|
.await?;
|
|
subdevice
|
|
.sdo_write_array(0x1c13, from_ref(&0x1a00u16))
|
|
.await?;
|
|
|
|
// Above code is equivalent to:
|
|
// subdevice.sdo_write(0x1c12, 0, 0u8).await?;
|
|
// subdevice.sdo_write(0x1c12, 1, 0x1600u16).await?;
|
|
// subdevice.sdo_write(0x1c12, 0, 1u8).await?;
|
|
// subdevice.sdo_write(0x1c13, 0, 0u8).await?;
|
|
// subdevice.sdo_write(0x1c13, 1, 0x1a00u16).await?;
|
|
// subdevice.sdo_write(0x1c13, 0, 1u8).await?;
|
|
|
|
// Opmode - Cyclic Synchronous Position
|
|
// subdevice.write_sdo(0x6060, 0, 0x08).await?;
|
|
// Opmode - Cyclic Synchronous Velocity
|
|
subdevice.sdo_write(0x6060, 0, 0x09u8).await?;
|
|
}
|
|
|
|
let mut group = group.into_op(&maindevice).await.expect("PRE-OP -> OP");
|
|
|
|
log::info!("SubDevices moved to OP state");
|
|
|
|
log::info!("Discovered {} SubDevices", group.len());
|
|
|
|
for subdevice in group.iter(&maindevice) {
|
|
let (i, o) = subdevice.io_raw();
|
|
|
|
log::info!(
|
|
"-> SubDevice {:#06x} {} inputs: {} bytes, outputs: {} bytes",
|
|
subdevice.configured_address(),
|
|
subdevice.name(),
|
|
i.len(),
|
|
o.len()
|
|
);
|
|
}
|
|
|
|
// Run twice to prime PDI
|
|
group.tx_rx(&maindevice).await.expect("TX/RX");
|
|
|
|
// Read cycle time from servo drive
|
|
let cycle_time = {
|
|
let subdevice = group.subdevice(&maindevice, 0).unwrap();
|
|
|
|
let base = subdevice.sdo_read::<u8>(0x60c2, 1).await?;
|
|
let x10 = subdevice.sdo_read::<i8>(0x60c2, 2).await?;
|
|
|
|
let base = f32::from(base);
|
|
let x10 = 10.0f32.powi(i32::from(x10));
|
|
|
|
let cycle_time_ms = (base * x10) * 1000.0;
|
|
|
|
Duration::from_millis(unsafe { cycle_time_ms.round().to_int_unchecked() })
|
|
};
|
|
|
|
log::info!("Cycle time: {} ms", cycle_time.as_millis());
|
|
|
|
let mut cyclic_interval = tokio::time::interval(cycle_time);
|
|
cyclic_interval.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
|
|
|
let subdevice = group.subdevice(&maindevice, 0).expect("No servo!");
|
|
let mut servo = Ds402Sm::new(Ds402::new(subdevice).expect("Failed to gather DS402"));
|
|
|
|
let mut velocity: i32 = 0;
|
|
|
|
let accel = 1;
|
|
let max_vel = 100;
|
|
|
|
let term = Arc::new(AtomicBool::new(false));
|
|
signal_hook::flag::register(signal_hook::consts::SIGINT, Arc::clone(&term))
|
|
.expect("Register hook");
|
|
|
|
loop {
|
|
group.tx_rx(&maindevice).await.expect("TX/RX");
|
|
|
|
if servo.tick() {
|
|
let status = servo.status_word();
|
|
let (i, o) = servo.subdevice().io_raw_mut();
|
|
|
|
let (pos, vel) = {
|
|
let pos = i32::from_le_bytes(i[2..=5].try_into().unwrap());
|
|
let vel = i32::from_le_bytes(i[6..=9].try_into().unwrap());
|
|
|
|
(pos, vel)
|
|
};
|
|
|
|
println!(
|
|
"Position: {pos}, velocity: {vel}, status: {status:?} | {:?}",
|
|
o
|
|
);
|
|
|
|
let vel_cmd = &mut o[2..=5];
|
|
|
|
vel_cmd.copy_from_slice(&velocity.to_le_bytes());
|
|
|
|
if status.contains(StatusWord::FAULT) {
|
|
let sl = servo.subdevice();
|
|
|
|
let num_errors = sl
|
|
.sdo_read::<u8>(0x1003, 0)
|
|
.await
|
|
.context("Read error count")?;
|
|
|
|
log::error!("Fault! ({})", num_errors);
|
|
|
|
for idx in 1..=num_errors {
|
|
let code = sl
|
|
.sdo_read::<C5Error>(0x1003, idx)
|
|
.await
|
|
.context("Read error code")?;
|
|
|
|
log::error!("--> {:?}", code);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
// Normal operation: accelerate up to max speed
|
|
if !term.load(Ordering::Relaxed) {
|
|
if vel < max_vel {
|
|
velocity += accel;
|
|
}
|
|
}
|
|
// Slow down to a stop when Ctrl + C is pressed
|
|
else if vel > 0 {
|
|
velocity -= accel;
|
|
}
|
|
// Deceleration is done, we can now exit this loop
|
|
else {
|
|
log::info!("Stopping...");
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
cyclic_interval.tick().await;
|
|
}
|
|
|
|
log::info!("Servo stopped, shutting drive down");
|
|
|
|
loop {
|
|
group.tx_rx(&maindevice).await.expect("TX/RX");
|
|
|
|
if servo.tick_shutdown() {
|
|
break;
|
|
}
|
|
|
|
let status = servo.status_word();
|
|
let (i, o) = servo.subdevice().io_raw_mut();
|
|
|
|
// In fault state, so don't bother trying to shut down gracefully.
|
|
if status.contains(StatusWord::FAULT) {
|
|
break;
|
|
}
|
|
|
|
let (pos, vel) = {
|
|
let pos = i32::from_le_bytes(i[2..=5].try_into().unwrap());
|
|
let vel = i32::from_le_bytes(i[6..=9].try_into().unwrap());
|
|
|
|
(pos, vel)
|
|
};
|
|
|
|
println!(
|
|
"Position: {pos}, velocity: {vel}, status: {status:?} | {:?}",
|
|
o
|
|
);
|
|
|
|
cyclic_interval.tick().await;
|
|
}
|
|
|
|
log::info!("Drive is shut down");
|
|
|
|
Ok(())
|
|
}
|