use std::collections::HashMap;
use crate::data::error::{DarraError, Result};
use crate::data::types::SDOError;
use crate::utils::ffi;
use std::os::raw::c_int;
#[derive(Debug, Clone)]
pub struct PdoMapEntry {
pub od_index: u16,
pub sub_index: u8,
pub bit_length: u8,
pub byte_offset: u32,
pub bit_offset: u8,
pub is_output: bool,
}
impl PdoMapEntry {
pub fn byte_size(&self) -> u32 {
(self.bit_length as u32 + 7) / 8
}
}
pub struct Cia402PdoMap {
master_index: u16,
slave_index: u16,
pub(crate) output_map: HashMap<(u16, u8), PdoMapEntry>,
pub(crate) input_map: HashMap<(u16, u8), PdoMapEntry>,
out_ptr: *mut u8,
out_size: i32,
in_ptr: *mut u8,
in_size: i32,
}
unsafe impl Send for Cia402PdoMap {}
unsafe impl Sync for Cia402PdoMap {}
impl Cia402PdoMap {
pub fn scan(master_index: u16, slave_index: u16) -> Result<Self> {
let mut out_size: c_int = 0;
let mut out_ptr: *mut u8 = std::ptr::null_mut();
let mut in_size: c_int = 0;
let mut in_ptr: *mut u8 = std::ptr::null_mut();
let io_ok = unsafe {
ffi::GetIO(master_index, slave_index,
&mut out_size, &mut out_ptr,
&mut in_size, &mut in_ptr)
};
if io_ok == 0 {
return Err(DarraError::PdoFailed("获取从站 IOmap 失败".into()));
}
let mut output_map = HashMap::new();
let mut input_map = HashMap::new();
let rxpdo_count = Self::read_u8_sdo(master_index, slave_index, 0x1C12, 0)
.unwrap_or(0);
let mut out_byte_offset: u32 = 0;
for sm_idx in 1..=rxpdo_count {
let pdo_idx = match Self::read_u16_sdo(master_index, slave_index, 0x1C12, sm_idx) {
Ok(v) => v,
Err(_) => continue,
};
let map_count = Self::read_u8_sdo(master_index, slave_index, pdo_idx, 0)
.unwrap_or(0);
for map_sub in 1..=map_count {
let map_val = match Self::read_u32_sdo(master_index, slave_index, pdo_idx, map_sub) {
Ok(v) => v,
Err(_) => continue,
};
let obj_idx = ((map_val >> 16) & 0xFFFF) as u16;
let obj_sub = ((map_val >> 8) & 0xFF) as u8;
let bit_len = (map_val & 0xFF) as u8;
if obj_idx == 0 || bit_len == 0 { continue; }
let entry = PdoMapEntry {
od_index: obj_idx,
sub_index: obj_sub,
bit_length: bit_len,
byte_offset: out_byte_offset,
bit_offset: 0,
is_output: true,
};
out_byte_offset += (bit_len as u32 + 7) / 8;
output_map.insert((obj_idx, obj_sub), entry);
}
}
let txpdo_count = Self::read_u8_sdo(master_index, slave_index, 0x1C13, 0)
.unwrap_or(0);
let mut in_byte_offset: u32 = 0;
for sm_idx in 1..=txpdo_count {
let pdo_idx = match Self::read_u16_sdo(master_index, slave_index, 0x1C13, sm_idx) {
Ok(v) => v,
Err(_) => continue,
};
let map_count = Self::read_u8_sdo(master_index, slave_index, pdo_idx, 0)
.unwrap_or(0);
for map_sub in 1..=map_count {
let map_val = match Self::read_u32_sdo(master_index, slave_index, pdo_idx, map_sub) {
Ok(v) => v,
Err(_) => continue,
};
let obj_idx = ((map_val >> 16) & 0xFFFF) as u16;
let obj_sub = ((map_val >> 8) & 0xFF) as u8;
let bit_len = (map_val & 0xFF) as u8;
if obj_idx == 0 || bit_len == 0 { continue; }
let entry = PdoMapEntry {
od_index: obj_idx,
sub_index: obj_sub,
bit_length: bit_len,
byte_offset: in_byte_offset,
bit_offset: 0,
is_output: false,
};
in_byte_offset += (bit_len as u32 + 7) / 8;
input_map.insert((obj_idx, obj_sub), entry);
}
}
Ok(Self {
master_index,
slave_index,
output_map,
input_map,
out_ptr,
out_size,
in_ptr,
in_size,
})
}
pub fn read_u32(&self, od_index: u16, sub_index: u8) -> Result<u32> {
if let Some(entry) = self.input_map.get(&(od_index, sub_index)) {
let off = entry.byte_offset as usize;
let sz = entry.byte_size() as usize;
if !self.in_ptr.is_null() && off + sz <= self.in_size as usize {
let val = unsafe {
let ptr = self.in_ptr.add(off);
match sz {
1 => *ptr as u32,
2 => u16::from_le_bytes([*ptr, *ptr.add(1)]) as u32,
4 => u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)]),
_ => 0,
}
};
return Ok(val);
}
}
Self::read_u32_sdo(self.master_index, self.slave_index, od_index, sub_index)
}
pub fn write_u32(&self, od_index: u16, sub_index: u8, value: u32) -> Result<()> {
if let Some(entry) = self.output_map.get(&(od_index, sub_index)) {
let off = entry.byte_offset as usize;
let sz = entry.byte_size() as usize;
if !self.out_ptr.is_null() && off + sz <= self.out_size as usize {
unsafe {
let ptr = self.out_ptr.add(off);
let bytes = value.to_le_bytes();
std::ptr::copy_nonoverlapping(bytes.as_ptr(), ptr, sz.min(4));
}
return Ok(());
}
}
let bytes = value.to_le_bytes();
let ok = unsafe {
ffi::SDOwrite_raw(self.master_index, self.slave_index, od_index, sub_index,
0, bytes.as_ptr(), 4)
};
if ok != 0 { Ok(()) } else { Err(DarraError::SdoWriteFailed { index: od_index, subindex: sub_index, abort_code: None }) }
}
pub fn read_u16(&self, od_index: u16, sub_index: u8) -> Result<u16> {
Ok(self.read_u32(od_index, sub_index)? as u16)
}
pub fn write_u16(&self, od_index: u16, sub_index: u8, value: u16) -> Result<()> {
if let Some(entry) = self.output_map.get(&(od_index, sub_index)) {
let off = entry.byte_offset as usize;
if !self.out_ptr.is_null() && off + 2 <= self.out_size as usize {
unsafe {
let bytes = value.to_le_bytes();
std::ptr::copy_nonoverlapping(bytes.as_ptr(), self.out_ptr.add(off), 2);
}
return Ok(());
}
}
let bytes = value.to_le_bytes();
let ok = unsafe {
ffi::SDOwrite_raw(self.master_index, self.slave_index, od_index, sub_index,
0, bytes.as_ptr(), 2)
};
if ok != 0 { Ok(()) } else { Err(DarraError::SdoWriteFailed { index: od_index, subindex: sub_index, abort_code: None }) }
}
pub fn output_entries(&self) -> impl Iterator<Item = &PdoMapEntry> {
self.output_map.values()
}
pub fn input_entries(&self) -> impl Iterator<Item = &PdoMapEntry> {
self.input_map.values()
}
pub fn is_output_mapped(&self, od_index: u16, sub_index: u8) -> bool {
self.output_map.contains_key(&(od_index, sub_index))
}
pub fn is_input_mapped(&self, od_index: u16, sub_index: u8) -> bool {
self.input_map.contains_key(&(od_index, sub_index))
}
fn read_u8_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u8> {
let mut size: c_int = 0;
let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
if ptr.is_null() || size < 1 {
if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
}
let val = unsafe { *ptr };
unsafe { ffi::FreeMemory(ptr as *mut _) };
Ok(val)
}
fn read_u16_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u16> {
let mut size: c_int = 0;
let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
if ptr.is_null() || size < 2 {
if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
}
let val = unsafe { u16::from_le_bytes([*ptr, *ptr.add(1)]) };
unsafe { ffi::FreeMemory(ptr as *mut _) };
Ok(val)
}
fn read_u32_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u32> {
let mut size: c_int = 0;
let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
if ptr.is_null() || size < 4 {
if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
}
let val = unsafe {
u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)])
};
unsafe { ffi::FreeMemory(ptr as *mut _) };
Ok(val)
}
}
impl std::fmt::Debug for Cia402PdoMap {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
f.debug_struct("Cia402PdoMap")
.field("master_index", &self.master_index)
.field("slave_index", &self.slave_index)
.field("output_entries", &self.output_map.len())
.field("input_entries", &self.input_map.len())
.finish()
}
}
pub const OD_CONTROLWORD: u16 = 0x6040;
pub const OD_STATUSWORD: u16 = 0x6041;
pub const OD_MODES_OF_OPERATION: u16 = 0x6060;
pub const OD_MODES_OF_OPERATION_DISPLAY: u16 = 0x6061;
pub const OD_TARGET_POSITION: u16 = 0x607A;
pub const OD_POSITION_ACTUAL: u16 = 0x6064;
pub const OD_TARGET_VELOCITY: u16 = 0x60FF;
pub const OD_VELOCITY_ACTUAL: u16 = 0x606C;
pub const OD_TARGET_TORQUE: u16 = 0x6071;
pub const OD_TORQUE_ACTUAL: u16 = 0x6077;
pub const OD_MAX_PROFILE_VELOCITY: u16 = 0x6080;
pub const OD_PROFILE_VELOCITY: u16 = 0x6081;
pub const OD_PROFILE_ACCELERATION: u16 = 0x6083;
pub const OD_PROFILE_DECELERATION: u16 = 0x6084;
pub const OD_QUICK_STOP_DECELERATION: u16 = 0x6085;
pub const OD_HOMING_METHOD: u16 = 0x6098;
pub const OD_HOMING_SPEEDS: u16 = 0x6099;
pub const OD_HOME_OFFSET: u16 = 0x607C;
pub const OD_TORQUE_OFFSET: u16 = 0x60B2;
pub const OD_SOFTWARE_POSITION_LIMIT: u16 = 0x607D;
pub const OD_SUPPORTED_DRIVE_MODES: u16 = 0x6502;
pub const OD_POLARITY: u16 = 0x607E;
pub const OD_MOTION_PROFILE_TYPE: u16 = 0x6086;
pub const OD_TOUCH_PROBE_FUNCTION: u16 = 0x60B8;
pub const OD_TOUCH_PROBE_STATUS: u16 = 0x60B9;
pub const OD_TOUCH_PROBE_POS_EDGE: u16 = 0x60BA;
pub const OD_TOUCH_PROBE_NEG_EDGE: u16 = 0x60BB;
pub const OD_HOMING_ACCELERATION: u16 = 0x609A;
pub const OD_DIGITAL_INPUTS: u16 = 0x60FD;
pub const OD_DIGITAL_OUTPUTS: u16 = 0x60FE;
pub const OD_MAX_TORQUE: u16 = 0x6072;
pub const OD_MOTOR_RATED_TORQUE: u16 = 0x6076;
pub const OD_POSITION_OFFSET: u16 = 0x60B0;
pub const OD_VELOCITY_OFFSET: u16 = 0x60B1;
pub const OD_INTERPOLATION_TIME_PERIOD: u16 = 0x60C2;
pub const OD_QUICK_STOP_OPTION_CODE: u16 = 0x605A;
pub const OD_POSITIVE_TORQUE_LIMIT: u16 = 0x60E0;
pub const OD_NEGATIVE_TORQUE_LIMIT: u16 = 0x60E1;
pub const OD_SUPPORTED_HOMING_METHODS: u16 = 0x60E3;
pub const OD_TXPDO_DATA_INVALID: u16 = 0x603E;
pub const OD_SYNCHRONIZATION_SETTINGS: u16 = 0x60D9;
pub const OD_DRIVE_SYNC_STATUS: u16 = 0x60DA;
pub const OD_ERROR_CODE: u16 = 0x603F;
pub const OD_DRIVE_DATA: u16 = 0x6510;
pub const CW_SHUTDOWN: u16 = 0x06;
pub const CW_SWITCH_ON: u16 = 0x07;
pub const CW_ENABLE_OPERATION: u16 = 0x0F;
pub const CW_DISABLE_VOLTAGE: u16 = 0x00;
pub const CW_QUICK_STOP: u16 = 0x02;
pub const CW_FAULT_RESET: u16 = 0x80;
pub const CW_HALT: u16 = 0x0100;
pub const SW_READY_TO_SWITCH_ON: u16 = 0x0001;
pub const SW_SWITCHED_ON: u16 = 0x0002;
pub const SW_OPERATION_ENABLED: u16 = 0x0004;
pub const SW_FAULT: u16 = 0x0008;
pub const SW_VOLTAGE_ENABLED: u16 = 0x0010;
pub const SW_QUICK_STOP: u16 = 0x0020;
pub const SW_SWITCH_ON_DISABLED: u16 = 0x0040;
pub const SW_WARNING: u16 = 0x0080;
pub const SW_REMOTE: u16 = 0x0200;
pub const SW_TARGET_REACHED: u16 = 0x0400;
pub const SW_INTERNAL_LIMIT: u16 = 0x0800;
pub const SW_OP_MODE_SPECIFIC_1: u16 = 0x1000;
pub const SW_OP_MODE_SPECIFIC_2: u16 = 0x2000;
#[repr(u8)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum StateCiA402 {
NotReadyToSwitchOn = 0,
SwitchOnDisabled = 1,
ReadyToSwitchOn = 2,
SwitchedOn = 3,
OperationEnabled = 4,
QuickStopActive = 5,
FaultReactionActive = 6,
Fault = 7,
Unknown = 99,
}
impl StateCiA402 {
pub fn from_statusword(sw: u16) -> Self {
if (sw & SW_FAULT) != 0 {
if (sw & 0x0F) == 0x0F {
return Self::FaultReactionActive;
}
return Self::Fault;
}
let mask = sw & 0x006F;
match mask {
0x0000 => Self::NotReadyToSwitchOn,
0x0040 => Self::SwitchOnDisabled,
0x0021 => Self::ReadyToSwitchOn,
0x0023 => Self::SwitchedOn,
0x0027 => Self::OperationEnabled,
0x0007 => Self::QuickStopActive,
_ => Self::Unknown,
}
}
pub fn description(&self) -> &'static str {
match self {
Self::NotReadyToSwitchOn => "初始化中",
Self::SwitchOnDisabled => "驱动禁用",
Self::ReadyToSwitchOn => "准备就绪",
Self::SwitchedOn => "已开启",
Self::OperationEnabled => "运行使能",
Self::QuickStopActive => "快速停止",
Self::FaultReactionActive => "故障反应中",
Self::Fault => "故障",
Self::Unknown => "未知",
}
}
}
impl std::fmt::Display for StateCiA402 {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(f, "{}", self.description())
}
}
#[repr(i8)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ModeCiA402 {
PP = 1,
VL = 2,
PV = 3,
PT = 4,
HM = 6,
IP = 7,
CSP = 8,
CSV = 9,
CST = 10,
CSTCA = 11,
}
impl ModeCiA402 {
pub fn supported_bit(&self) -> Option<u32> {
match self {
Self::PP => Some(0),
Self::VL => Some(1),
Self::PV => Some(2),
Self::PT => Some(3),
Self::HM => Some(5),
Self::IP => Some(6),
Self::CSP => Some(7),
Self::CSV => Some(8),
Self::CST => Some(9),
Self::CSTCA => Some(10),
}
}
pub fn description(&self) -> &'static str {
match self {
Self::PP => "轮廓位置 (PP)",
Self::VL => "速度 (VL)",
Self::PV => "轮廓速度 (PV)",
Self::PT => "轮廓转矩 (PT)",
Self::HM => "回零 (HM)",
Self::IP => "插补位置 (IP)",
Self::CSP => "周期同步位置 (CSP)",
Self::CSV => "周期同步速度 (CSV)",
Self::CST => "周期同步转矩 (CST)",
Self::CSTCA => "周期同步转矩加速度 (CSTCA)",
}
}
}
impl std::fmt::Display for ModeCiA402 {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(f, "{}", self.description())
}
}
#[repr(u8)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Cia402ReadStatus {
NotAttempted = 0,
Success = 1,
NotSupported = 2,
ReadFailed = 3,
}
impl Cia402ReadStatus {
pub fn classify_abort(abort_code: u32) -> Self {
match SDOError::from_u32(abort_code) {
SDOError::ObjectDoesNotExist | SDOError::SubindexDoesNotExist => Self::NotSupported,
_ => Self::ReadFailed,
}
}
pub fn description(&self) -> &'static str {
match self {
Self::NotAttempted => "未读取",
Self::Success => "读取成功",
Self::NotSupported => "从站不支持",
Self::ReadFailed => "读取失败",
}
}
}
impl std::fmt::Display for Cia402ReadStatus {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(f, "{}", self.description())
}
}
#[derive(Debug, Clone, Copy)]
pub struct Cia402ReadResult<T> {
pub value: T,
pub status: Cia402ReadStatus,
pub abort_code: u32,
}
impl<T> Cia402ReadResult<T> {
pub fn new(value: T, status: Cia402ReadStatus, abort_code: u32) -> Self {
Self { value, status, abort_code }
}
pub fn ok(&self) -> bool {
self.status == Cia402ReadStatus::Success
}
pub fn not_supported(&self) -> bool {
self.status == Cia402ReadStatus::NotSupported
}
}
pub trait Cia402Readable: Copy + Default {
fn from_le_slice(data: &[u8]) -> Self;
}
macro_rules! impl_cia402_readable {
($($t:ty),+ $(,)?) => {$(
impl Cia402Readable for $t {
fn from_le_slice(data: &[u8]) -> Self {
const N: usize = std::mem::size_of::<$t>();
let mut buf = [0u8; N];
let n = data.len().min(N);
buf[..n].copy_from_slice(&data[..n]);
<$t>::from_le_bytes(buf)
}
}
)+};
}
impl_cia402_readable!(u8, i8, u16, i16, u32, i32, u64, i64);
pub struct CiA402Instance {
master_index: u16,
slave_index: u16,
pdo_map: Cia402PdoMap,
}
impl CiA402Instance {
pub fn new(master_index: u16, slave_index: u16, pdo_map: Cia402PdoMap) -> Self {
Self { master_index, slave_index, pdo_map }
}
pub fn slave_index(&self) -> u16 {
self.slave_index
}
pub fn pdo_map(&self) -> &Cia402PdoMap {
&self.pdo_map
}
pub fn initialize_pdo_offsets(&mut self) -> Result<()> {
self.pdo_map = Cia402PdoMap::scan(self.master_index, self.slave_index)?;
Ok(())
}
pub fn reset_pdo_offsets(&mut self) {
self.pdo_map.output_map.clear();
self.pdo_map.input_map.clear();
}
fn sdo_read_u16(&self, index: u16, sub: u8) -> u16 {
self.pdo_map.read_u16(index, sub).unwrap_or(0)
}
fn sdo_read_i32(&self, index: u16, sub: u8) -> i32 {
self.pdo_map.read_u32(index, sub).unwrap_or(0) as i32
}
fn sdo_read_u32(&self, index: u16, sub: u8) -> u32 {
self.pdo_map.read_u32(index, sub).unwrap_or(0)
}
fn sdo_read_u8(&self, index: u16, sub: u8) -> u8 {
self.pdo_map.read_u32(index, sub).unwrap_or(0) as u8
}
fn sdo_read_i8(&self, index: u16, sub: u8) -> i8 {
self.pdo_map.read_u32(index, sub).unwrap_or(0) as i8
}
fn sdo_read_i16(&self, index: u16, sub: u8) -> i16 {
self.pdo_map.read_u16(index, sub).unwrap_or(0) as i16
}
fn sdo_write_u16(&self, index: u16, sub: u8, val: u16) {
let _ = self.pdo_map.write_u16(index, sub, val);
}
fn sdo_write_u32(&self, index: u16, sub: u8, val: u32) {
let _ = self.pdo_map.write_u32(index, sub, val);
}
fn sdo_write_i32(&self, index: u16, sub: u8, val: i32) {
let _ = self.pdo_map.write_u32(index, sub, val as u32);
}
fn sdo_write_u8(&self, index: u16, sub: u8, val: u8) {
let bytes = [val];
unsafe {
ffi::SDOwrite_raw(self.master_index, self.slave_index, index, sub,
0, bytes.as_ptr(), 1);
}
}
fn sdo_write_i8(&self, index: u16, sub: u8, val: i8) {
self.sdo_write_u8(index, sub, val as u8);
}
fn sdo_write_i16(&self, index: u16, sub: u8, val: i16) {
self.sdo_write_u16(index, sub, val as u16);
}
pub fn statusword(&self) -> u16 {
self.sdo_read_u16(OD_STATUSWORD, 0)
}
pub fn controlword(&self) -> u16 {
self.sdo_read_u16(OD_CONTROLWORD, 0)
}
pub fn set_controlword(&self, value: u16) {
self.sdo_write_u16(OD_CONTROLWORD, 0, value);
}
pub fn state_drive(&self) -> StateCiA402 {
StateCiA402::from_statusword(self.statusword())
}
pub fn target_reached(&self) -> bool {
(self.statusword() & SW_TARGET_REACHED) != 0
}
pub fn has_fault(&self) -> bool {
(self.statusword() & SW_FAULT) != 0
}
pub fn has_warning(&self) -> bool {
(self.statusword() & SW_WARNING) != 0
}
pub fn is_remote(&self) -> bool {
(self.statusword() & SW_REMOTE) != 0
}
pub fn operation_mode(&self) -> i8 {
self.sdo_read_i8(OD_MODES_OF_OPERATION_DISPLAY, 0)
}
pub fn set_operation_mode(&self, mode: ModeCiA402) {
self.sdo_write_i8(OD_MODES_OF_OPERATION, 0, mode as i8);
}
pub fn enable(&self, max_retries: i32) -> bool {
unsafe { ffi::CiA402_Enable(self.master_index, self.slave_index, max_retries) != 0 }
}
pub fn enable_default(&self) -> bool {
self.enable(10)
}
pub fn disable_operation(&self) {
self.set_controlword(CW_SWITCH_ON);
}
pub fn disable(&self) {
self.set_controlword(CW_DISABLE_VOLTAGE);
}
pub fn quick_stop(&self) {
self.set_controlword(CW_QUICK_STOP);
}
pub fn fault_reset(&self) {
self.set_controlword(0x00);
std::thread::sleep(std::time::Duration::from_millis(1));
self.set_controlword(CW_FAULT_RESET);
}
pub fn position_actual(&self) -> i32 {
self.sdo_read_i32(OD_POSITION_ACTUAL, 0)
}
pub fn velocity_actual(&self) -> i32 {
self.sdo_read_i32(OD_VELOCITY_ACTUAL, 0)
}
pub fn torque_actual(&self) -> i16 {
self.sdo_read_i16(OD_TORQUE_ACTUAL, 0)
}
pub fn set_target_position(&self, value: i32) {
self.sdo_write_i32(OD_TARGET_POSITION, 0, value);
}
pub fn target_position(&self) -> i32 {
self.sdo_read_i32(OD_TARGET_POSITION, 0)
}
pub fn set_target_velocity(&self, value: i32) {
self.sdo_write_i32(OD_TARGET_VELOCITY, 0, value);
}
pub fn target_velocity(&self) -> i32 {
self.sdo_read_i32(OD_TARGET_VELOCITY, 0)
}
pub fn set_target_torque(&self, value: i16) {
self.sdo_write_i16(OD_TARGET_TORQUE, 0, value);
}
pub fn target_torque(&self) -> i16 {
self.sdo_read_i16(OD_TARGET_TORQUE, 0)
}
pub fn profile_velocity(&self) -> u32 {
self.sdo_read_u32(OD_PROFILE_VELOCITY, 0)
}
pub fn set_profile_velocity(&self, value: u32) {
self.sdo_write_u32(OD_PROFILE_VELOCITY, 0, value);
}
pub fn profile_acceleration(&self) -> u32 {
self.sdo_read_u32(OD_PROFILE_ACCELERATION, 0)
}
pub fn set_profile_acceleration(&self, value: u32) {
self.sdo_write_u32(OD_PROFILE_ACCELERATION, 0, value);
}
pub fn profile_deceleration(&self) -> u32 {
self.sdo_read_u32(OD_PROFILE_DECELERATION, 0)
}
pub fn set_profile_deceleration(&self, value: u32) {
self.sdo_write_u32(OD_PROFILE_DECELERATION, 0, value);
}
pub fn quick_stop_deceleration(&self) -> u32 {
self.sdo_read_u32(OD_QUICK_STOP_DECELERATION, 0)
}
pub fn set_quick_stop_deceleration(&self, value: u32) {
self.sdo_write_u32(OD_QUICK_STOP_DECELERATION, 0, value);
}
pub fn polarity(&self) -> u8 {
self.sdo_read_u8(OD_POLARITY, 0)
}
pub fn set_polarity(&self, value: u8) {
self.sdo_write_u8(OD_POLARITY, 0, value);
}
pub fn motion_profile_type(&self) -> i16 {
self.sdo_read_i16(OD_MOTION_PROFILE_TYPE, 0)
}
pub fn set_motion_profile_type(&self, value: i16) {
self.sdo_write_i16(OD_MOTION_PROFILE_TYPE, 0, value);
}
pub fn software_position_limit_min(&self) -> i32 {
self.sdo_read_i32(OD_SOFTWARE_POSITION_LIMIT, 1)
}
pub fn set_software_position_limit_min(&self, value: i32) {
self.sdo_write_i32(OD_SOFTWARE_POSITION_LIMIT, 1, value);
}
pub fn software_position_limit_max(&self) -> i32 {
self.sdo_read_i32(OD_SOFTWARE_POSITION_LIMIT, 2)
}
pub fn set_software_position_limit_max(&self, value: i32) {
self.sdo_write_i32(OD_SOFTWARE_POSITION_LIMIT, 2, value);
}
pub fn home_offset(&self) -> i32 {
self.sdo_read_i32(OD_HOME_OFFSET, 0)
}
pub fn set_home_offset(&self, value: i32) {
self.sdo_write_i32(OD_HOME_OFFSET, 0, value);
}
pub fn homing_speed_search(&self) -> u32 {
self.sdo_read_u32(OD_HOMING_SPEEDS, 1)
}
pub fn set_homing_speed_search(&self, value: u32) {
self.sdo_write_u32(OD_HOMING_SPEEDS, 1, value);
}
pub fn homing_speed_zero(&self) -> u32 {
self.sdo_read_u32(OD_HOMING_SPEEDS, 2)
}
pub fn set_homing_speed_zero(&self, value: u32) {
self.sdo_write_u32(OD_HOMING_SPEEDS, 2, value);
}
pub fn homing_acceleration(&self) -> u32 {
self.sdo_read_u32(OD_HOMING_ACCELERATION, 0)
}
pub fn set_homing_acceleration(&self, value: u32) {
self.sdo_write_u32(OD_HOMING_ACCELERATION, 0, value);
}
pub fn configure_touch_probe(&self, function: u16) {
self.sdo_write_u16(OD_TOUCH_PROBE_FUNCTION, 0, function);
}
pub fn touch_probe_status(&self) -> u16 {
self.sdo_read_u16(OD_TOUCH_PROBE_STATUS, 0)
}
pub fn touch_probe_positive_edge(&self) -> i32 {
self.sdo_read_i32(OD_TOUCH_PROBE_POS_EDGE, 0)
}
pub fn touch_probe_negative_edge(&self) -> i32 {
self.sdo_read_i32(OD_TOUCH_PROBE_NEG_EDGE, 0)
}
pub fn digital_inputs(&self) -> u32 {
self.sdo_read_u32(OD_DIGITAL_INPUTS, 0)
}
pub fn digital_outputs(&self) -> u32 {
self.sdo_read_u32(OD_DIGITAL_OUTPUTS, 1)
}
pub fn set_digital_outputs(&self, value: u32) {
self.sdo_write_u32(OD_DIGITAL_OUTPUTS, 1, value);
}
pub fn max_torque(&self) -> u16 {
self.sdo_read_u16(OD_MAX_TORQUE, 0)
}
pub fn set_max_torque(&self, value: u16) {
self.sdo_write_u16(OD_MAX_TORQUE, 0, value);
}
pub fn motor_rated_torque(&self) -> u32 {
self.sdo_read_u32(OD_MOTOR_RATED_TORQUE, 0)
}
pub fn set_motor_rated_torque(&self, value: u32) {
self.sdo_write_u32(OD_MOTOR_RATED_TORQUE, 0, value);
}
pub fn position_offset(&self) -> i32 {
self.sdo_read_i32(OD_POSITION_OFFSET, 0)
}
pub fn set_position_offset(&self, value: i32) {
self.sdo_write_i32(OD_POSITION_OFFSET, 0, value);
}
pub fn velocity_offset(&self) -> i32 {
self.sdo_read_i32(OD_VELOCITY_OFFSET, 0)
}
pub fn set_velocity_offset(&self, value: i32) {
self.sdo_write_i32(OD_VELOCITY_OFFSET, 0, value);
}
pub fn torque_offset(&self) -> i16 {
self.sdo_read_i16(OD_TORQUE_OFFSET, 0)
}
pub fn set_torque_offset(&self, value: i16) {
self.sdo_write_i16(OD_TORQUE_OFFSET, 0, value);
}
pub fn interpolation_time_period_value(&self) -> u8 {
self.sdo_read_u8(OD_INTERPOLATION_TIME_PERIOD, 1)
}
pub fn set_interpolation_time_period_value(&self, value: u8) {
self.sdo_write_u8(OD_INTERPOLATION_TIME_PERIOD, 1, value);
}
pub fn interpolation_time_period_index(&self) -> i8 {
self.sdo_read_i8(OD_INTERPOLATION_TIME_PERIOD, 2)
}
pub fn set_interpolation_time_period_index(&self, value: i8) {
self.sdo_write_i8(OD_INTERPOLATION_TIME_PERIOD, 2, value);
}
pub fn quick_stop_option_code(&self) -> i16 {
self.sdo_read_i16(OD_QUICK_STOP_OPTION_CODE, 0)
}
pub fn set_quick_stop_option_code(&self, value: i16) {
self.sdo_write_i16(OD_QUICK_STOP_OPTION_CODE, 0, value);
}
pub fn positive_torque_limit(&self) -> u16 {
self.sdo_read_u16(OD_POSITIVE_TORQUE_LIMIT, 0)
}
pub fn set_positive_torque_limit(&self, value: u16) {
self.sdo_write_u16(OD_POSITIVE_TORQUE_LIMIT, 0, value);
}
pub fn negative_torque_limit(&self) -> u16 {
self.sdo_read_u16(OD_NEGATIVE_TORQUE_LIMIT, 0)
}
pub fn set_negative_torque_limit(&self, value: u16) {
self.sdo_write_u16(OD_NEGATIVE_TORQUE_LIMIT, 0, value);
}
pub fn supported_homing_methods(&self) -> Vec<i8> {
let count = self.sdo_read_u8(OD_SUPPORTED_HOMING_METHODS, 0);
let mut methods = Vec::new();
for i in 1..=count {
methods.push(self.sdo_read_i8(OD_SUPPORTED_HOMING_METHODS, i));
}
methods
}
pub fn txpdo_data_invalid(&self) -> bool {
self.sdo_read_u8(OD_TXPDO_DATA_INVALID, 0) != 0
}
pub fn synchronization_settings(&self) -> u16 {
self.sdo_read_u16(OD_SYNCHRONIZATION_SETTINGS, 1)
}
pub fn set_synchronization_settings(&self, value: u16) {
self.sdo_write_u16(OD_SYNCHRONIZATION_SETTINGS, 1, value);
}
pub fn drive_sync_status(&self) -> u16 {
self.sdo_read_u16(OD_DRIVE_SYNC_STATUS, 0)
}
pub fn new_setpoint(&self, position: i32, relative: bool) {
self.set_target_position(position);
let mut cw = CW_ENABLE_OPERATION | 0x10;
if relative { cw |= 0x40; }
self.set_controlword(cw);
}
pub fn clear_new_setpoint(&self) {
self.set_controlword(CW_ENABLE_OPERATION);
}
pub fn start_homing(&self) {
self.set_controlword(CW_ENABLE_OPERATION | 0x10);
}
pub fn homing_attained(&self) -> bool {
(self.statusword() & SW_OP_MODE_SPECIFIC_1) != 0
}
pub fn homing_error(&self) -> bool {
(self.statusword() & SW_OP_MODE_SPECIFIC_2) != 0
}
pub fn validate_profile_parameters(&self, configured_mode: Option<i8>) -> Vec<String> {
let mut warnings = Vec::new();
let mode = configured_mode.unwrap_or_else(|| self.sdo_read_i8(OD_MODES_OF_OPERATION, 0));
if mode != 1 && mode != 3 {
return warnings;
}
let mode_name = if mode == 1 { "PP" } else { "PV" };
let checks: [(u16, &str); 3] = [
(OD_MAX_PROFILE_VELOCITY, "Max Profile Velocity (0x6080)"),
(OD_PROFILE_ACCELERATION, "Profile Acceleration (0x6083)"),
(OD_PROFILE_DECELERATION, "Profile Deceleration (0x6084)"),
];
for (index, name) in &checks {
let val = self.sdo_read_u32(*index, 0);
if val == 0 {
warnings.push(format!(
"{} 模式: {} = 0, 驱动器无法生成运动曲线, 请在 Startup 中配置此参数",
mode_name, name
));
}
}
warnings
}
fn try_read_attribute<T: Cia402Readable>(&self, index: u16, subindex: u8)
-> (T, Cia402ReadStatus, u32)
{
let mut size: c_int = 0;
let ptr = unsafe {
ffi::SDOread(self.master_index, self.slave_index, index, subindex, 0, &mut size)
};
if !ptr.is_null() && size > 0 {
let data = unsafe { std::slice::from_raw_parts(ptr, size as usize).to_vec() };
unsafe { ffi::FreeMemory(ptr as *mut _) };
return (T::from_le_slice(&data), Cia402ReadStatus::Success, 0);
}
if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
let abort_code = if size < 0 { (-size) as u32 } else { 0 };
let status = if abort_code != 0 {
Cia402ReadStatus::classify_abort(abort_code)
} else {
Cia402ReadStatus::ReadFailed
};
(T::default(), status, abort_code)
}
pub fn try_read_object<T: Cia402Readable>(&self, index: u16, subindex: u8)
-> Cia402ReadResult<T>
{
let (value, status, abort) = self.try_read_attribute::<T>(index, subindex);
Cia402ReadResult::new(value, status, abort)
}
pub fn try_read_error_code(&self) -> Cia402ReadResult<u16> {
self.try_read_object::<u16>(OD_ERROR_CODE, 0)
}
pub fn try_read_supported_drive_modes(&self) -> Cia402ReadResult<u32> {
self.try_read_object::<u32>(OD_SUPPORTED_DRIVE_MODES, 0)
}
pub fn try_read_digital_inputs(&self) -> Cia402ReadResult<u32> {
self.try_read_object::<u32>(OD_DIGITAL_INPUTS, 0)
}
pub fn try_read_operation_mode_display(&self) -> Cia402ReadResult<i8> {
self.try_read_object::<i8>(OD_MODES_OF_OPERATION_DISPLAY, 0)
}
pub fn try_read_drive_data(&self, subindex: u8) -> Cia402ReadResult<u32> {
self.try_read_object::<u32>(OD_DRIVE_DATA, subindex)
}
pub fn try_read_statusword(&self) -> Cia402ReadResult<u16> {
self.try_read_object::<u16>(OD_STATUSWORD, 0)
}
pub fn try_read_position_actual(&self) -> Cia402ReadResult<i32> {
self.try_read_object::<i32>(OD_POSITION_ACTUAL, 0)
}
pub fn try_read_torque_actual(&self) -> Cia402ReadResult<i16> {
self.try_read_object::<i16>(OD_TORQUE_ACTUAL, 0)
}
pub fn supported_drive_modes_readable(&self) -> bool {
self.try_read_supported_drive_modes().ok()
}
pub fn transition_command(current: StateCiA402, target: StateCiA402) -> u16 {
unsafe { ffi::CiA402_GetTransitionCommand(current as c_int, target as c_int) }
}
pub fn request_transition(&self, target: StateCiA402) -> bool {
unsafe {
ffi::CiA402_RequestTransition(self.master_index, self.slave_index, target as c_int) != 0
}
}
pub fn fault_reset_step(&self, step: i32) -> bool {
unsafe {
ffi::CiA402_FaultResetStep(self.master_index, self.slave_index, step) != 0
}
}
pub fn supported_drive_modes(&self) -> u32 {
unsafe { ffi::CiA402_GetSupportedDriveModes(self.master_index, self.slave_index) }
}
pub fn is_mode_supported(&self, mode: ModeCiA402) -> bool {
unsafe {
ffi::CiA402_IsModeSupported(self.master_index, self.slave_index, mode as c_int) != 0
}
}
pub fn drive_error_code(&self) -> u16 {
unsafe { ffi::CiA402_GetErrorCode(self.master_index, self.slave_index) }
}
pub fn error_history(&self) -> Vec<u32> {
const MAX: usize = 64;
let mut buf = [0u32; MAX];
let n = unsafe {
ffi::CiA402_GetErrorHistory(
self.master_index, self.slave_index, buf.as_mut_ptr(), MAX as c_int,
)
};
if n <= 0 { return Vec::new(); }
buf[..(n as usize).min(MAX)].to_vec()
}
pub fn clear_error_history(&self) -> bool {
unsafe { ffi::CiA402_ClearErrorHistory(self.master_index, self.slave_index) != 0 }
}
pub fn set_homing_method(&self, method: i8) -> bool {
unsafe { ffi::CiA402_SetHomingMethod(self.master_index, self.slave_index, method) != 0 }
}
pub fn homing_method(&self) -> i8 {
unsafe { ffi::CiA402_GetHomingMethod(self.master_index, self.slave_index) }
}
pub fn supported_homing_methods_native(&self) -> Vec<i8> {
const MAX: usize = 64;
let mut buf = [0i8; MAX];
let n = unsafe {
ffi::CiA402_GetSupportedHomingMethods(
self.master_index, self.slave_index, buf.as_mut_ptr(), MAX as c_int,
)
};
if n <= 0 { return Vec::new(); }
buf[..(n as usize).min(MAX)].to_vec()
}
pub fn is_standard_homing_method(method: i8) -> bool {
unsafe { ffi::CiA402_IsStandardHomingMethod(method) != 0 }
}
pub fn quick_stop_option_native(&self) -> i16 {
unsafe { ffi::CiA402_GetQuickStopOption(self.master_index, self.slave_index) }
}
pub fn set_quick_stop_option_native(&self, value: i16) -> bool {
unsafe { ffi::CiA402_SetQuickStopOption(self.master_index, self.slave_index, value) != 0 }
}
pub fn shutdown_option(&self) -> i16 {
unsafe { ffi::CiA402_GetShutdownOption(self.master_index, self.slave_index) }
}
pub fn set_shutdown_option(&self, value: i16) -> bool {
unsafe { ffi::CiA402_SetShutdownOption(self.master_index, self.slave_index, value) != 0 }
}
pub fn disable_operation_option(&self) -> i16 {
unsafe { ffi::CiA402_GetDisableOperationOption(self.master_index, self.slave_index) }
}
pub fn set_disable_operation_option(&self, value: i16) -> bool {
unsafe {
ffi::CiA402_SetDisableOperationOption(self.master_index, self.slave_index, value) != 0
}
}
pub fn halt_option(&self) -> i16 {
unsafe { ffi::CiA402_GetHaltOption(self.master_index, self.slave_index) }
}
pub fn set_halt_option(&self, value: i16) -> bool {
unsafe { ffi::CiA402_SetHaltOption(self.master_index, self.slave_index, value) != 0 }
}
pub fn fault_reaction_option(&self) -> i16 {
unsafe { ffi::CiA402_GetFaultReactionOption(self.master_index, self.slave_index) }
}
pub fn set_fault_reaction_option(&self, value: i16) -> bool {
unsafe {
ffi::CiA402_SetFaultReactionOption(self.master_index, self.slave_index, value) != 0
}
}
}
impl std::fmt::Display for CiA402Instance {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
let sw = self.statusword();
let state = StateCiA402::from_statusword(sw);
let mode = self.operation_mode();
write!(f, "从站 {}: CiA402 [{}, 模式={}, SW=0x{:04X}]",
self.slave_index, state, crate::statics::print::cia402_mode_description(mode), sw)
}
}