poulpe_ethercat_controller/
state_machine.rs1#[derive(FromPrimitive, Debug, PartialEq)]
2pub enum StatusBit {
3 ReadyToSwitchOn = 0,
4 SwitchedOn = 1,
5 OperationEnabled = 2,
6 Fault = 3,
7 VoltageEnabled = 4,
8 QuickStop = 5,
9 SwitchedOnDisabled = 6,
10 Warning = 7,
11 Reserved8 = 8,
12 Remote = 9,
13 OperatingModeSpecific10 = 10,
14 InternalLimitActive = 11,
15 OperatingModeSpecific12 = 12,
16 OperatingModeSpecific13 = 13,
17 Reserved14 = 14,
18 PositionReferencedToHomePosition = 15,
19}
20
21#[derive(FromPrimitive, Debug, PartialEq)]
22pub enum ControlWord {
23 Shutdown,
24 SwitchOn, DisableVoltage, EnableOperation,
27 DisableOperation,
28 QuickStop,
29 FaultReset,
30 Unknown,
31}
32
33impl ControlWord {
34 pub fn to_u16(&self) -> u16 {
35 match self {
36 ControlWord::Shutdown => 0b0110,
37 ControlWord::SwitchOn => 0b0111,
38 ControlWord::DisableVoltage => 0b0000, ControlWord::EnableOperation => 0b1111,
40 ControlWord::DisableOperation => 0b0111,
41 ControlWord::QuickStop => 0b0010,
42 ControlWord::FaultReset => 0b10000000,
43 ControlWord::Unknown => 0b0,
44 }
45 }
46}
47
48#[derive(FromPrimitive, PartialEq, Clone, Copy, Debug)]
53pub enum CiA402ModeOfOperation {
54 ProfilePositionMode = 1,
55 VelocityMode = 2,
56 ProfileVelocityMode = 3,
57 ProfileTorqueMode = 4,
58 HomingMode = 6,
59 InterpolatedPositionMode = 7,
60 CyclicSynchronousPositionMode = 8,
61 CyclicSynchronousVelocityMode = 9,
62 CyclicSynchronousTorqueMode = 10,
63 Reserved = 15,
64}
65
66impl CiA402ModeOfOperation {
67 pub fn from_u8(value: u8) -> Option<CiA402ModeOfOperation> {
68 num::FromPrimitive::from_u8(value)
69 }
70}
71
72#[derive(FromPrimitive, PartialEq, Clone, Copy, Debug)]
83pub enum MotorErrorFlag {
84 ConfigFail = 0,
86 MotorAlignFail = 1,
87 HighTemperatureWarning = 2,
88 OverTemperatureMotor = 3,
89 OverTemperatureBoard = 4,
90 OverCurrent = 5,
91 LowBusVoltage = 6,
92 DriverFault = 7,
93 TemperatureSensorMalfunctionWarning = 8,
94}
95
96#[derive(FromPrimitive, PartialEq, Clone, Copy, Debug)]
105pub enum HomingErrorFlag {
106 AxisSensorReadFail = 0,
108 MotorMovementCheckFail = 1,
109 AxisSensorAlignFail = 2,
110 ZeroingFail = 3,
111 IndexSearchFail = 4,
112 LowLevelCommunicaiton = 5,
113}
114
115#[derive(FromPrimitive, PartialEq, Clone, Copy, Debug)]
116#[repr(u16)]
117pub enum CiA402State {
118 NotReadyToSwitchOn = 0b00000000, SwitchOnDisabled = 0b01000000, ReadyToSwitchOn = 0b00100001, SwitchedOn = 0b00100011, OperationEnabled = 0b00110111, QuickStopActive = 0b00000111, FaultReactionActive = 0b00011111, Fault = 0b00001000, }
128
129#[derive(Debug)]
130pub struct ErrorFlags {
131 pub motor_error_flags: Vec<Vec<MotorErrorFlag>>,
132 pub homing_error_flags: Vec<HomingErrorFlag>,
133}
134
135pub fn parse_status_word(status: u16) -> Vec<StatusBit> {
136 let mut status_bits = Vec::new();
137 for i in 0..16 {
138 if status & (1 << i) != 0 {
139 status_bits.push(num::FromPrimitive::from_u8(i as u8).unwrap());
140 }
141 }
142 status_bits
143}
144
145pub fn parse_motor_error_flags(error: [u8; 2]) -> Vec<MotorErrorFlag> {
146 let motor_error = u16::from_le_bytes(error);
147 let mut error_flags = Vec::new();
148 for i in 0..16 {
149 if motor_error & (1 << i) != 0 {
150 error_flags.push(num::FromPrimitive::from_u16(i as u16).unwrap());
151 }
152 }
153 error_flags
154}
155
156pub fn parse_homing_error_flags(error: [u8; 2]) -> Vec<HomingErrorFlag> {
157 let homming_error = u16::from_le_bytes(error);
158 let mut error_flags = Vec::new();
159 for i in 0..16 {
160 if homming_error & (1 << i) != 0 {
161 error_flags.push(num::FromPrimitive::from_u16(i as u16).unwrap());
162 }
163 }
164 error_flags
165}
166
167pub fn parse_state_from_status_word(status: u16) -> CiA402State {
168 num::FromPrimitive::from_u16(status).unwrap()
169}
170
171pub fn parse_state_from_status_bits(
172 status_bits: Vec<StatusBit>,
173) -> Result<CiA402State, Box<dyn std::error::Error>> {
174 let mut state = 0;
175 for bit in status_bits {
176 state |= 1 << bit as u16;
177 }
178
179 {
182 state = state & 0b0011111011111111;
183 }
184
185 state = state & 0b1111111101111111;
187
188 match num::FromPrimitive::from_u16(state) {
189 Some(s) => Ok(s),
190 None => Err("Invalid state".into()),
191 }
192}