use crate::protocol::{MotorFeedback, MotorModel};
use crate::protocols::knott_dynamics::{KnottDynamicsFeedback, KnottDynamicsModel};
use crate::protocols::robstride::{RobStrideFeedback, RobStrideModel};
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum ActuatorProtocol {
CubeMars(MotorModel),
RobStride(RobStrideModel),
KnottDynamics(KnottDynamicsModel),
}
impl ActuatorProtocol {
pub const fn family_name(self) -> &'static str {
match self {
Self::CubeMars(_) => "CubeMars",
Self::RobStride(_) => "RobStride/Seeed",
Self::KnottDynamics(_) => "Knott Dynamics",
}
}
pub const fn model_name(self) -> &'static str {
match self {
Self::CubeMars(model) => model.name(),
Self::RobStride(model) => model.name(),
Self::KnottDynamics(model) => model.name(),
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ActuatorConfig {
pub id: u8,
pub label: Option<String>,
pub protocol: ActuatorProtocol,
}
impl ActuatorConfig {
pub fn display_name(&self) -> String {
match &self.label {
Some(label) => format!("{label} (0x{:02X})", self.id),
None => format!("{} 0x{:02X}", self.protocol.model_name(), self.id),
}
}
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum ActuatorFeedback {
CubeMars(MotorFeedback),
RobStride(RobStrideFeedback),
KnottDynamics(KnottDynamicsFeedback),
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum ActuatorHealth {
Nominal,
Warning,
Fault,
Stale,
}
impl ActuatorHealth {
pub const fn label(self) -> &'static str {
match self {
Self::Nominal => "NOMINAL",
Self::Warning => "WARNING",
Self::Fault => "FAULT",
Self::Stale => "STALE",
}
}
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ActuatorMetric {
pub label: String,
pub value: String,
pub unit: String,
pub health: ActuatorHealth,
}
impl ActuatorMetric {
pub fn new(
label: impl Into<String>,
value: impl Into<String>,
unit: impl Into<String>,
health: ActuatorHealth,
) -> Self {
Self {
label: label.into(),
value: value.into(),
unit: unit.into(),
health,
}
}
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct ActuatorSnapshot {
pub config: ActuatorConfig,
pub feedback: Option<ActuatorFeedback>,
}
impl ActuatorSnapshot {
pub fn health(&self) -> ActuatorHealth {
match &self.feedback {
None => ActuatorHealth::Stale,
Some(ActuatorFeedback::CubeMars(feedback)) if feedback.has_fault() => {
ActuatorHealth::Fault
}
Some(ActuatorFeedback::RobStride(feedback)) if feedback.has_error() => {
ActuatorHealth::Fault
}
Some(ActuatorFeedback::KnottDynamics(feedback))
if feedback.has_fault() == Some(true) =>
{
ActuatorHealth::Fault
}
Some(_) => ActuatorHealth::Nominal,
}
}
pub fn metrics(&self) -> Vec<ActuatorMetric> {
match &self.feedback {
Some(ActuatorFeedback::CubeMars(feedback)) => vec![
ActuatorMetric::new(
"pos",
format!("{:.2}", feedback.position_degrees),
"deg",
ActuatorHealth::Nominal,
),
ActuatorMetric::new(
"speed",
feedback.speed_erpm.to_string(),
"erpm",
ActuatorHealth::Nominal,
),
ActuatorMetric::new(
"current",
format!("{:.2}", feedback.current_amps),
"A",
ActuatorHealth::Nominal,
),
ActuatorMetric::new(
"temp",
feedback.temperature_celsius.to_string(),
"C",
ActuatorHealth::Nominal,
),
ActuatorMetric::new(
"fault",
feedback.error_code.to_string(),
"",
if feedback.has_fault() {
ActuatorHealth::Fault
} else {
ActuatorHealth::Nominal
},
),
],
Some(ActuatorFeedback::RobStride(feedback)) => vec![
ActuatorMetric::new(
"pos",
format!("{:.3}", feedback.position_rad),
"rad",
ActuatorHealth::Nominal,
),
ActuatorMetric::new(
"vel",
format!("{:.3}", feedback.velocity_rad_s),
"rad/s",
ActuatorHealth::Nominal,
),
ActuatorMetric::new(
"torque",
format!("{:.2}", feedback.torque_nm),
"Nm",
ActuatorHealth::Nominal,
),
ActuatorMetric::new(
"error",
feedback.error_code.unwrap_or(0).to_string(),
"",
if feedback.has_error() {
ActuatorHealth::Fault
} else {
ActuatorHealth::Nominal
},
),
],
Some(ActuatorFeedback::KnottDynamics(feedback)) => {
let mut metrics = vec![ActuatorMetric::new(
"pos",
format!("{:.3}", feedback.position_rad),
"rad",
ActuatorHealth::Nominal,
)];
if let Some(velocity) = feedback.velocity_rad_s {
metrics.push(ActuatorMetric::new(
"vel",
format!("{velocity:.3}"),
"rad/s",
ActuatorHealth::Nominal,
));
}
if let Some(torque) = feedback.torque_nm {
metrics.push(ActuatorMetric::new(
"torque",
format!("{torque:.2}"),
"Nm",
ActuatorHealth::Nominal,
));
}
if let Some(mode) = feedback.mode {
metrics.push(ActuatorMetric::new(
"mode",
mode.name(),
"",
ActuatorHealth::Nominal,
));
}
if let Some(faults) = feedback.faults {
metrics.push(ActuatorMetric::new(
"faults",
format!("0x{:04X}", faults.bits()),
"",
if faults.is_empty() {
ActuatorHealth::Nominal
} else {
ActuatorHealth::Fault
},
));
}
metrics
}
None => vec![ActuatorMetric::new(
"feedback",
"missing",
"",
ActuatorHealth::Stale,
)],
}
}
pub fn status_text(&self) -> String {
match &self.feedback {
Some(ActuatorFeedback::CubeMars(feedback)) => format!(
"pos={:.2}deg speed={}erpm current={:.2}A temp={}C fault={}",
feedback.position_degrees,
feedback.speed_erpm,
feedback.current_amps,
feedback.temperature_celsius,
feedback.error_code
),
Some(ActuatorFeedback::RobStride(feedback)) => format!(
"pos={:.3}rad vel={:.3}rad/s torque={:.2}Nm error={}",
feedback.position_rad,
feedback.velocity_rad_s,
feedback.torque_nm,
feedback.error_code.unwrap_or(0)
),
Some(ActuatorFeedback::KnottDynamics(feedback)) => {
let mut status = format!("pos={:.3}rad", feedback.position_rad);
if let Some(velocity) = feedback.velocity_rad_s {
status.push_str(&format!(" vel={velocity:.3}rad/s"));
}
if let Some(torque) = feedback.torque_nm {
status.push_str(&format!(" torque={torque:.2}Nm"));
}
if let Some(mode) = feedback.mode {
status.push_str(&format!(" mode={}", mode.name()));
}
if let Some(faults) = feedback.faults {
status.push_str(&format!(" faults=0x{:04X}", faults.bits()));
}
status
}
None => "no feedback".to_owned(),
}
}
}