poulpe_ethercat_controller/
state_machine.rs

1#[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,       // the same as DisableOperation
25    DisableVoltage, // NOT USED
26    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, // NOT USED
39            ControlWord::EnableOperation => 0b1111,
40            ControlWord::DisableOperation => 0b0111,
41            ControlWord::QuickStop => 0b0010,
42            ControlWord::FaultReset => 0b10000000,
43            ControlWord::Unknown => 0b0,
44        }
45    }
46}
47
48// The mode of operation for the motor
49// - 1: ProfilePositionMode - the motor will move to a specific position
50// - 2: VelocityMode - the motor will move at a specific velocity
51// - 4: ProfileTorqueMode - the motor will move with a specific torque
52#[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// Error codes for the motors, we will have one error code per motor
73// - None - no error
74// - ConfigFail - error during the configuration of the motor
75// - MotorAlignFail - error during the motor alignment
76// - HighTemperatureWarning - warning for high temperature
77// - OverTemperature - error due to the temperature being too high
78// - OverCurrent - error due to the current being too high
79// - LowBusVoltage - error due to the bus voltage being too low
80// - DriverFault - error due to a fault in the driver
81// - TemperatureSensorMalfunctionWarning - warning for a malfunction in the temperature sensor
82#[derive(FromPrimitive, PartialEq, Clone, Copy, Debug)]
83pub enum MotorErrorFlag {
84    // None = 0,
85    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// Error codes for the homing procedure
97// - None - no error
98// - AxisSensorReadFail - error during the reading of the axis sensor
99// - MotorMovementCheckFail - error during the check of the motor movement
100// - AxisSensorAlignFail - error during the alignment of the axis sensor
101// - ZeroingFail - error during the zeroing of the axis positions
102// - IndexSearchFail - error during the search of the index (only orbita3d)
103// - LowLevelCommunicaiton - error due to communication failure with the motor driver
104#[derive(FromPrimitive, PartialEq, Clone, Copy, Debug)]
105pub enum HomingErrorFlag {
106    // None = 0,
107    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, // initialisation and test of the drive is not yet completed
119    SwitchOnDisabled = 0b01000000,   // init passed successfully
120    ReadyToSwitchOn = 0b00100001, // init sucess + switch off received - (more or less saying that the EtherCAT is connected)
121    SwitchedOn = 0b00100011,      // init sucess + switch on received
122    //  - in our case we send operation enabled and switch on at the same time, so we dont really use this state
123    OperationEnabled = 0b00110111, // switched on + enable operation received
124    QuickStopActive = 0b00000111, // quick stop procedure going to Switch_on_disabled state ( we don't use quick stop )
125    FaultReactionActive = 0b00011111, // fault reaction going to Fault state
126    Fault = 0b00001000,           // fault state
127}
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    // remove the manufacturer specific bits
180    // bits 8, 14 and 15
181    {
182        state = state & 0b0011111011111111;
183    }
184
185    // remove the warning bit
186    state = state & 0b1111111101111111;
187
188    match num::FromPrimitive::from_u16(state) {
189        Some(s) => Ok(s),
190        None => Err("Invalid state".into()),
191    }
192}
poulpe_ethercat_controller/state_machine.rs

poulpe_ethercat_controller/
state_machine.rs
