poulpe_ethercat_controller/
register.rs

1use crate::state_machine::{CiA402State, ErrorFlags, HomingErrorFlag, MotorErrorFlag};
2
3pub(crate) enum PdoRegister {
4    ErrorCode,
5    ActuatorType,
6    AxisZeroPosition,
7    BoardTemperature,
8    MotorTemperature,
9
10    ControlWord,
11    ModeOfOperation,
12    TargetPosition,
13    TargetVelocity,
14    TargetTorque,
15    VelocityLimit,
16    TorqueLimit,
17
18    StatusWord,
19    ModeOfOperationDisplay,
20    PositionActualValue,
21    VelocityActualValue,
22    TorqueActualValue,
23    AxisSensorActualValue,
24}
25
26impl PdoRegister {
27    pub(crate) fn name(&self) -> &'static str {
28        match *self {
29            PdoRegister::ErrorCode => "error_code",
30            PdoRegister::ActuatorType => "actuator_type",
31            PdoRegister::AxisZeroPosition => "axis_position_zero_offset",
32            PdoRegister::BoardTemperature => "board_temperatures",
33            PdoRegister::MotorTemperature => "motor_temperatures",
34
35            PdoRegister::ControlWord => "controlword",
36            PdoRegister::ModeOfOperation => "mode_of_operation",
37            PdoRegister::TargetTorque => "target_torque",
38            PdoRegister::TargetPosition => "target_position",
39            PdoRegister::TargetVelocity => "target_velocity",
40            PdoRegister::VelocityLimit => "velocity_limit",
41            PdoRegister::TorqueLimit => "torque_limit",
42
43            PdoRegister::StatusWord => "statusword",
44            PdoRegister::ModeOfOperationDisplay => "mode_of_operation_display",
45            PdoRegister::PositionActualValue => "actual_position",
46            PdoRegister::VelocityActualValue => "actual_velocity",
47            PdoRegister::TorqueActualValue => "actual_torque",
48            PdoRegister::AxisSensorActualValue => "actual_axis_position",
49        }
50    }
51}
52
53// taken from  https://github.com/pollen-robotics/firmware_Poulpe/blob/dbafefe93868296ace1517908a2280c336af03a4/src/motor_control/mod.rs#L20-L31
54#[derive(FromPrimitive, Debug, PartialEq)]
55#[repr(u8)]
56pub enum BoardStatus {
57    Ok = 0,
58    InitError = 1,
59    SensorError = 2,
60    IndexError = 3,
61    ZeroingError = 4,
62    OverTemperatureError = 5,
63    OverCurrentError = 6,
64    BusVoltageError = 7,
65    Init = 20,
66    HighTemperatureState = 100,
67    Unknown = 255,
68}
69
70impl BoardStatus {
71    pub fn from_cia402_to_board_status(state: u32, flags: Vec<i32>) -> Result<BoardStatus, ()> {
72        let state: CiA402State = num::FromPrimitive::from_u32(state).unwrap();
73
74        let homing_errors: Vec<HomingErrorFlag> = flags
75            .first()
76            .map(|x| {
77                let error = u16::from_le_bytes((*x as u16).to_le_bytes());
78                let mut error_flags = Vec::new();
79                for i in 0..16 {
80                    if error & (1 << i) != 0 {
81                        error_flags.push(num::FromPrimitive::from_u16(i as u16).unwrap());
82                    }
83                }
84                error_flags
85            })
86            .unwrap_or(Vec::new());
87
88        // start from the second flags element
89        let motor_errors: Vec<Vec<MotorErrorFlag>> = flags[1..]
90            .iter()
91            .map(|x| {
92                let error = u16::from_le_bytes((*x as u16).to_le_bytes());
93                let mut error_flags = Vec::new();
94                for i in 0..16 {
95                    if error & (1 << i) != 0 {
96                        error_flags.push(num::FromPrimitive::from_u16(i as u16).unwrap());
97                    }
98                }
99                error_flags
100            })
101            .collect();
102
103        let flags = ErrorFlags {
104            motor_error_flags: motor_errors,
105            homing_error_flags: homing_errors,
106        };
107
108        // baordstatus is much less informative than the error flags so we are obliged to make some strange decisions here
109        match state {
110            CiA402State::NotReadyToSwitchOn => Ok(BoardStatus::Init),
111            CiA402State::SwitchOnDisabled
112            | CiA402State::ReadyToSwitchOn
113            | CiA402State::SwitchedOn
114            | CiA402State::OperationEnabled
115            | CiA402State::QuickStopActive => {
116                if flags
117                    .motor_error_flags
118                    .iter()
119                    .any(|x| x.contains(&MotorErrorFlag::HighTemperatureWarning))
120                {
121                    Ok(BoardStatus::HighTemperatureState)
122                } else {
123                    Ok(BoardStatus::Ok)
124                }
125            }
126            CiA402State::Fault | CiA402State::FaultReactionActive => {
127                if flags.motor_error_flags.iter().any(|x| {
128                    x.contains(&MotorErrorFlag::OverTemperatureMotor)
129                        || x.contains(&MotorErrorFlag::OverTemperatureBoard)
130                }) {
131                    Ok(BoardStatus::OverTemperatureError)
132                } else if flags
133                    .motor_error_flags
134                    .iter()
135                    .any(|x| x.contains(&MotorErrorFlag::OverCurrent))
136                {
137                    Ok(BoardStatus::OverCurrentError)
138                } else if flags.motor_error_flags.iter().any(|x| {
139                    x.contains(&MotorErrorFlag::LowBusVoltage)
140                        || x.contains(&MotorErrorFlag::DriverFault)
141                }) {
142                    Ok(BoardStatus::BusVoltageError)
143                } else if flags
144                    .homing_error_flags
145                    .contains(&HomingErrorFlag::IndexSearchFail)
146                {
147                    Ok(BoardStatus::IndexError)
148                } else if flags
149                    .homing_error_flags
150                    .contains(&HomingErrorFlag::ZeroingFail)
151                {
152                    Ok(BoardStatus::ZeroingError)
153                } else if flags
154                    .homing_error_flags
155                    .contains(&HomingErrorFlag::AxisSensorReadFail)
156                {
157                    Ok(BoardStatus::SensorError)
158                } else {
159                    Ok(BoardStatus::InitError)
160                }
161            }
162        }
163    }
164}
poulpe_ethercat_controller/register.rs

poulpe_ethercat_controller/
register.rs
