#[cfg(all(feature = "socketcan", feature = "tui"))]
use std::time::Duration;
#[cfg(all(feature = "socketcan", feature = "tui"))]
use crossterm::event::{Event, KeyCode};
#[cfg(all(feature = "socketcan", feature = "tui"))]
use kcan::{
ActuatorConfig, ActuatorFeedback, ActuatorProtocol, ActuatorSnapshot, CanBus, CanFrame, CanId,
Error, FeedbackFilter, KnottDynamicsFunction, KnottDynamicsModel, MotorModel,
RobStrideFeedback, RobStrideFrameKind, RobStrideModel, SocketCanBus, TuiControlIntent,
TuiDashboard, is_position_32bit_feedback_frame, is_servo_feedback_frame,
parse_knott_dynamics_feedback, parse_servo_feedback,
};
#[cfg(all(feature = "socketcan", feature = "tui"))]
use scrin::{Terminal, TerminalOptions};
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn main() -> Result<(), Box<dyn std::error::Error>> {
let options = DashboardOptions::from_args()?;
let snapshots = options
.targets
.iter()
.map(DashboardTarget::snapshot_without_feedback)
.collect();
let mut bus = SocketCanBus::open(&options.interface)?;
let mut dashboard = TuiDashboard::new(snapshots).with_controls_enabled(false);
let mut terminal = Terminal::init_with(TerminalOptions::new())?;
loop {
if let Some(frame) = bus.receive(Duration::from_millis(25))? {
if let Some(snapshot) = snapshot_from_frame(&options.targets, &frame)? {
dashboard.upsert_snapshot(snapshot);
}
}
dashboard.advance_tick();
terminal.draw(|frame| dashboard.render_frame(frame))?;
if let Some(Event::Key(key)) = terminal.poll_event()? {
let ch = match key.code {
KeyCode::Char(ch) => Some(ch),
KeyCode::Esc => Some('q'),
_ => None,
};
if let Some(command) = ch.and_then(kcan::TuiCommand::from_key) {
if dashboard.apply_command(command) == TuiControlIntent::Quit {
break;
}
}
}
}
terminal.restore()?;
Ok(())
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
#[derive(Debug, Clone)]
struct DashboardOptions {
interface: String,
targets: Vec<DashboardTarget>,
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
#[derive(Debug, Clone)]
struct DashboardTarget {
config: ActuatorConfig,
protocol: DashboardProtocol,
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
#[derive(Debug, Clone)]
enum DashboardProtocol {
CubeMars {
model: MotorModel,
filter: FeedbackFilter,
},
RobStride {
status_id: CanId,
},
KnottDynamics {
feedback_ids: [CanId; 3],
},
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
impl DashboardOptions {
fn from_args() -> kcan::Result<Self> {
let mut args = std::env::args().skip(1);
let interface = args.next().unwrap_or_else(|| "can0".to_owned());
let target_args = args.collect::<Vec<_>>();
let targets = parse_targets(&target_args)?;
Ok(Self { interface, targets })
}
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
impl DashboardTarget {
fn cubemars(model: MotorModel, motor_id: u8) -> Self {
Self {
config: ActuatorConfig {
id: motor_id,
label: Some(format!("{} 0x{:02X}", model.name(), motor_id)),
protocol: ActuatorProtocol::CubeMars(model),
},
protocol: DashboardProtocol::CubeMars {
model,
filter: FeedbackFilter::for_motor(model, motor_id),
},
}
}
fn robstride(model: RobStrideModel, motor_id: u8) -> kcan::Result<Self> {
Ok(Self {
config: ActuatorConfig {
id: motor_id,
label: Some(format!("{} 0x{:02X}", model.name(), motor_id)),
protocol: ActuatorProtocol::RobStride(model),
},
protocol: DashboardProtocol::RobStride {
status_id: RobStrideFrameKind::Status.can_id(motor_id)?,
},
})
}
fn knott_dynamics(model: KnottDynamicsModel, node_id: u8) -> kcan::Result<Self> {
Ok(Self {
config: ActuatorConfig {
id: node_id,
label: Some(format!("{} node {}", model.name(), node_id)),
protocol: ActuatorProtocol::KnottDynamics(model),
},
protocol: DashboardProtocol::KnottDynamics {
feedback_ids: [
KnottDynamicsFunction::TransmitPdo2.can_id(node_id)?,
KnottDynamicsFunction::TransmitPdo3.can_id(node_id)?,
KnottDynamicsFunction::TransmitPdo4.can_id(node_id)?,
],
},
})
}
fn snapshot_without_feedback(&self) -> ActuatorSnapshot {
ActuatorSnapshot {
config: self.config.clone(),
feedback: None,
}
}
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn snapshot_from_frame(
targets: &[DashboardTarget],
frame: &CanFrame,
) -> kcan::Result<Option<ActuatorSnapshot>> {
for target in targets {
match &target.protocol {
DashboardProtocol::CubeMars { model, filter } if filter.matches(frame) => {
if is_position_32bit_feedback_frame(frame, target.config.id) {
return Ok(None);
}
let feedback = if is_servo_feedback_frame(frame, target.config.id) {
parse_servo_feedback(frame.data())?
} else {
model.parse_feedback(frame.data())?
};
return Ok(Some(ActuatorSnapshot {
config: target.config.clone(),
feedback: Some(ActuatorFeedback::CubeMars(feedback)),
}));
}
DashboardProtocol::RobStride { status_id } if frame.id() == *status_id => {
if let Some(feedback) = RobStrideFeedback::parse(frame.data()) {
return Ok(Some(ActuatorSnapshot {
config: target.config.clone(),
feedback: Some(ActuatorFeedback::RobStride(feedback)),
}));
}
}
DashboardProtocol::KnottDynamics { feedback_ids }
if feedback_ids.contains(&frame.id()) =>
{
if let Some(feedback) = parse_knott_dynamics_feedback(frame)? {
return Ok(Some(ActuatorSnapshot {
config: target.config.clone(),
feedback: Some(ActuatorFeedback::KnottDynamics(feedback)),
}));
}
}
_ => {}
}
}
Ok(None)
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_targets(args: &[String]) -> kcan::Result<Vec<DashboardTarget>> {
let Some(first) = args.first() else {
return Ok(vec![DashboardTarget::cubemars(MotorModel::Ak60_6, 0x03)]);
};
if let Some(model) = parse_cubemars_model(first) {
let ids = parse_motor_ids_or_default(&args[1..], 0x03)?;
return Ok(ids
.into_iter()
.map(|motor_id| DashboardTarget::cubemars(model, motor_id))
.collect());
}
args.iter().map(|arg| parse_target(arg)).collect()
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_motor_ids_or_default(args: &[String], default_id: u8) -> kcan::Result<Vec<u8>> {
if args.is_empty() {
Ok(vec![default_id])
} else {
args.iter().map(|value| parse_motor_id(value)).collect()
}
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_target(value: &str) -> kcan::Result<DashboardTarget> {
let parts = value.split(':').collect::<Vec<_>>();
match parts.as_slice() {
[family, model, motor_id] if is_cubemars_family(family) => {
let model = parse_required_cubemars_model(model)?;
Ok(DashboardTarget::cubemars(model, parse_motor_id(motor_id)?))
}
[family, model, motor_id] if is_robstride_family(family) => {
let model = parse_required_robstride_model(model)?;
DashboardTarget::robstride(model, parse_motor_id(motor_id)?)
}
[family, model, node_id] if is_knott_dynamics_family(family) => {
let model = parse_required_knott_dynamics_model(model)?;
DashboardTarget::knott_dynamics(model, parse_motor_id(node_id)?)
}
[model, motor_id] => {
if let Some(model) = parse_cubemars_model(model) {
return Ok(DashboardTarget::cubemars(model, parse_motor_id(motor_id)?));
}
if let Some(model) = parse_robstride_model(model) {
return DashboardTarget::robstride(model, parse_motor_id(motor_id)?);
}
if let Some(model) = parse_knott_dynamics_model(model) {
return DashboardTarget::knott_dynamics(model, parse_motor_id(motor_id)?);
}
Err(target_error(value))
}
_ => Err(target_error(value)),
}
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn is_cubemars_family(value: &str) -> bool {
matches!(
value.trim().to_ascii_lowercase().as_str(),
"cubemars" | "cm"
)
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn is_robstride_family(value: &str) -> bool {
matches!(
value.trim().to_ascii_lowercase().as_str(),
"robstride" | "rs" | "seeed"
)
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn is_knott_dynamics_family(value: &str) -> bool {
matches!(
value.trim().to_ascii_lowercase().as_str(),
"knott" | "knott-dynamics" | "knottdynamics" | "kd"
)
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_required_cubemars_model(value: &str) -> kcan::Result<MotorModel> {
parse_cubemars_model(value).ok_or_else(|| {
Error::Transport(format!(
"unsupported CubeMars model {value:?}; run `cargo run --example cubemars_models` for names"
))
})
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_required_robstride_model(value: &str) -> kcan::Result<RobStrideModel> {
parse_robstride_model(value).ok_or_else(|| {
Error::Transport(format!(
"RobStride model must be rs00 through rs06, got {value:?}"
))
})
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_required_knott_dynamics_model(value: &str) -> kcan::Result<KnottDynamicsModel> {
parse_knott_dynamics_model(value).ok_or_else(|| {
Error::Transport(format!(
"Knott Dynamics model must be K00 or K01, got {value:?}"
))
})
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_cubemars_model(value: &str) -> Option<MotorModel> {
MotorModel::from_name(value)
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_robstride_model(value: &str) -> Option<RobStrideModel> {
match value.trim().to_ascii_lowercase().as_str() {
"rs00" | "rs-00" | "rs0" => Some(RobStrideModel::Rs00),
"rs01" | "rs-01" | "rs1" => Some(RobStrideModel::Rs01),
"rs02" | "rs-02" | "rs2" => Some(RobStrideModel::Rs02),
"rs03" | "rs-03" | "rs3" => Some(RobStrideModel::Rs03),
"rs04" | "rs-04" | "rs4" => Some(RobStrideModel::Rs04),
"rs05" | "rs-05" | "rs5" => Some(RobStrideModel::Rs05),
"rs06" | "rs-06" | "rs6" => Some(RobStrideModel::Rs06),
_ => None,
}
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_knott_dynamics_model(value: &str) -> Option<KnottDynamicsModel> {
KnottDynamicsModel::from_name(value)
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn parse_motor_id(value: &str) -> kcan::Result<u8> {
let value = value.trim();
let parsed = if let Some(hex) = value
.strip_prefix("0x")
.or_else(|| value.strip_prefix("0X"))
{
u8::from_str_radix(hex, 16)
} else {
value.parse::<u8>()
};
parsed.map_err(|err| {
Error::Transport(format!(
"motor id must be decimal 0..255 or hex 0x00..0xFF, got {value:?}: {err}"
))
})
}
#[cfg(all(feature = "socketcan", feature = "tui"))]
fn target_error(value: &str) -> Error {
Error::Transport(format!(
"target must look like cubemars:ak60-6:0x03, robstride:rs01:0x01, or knott:k01:0x01, got {value:?}"
))
}
#[cfg(all(test, feature = "socketcan", feature = "tui"))]
mod tests {
use super::*;
#[test]
fn parses_mixed_target_specs() {
let args = vec![
"cubemars:ak60-6:0x03".to_owned(),
"cubemars:ak100-33:0x04".to_owned(),
"robstride:rs01:1".to_owned(),
"knott:k01:2".to_owned(),
];
let targets = parse_targets(&args).unwrap();
assert_eq!(targets.len(), 4);
assert_eq!(
targets[0].config.protocol,
ActuatorProtocol::CubeMars(MotorModel::Ak60_6)
);
assert_eq!(targets[0].config.id, 0x03);
assert_eq!(
targets[1].config.protocol,
ActuatorProtocol::CubeMars(MotorModel::Ak100_33)
);
assert_eq!(targets[1].config.id, 0x04);
assert_eq!(
targets[2].config.protocol,
ActuatorProtocol::RobStride(RobStrideModel::Rs01)
);
assert_eq!(targets[2].config.id, 0x01);
assert_eq!(
targets[3].config.protocol,
ActuatorProtocol::KnottDynamics(KnottDynamicsModel::K01)
);
assert_eq!(targets[3].config.id, 0x02);
}
#[test]
fn preserves_cubemars_shorthand() {
let args = vec!["ak80-6".to_owned(), "0x04".to_owned(), "5".to_owned()];
let targets = parse_targets(&args).unwrap();
assert_eq!(targets.len(), 2);
assert_eq!(
targets[0].config.protocol,
ActuatorProtocol::CubeMars(MotorModel::Ak80_6)
);
assert_eq!(targets[0].config.id, 0x04);
assert_eq!(targets[1].config.id, 5);
}
#[test]
fn routes_v3_servo_status_and_ignores_position_only_frames() {
let targets = vec![DashboardTarget::cubemars(MotorModel::Ak80_6, 0x03)];
let status = CanFrame::new_padded(
CanId::Extended(0x2903),
&[0x00, 0x64, 0x00, 0x0A, 0xFF, 0x9C, 0x19, 0x00],
)
.unwrap();
let position = CanFrame::new(CanId::Extended(0x2A03), &123_456_i32.to_be_bytes()).unwrap();
let snapshot = snapshot_from_frame(&targets, &status).unwrap().unwrap();
let Some(ActuatorFeedback::CubeMars(feedback)) = snapshot.feedback else {
panic!("expected CubeMars feedback");
};
assert_eq!(feedback.position_degrees, 10.0);
assert_eq!(feedback.speed_erpm, 100);
assert!(snapshot_from_frame(&targets, &position).unwrap().is_none());
}
#[test]
fn routes_knott_dynamics_feedback_by_node() {
let targets = vec![DashboardTarget::knott_dynamics(KnottDynamicsModel::K00, 2).unwrap()];
let mut data = [0_u8; 8];
data[..4].copy_from_slice(&1.25_f32.to_le_bytes());
data[4..].copy_from_slice(&(-0.5_f32).to_le_bytes());
let feedback = CanFrame::new(
KnottDynamicsFunction::TransmitPdo2.can_id(2).unwrap(),
&data,
)
.unwrap();
let snapshot = snapshot_from_frame(&targets, &feedback).unwrap().unwrap();
let Some(ActuatorFeedback::KnottDynamics(feedback)) = snapshot.feedback else {
panic!("expected Knott Dynamics feedback");
};
assert_eq!(feedback.node_id, 2);
assert_eq!(feedback.position_rad, 1.25);
assert_eq!(feedback.velocity_rad_s, Some(-0.5));
}
}
#[cfg(not(all(feature = "socketcan", feature = "tui")))]
fn main() {
eprintln!("enable the `socketcan` and `tui` features to run this example");
}