#![forbid(unsafe_code)]
use std::env;
use std::process;
use kcan::protocol::{direct_command_frame, mit_command_frame, mit_helper_frame};
use kcan::protocols::robstride::{robstride_command_frame, robstride_status_query_frame};
use kcan::{
CanFrame, DirectCommand, FeedbackMessageConfig, KnottDynamicsFlashCommand, KnottDynamicsMode,
KnottDynamicsModel, KnottDynamicsParameter, KnottDynamicsParameterType,
KnottDynamicsParameterValue, MitCommand, MitHelperCommand, MotorModel, OriginMode,
RobStrideCommand, RobStrideFrameKind, RobStrideMitCommand, RobStrideModel,
RobStridePositionCommand, RobStrideRawCommand, RobStrideSpeedCommand,
knott_dynamics_disable_frame, knott_dynamics_flash_frame_in_mode,
knott_dynamics_heartbeat_frame, knott_dynamics_nmt_frame_from_mode,
knott_dynamics_opaque_watchdog_frame, knott_dynamics_parameter_read_frame,
knott_dynamics_parameter_write_frame_in_mode, knott_dynamics_position_torque_frame,
knott_dynamics_position_velocity_frame, knott_dynamics_probe_frame,
};
const HELP: &str = "kcan - Knott Dynamics CAN actuator command
Usage:
kcan help
kcan models [cubemars|robstride|knott-dynamics]
kcan cubemars mit <model> <id> [position_rad velocity_rad_s kp kd torque_nm]
kcan cubemars helper <model> <id> <enable|disable|zero|enable-legacy|disable-legacy>
kcan cubemars direct <id> <duty|current|brake-current|velocity|position|origin|position-velocity|disable|feedback-config> <values...>
feedback-config flags: <position-32bit|single-turn|clear> [...]
kcan robstride status <model> <id>
kcan robstride mit <model> <id> [position_rad velocity_rad_s kp kd torque_nm]
kcan robstride position <model> <id> <position_rad> <velocity_rad_s> <max_torque_nm>
kcan robstride speed <model> <id> <velocity_rad_s> <max_torque_nm>
kcan robstride raw <model> <id> <mit|position|speed|status|config> <8 hex bytes>
kcan knott nmt <broadcast|node> <current_mode> <requested_mode>
kcan knott disable <broadcast|node>
kcan knott probe <node>
kcan knott position-velocity <node> <position_rad> <velocity_rad_s>
kcan knott position-torque <node> <position_rad> <torque_nm>
kcan knott watchdog <node> <8 hex bytes>
kcan knott heartbeat <node>
kcan knott flash <node> <current_mode> <store|load>
kcan knott read <node> <parameter>
kcan knott write <node> <current_mode> <parameter> <value>
Scrin and Aisling dashboards stay available through feature-gated examples.
";
fn main() {
if let Err(message) = run(env::args().skip(1).collect()) {
eprintln!("error: {message}");
eprintln!("run `kcan help` for usage");
process::exit(2);
}
}
fn run(args: Vec<String>) -> Result<(), String> {
let output = dispatch(&args)?;
if !output.is_empty() {
println!("{output}");
}
Ok(())
}
fn dispatch(args: &[String]) -> Result<String, String> {
match args {
[] => Ok(HELP.trim_end().to_owned()),
[command] if command == "help" || command == "--help" || command == "-h" => {
Ok(HELP.trim_end().to_owned())
}
[command, rest @ ..] if command == "models" => models_command(rest),
[family, rest @ ..] if family == "cubemars" => cubemars_command(rest),
[family, rest @ ..] if family == "robstride" || family == "rs" => robstride_command(rest),
[family, rest @ ..]
if family == "knott" || family == "knott-dynamics" || family == "kd" =>
{
knott_dynamics_command(rest)
}
[other, ..] => Err(format!("unknown command {other:?}")),
}
}
fn models_command(args: &[String]) -> Result<String, String> {
match args {
[] => Ok(format!(
"CubeMars:\n{}\n\nRobStride:\n{}\n\nKnott Dynamics:\n{}",
cubemars_models(),
robstride_models(),
knott_dynamics_models(),
)),
[family] if family == "cubemars" || family == "cm" => Ok(cubemars_models()),
[family] if family == "robstride" || family == "rs" => Ok(robstride_models()),
[family] if family == "knott" || family == "knott-dynamics" || family == "kd" => {
Ok(knott_dynamics_models())
}
[other, ..] => Err(format!("unknown model family {other:?}")),
}
}
fn cubemars_models() -> String {
MotorModel::all()
.iter()
.map(|model| {
let spec = model.spec();
format!(
"{:<9} layout={:<8} gear={} max_erpm={}",
model.name(),
if model.uses_extended_mit_layout() {
"extended"
} else {
"standard"
},
spec.gear_ratio,
spec.max_velocity_erpm
)
})
.collect::<Vec<_>>()
.join("\n")
}
fn robstride_models() -> String {
RobStrideModel::all()
.iter()
.map(|model| {
let spec = model.spec();
format!(
"{:<5} torque={:.1}Nm speed={:.0}rpm kp={:.0}..{:.0} kd={:.0}..{:.0}",
model.name(),
spec.max_torque_nm,
spec.max_speed_rpm,
spec.kp_min,
spec.kp_max,
spec.kd_min,
spec.kd_max
)
})
.collect::<Vec<_>>()
.join("\n")
}
fn knott_dynamics_models() -> String {
KnottDynamicsModel::all()
.iter()
.map(|model| {
format!(
"{:<3} size={} protocol=shared",
model.name(),
model.size_class()
)
})
.collect::<Vec<_>>()
.join("\n")
}
fn cubemars_command(args: &[String]) -> Result<String, String> {
match args {
[command, rest @ ..] if command == "mit" => {
let frame = cubemars_mit(rest)?;
Ok(format_frame(&frame))
}
[command, rest @ ..] if command == "helper" => {
let frame = cubemars_helper(rest)?;
Ok(format_frame(&frame))
}
[command, rest @ ..] if command == "direct" => {
let frame = cubemars_direct(rest)?;
Ok(format_frame(&frame))
}
[] => Err("missing CubeMars command".to_owned()),
[other, ..] => Err(format!("unknown CubeMars command {other:?}")),
}
}
fn cubemars_mit(args: &[String]) -> Result<CanFrame, String> {
let (model, motor_id, values) = parse_model_and_id(args)?;
let command = match values {
[] => MitCommand::neutral(),
[position, velocity, kp, kd, torque] => MitCommand {
position_rad: parse_f64(position, "position_rad")?,
velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
kp: parse_f64(kp, "kp")?,
kd: parse_f64(kd, "kd")?,
torque_nm: parse_f64(torque, "torque_nm")?,
},
_ => return Err("mit expects either no values or five numeric values".to_owned()),
};
mit_command_frame(model, motor_id, command).map_err(|err| err.to_string())
}
fn cubemars_helper(args: &[String]) -> Result<CanFrame, String> {
let (model, motor_id, values) = parse_model_and_id(args)?;
let [helper] = values else {
return Err("helper expects one helper name".to_owned());
};
let helper = helper
.parse::<MitHelperCommand>()
.map_err(|err| err.replace("MIT helper command", "helper"))?;
mit_helper_frame(model, motor_id, helper).map_err(|err| err.to_string())
}
fn cubemars_direct(args: &[String]) -> Result<CanFrame, String> {
let [motor_id, command, values @ ..] = args else {
return Err("direct expects an id and command".to_owned());
};
let motor_id = parse_u8(motor_id, "id")?;
let command = match (command.as_str(), values) {
("duty", [value]) => DirectCommand::DutyCycle(parse_f64(value, "duty")?),
("current", [value]) => DirectCommand::Current(parse_f64(value, "current")?),
("brake-current", [value]) => {
DirectCommand::BrakeCurrent(parse_f64(value, "brake-current")?)
}
("velocity", [value]) => DirectCommand::Velocity(parse_i32(value, "velocity")?),
("position", [value]) => DirectCommand::Position(parse_f64(value, "position")?),
("origin", [mode]) => DirectCommand::SetOrigin(parse_origin_mode(mode)?),
("position-velocity", [position, velocity, acceleration]) => {
DirectCommand::PositionVelocity {
position_degrees: parse_f64(position, "position")?,
velocity_erpm: parse_i32(velocity, "velocity")?,
acceleration_erpm_per_sec: parse_i32(acceleration, "acceleration")?,
}
}
("disable", []) => DirectCommand::DisableMotor,
("feedback-config", flags) => {
DirectCommand::ConfigureFeedback(parse_feedback_config(flags)?)
}
(other, _) => return Err(format!("unknown or incomplete direct command {other:?}")),
};
direct_command_frame(motor_id, command).map_err(|err| err.to_string())
}
fn parse_feedback_config(flags: &[String]) -> Result<FeedbackMessageConfig, String> {
if flags.is_empty() {
return Err(
"feedback-config requires position-32bit, single-turn, or explicit clear".to_owned(),
);
}
if flags.iter().any(|flag| flag == "clear") {
return if flags.len() == 1 {
Ok(FeedbackMessageConfig::new())
} else {
Err("feedback-config clear cannot be combined with other flags".to_owned())
};
}
let mut config = FeedbackMessageConfig::new();
for flag in flags {
match flag.as_str() {
"position-32bit" => config.position_32bit_feedback = true,
"single-turn" => config.single_turn_position = true,
other => return Err(format!("unknown feedback-config flag {other:?}")),
}
}
Ok(config)
}
fn robstride_command(args: &[String]) -> Result<String, String> {
match args {
[command, rest @ ..] if command == "status" => {
let (_model, motor_id, values) = parse_robstride_model_and_id(rest)?;
if !values.is_empty() {
return Err("status does not take payload values".to_owned());
}
Ok(format_frame(
&robstride_status_query_frame(motor_id).map_err(|err| err.to_string())?,
))
}
[command, rest @ ..] if command == "mit" => {
let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
let command = match values {
[] => RobStrideMitCommand::neutral(),
[position, velocity, kp, kd, torque] => RobStrideMitCommand {
position_rad: parse_f64(position, "position_rad")?,
velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
kp: parse_f64(kp, "kp")?,
kd: parse_f64(kd, "kd")?,
torque_nm: parse_f64(torque, "torque_nm")?,
},
_ => return Err("mit expects either no values or five numeric values".to_owned()),
};
Ok(format_frame(
&robstride_command_frame(model, motor_id, RobStrideCommand::Mit(command))
.map_err(|err| err.to_string())?,
))
}
[command, rest @ ..] if command == "position" => {
let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
let [position, velocity, torque] = values else {
return Err("position expects position, velocity, and max torque".to_owned());
};
Ok(format_frame(
&robstride_command_frame(
model,
motor_id,
RobStrideCommand::Position(RobStridePositionCommand {
position_rad: parse_f64(position, "position_rad")?,
velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
max_torque_nm: parse_f64(torque, "max_torque_nm")?,
}),
)
.map_err(|err| err.to_string())?,
))
}
[command, rest @ ..] if command == "speed" => {
let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
let [velocity, torque] = values else {
return Err("speed expects velocity and max torque".to_owned());
};
Ok(format_frame(
&robstride_command_frame(
model,
motor_id,
RobStrideCommand::Speed(RobStrideSpeedCommand {
velocity_rad_s: parse_f64(velocity, "velocity_rad_s")?,
max_torque_nm: parse_f64(torque, "max_torque_nm")?,
}),
)
.map_err(|err| err.to_string())?,
))
}
[command, rest @ ..] if command == "raw" => {
let (model, motor_id, values) = parse_robstride_model_and_id(rest)?;
let [kind, bytes @ ..] = values else {
return Err("raw expects kind and 8 hex bytes".to_owned());
};
if bytes.len() != 8 {
return Err("raw expects exactly 8 hex bytes".to_owned());
}
let mut data = [0_u8; 8];
for (index, value) in bytes.iter().enumerate() {
data[index] = parse_hex_byte(value)?;
}
let kind = parse_robstride_kind(kind)?;
Ok(format_frame(
&robstride_command_frame(
model,
motor_id,
RobStrideCommand::Raw(RobStrideRawCommand { kind, data }),
)
.map_err(|err| err.to_string())?,
))
}
[] => Err("missing RobStride command".to_owned()),
[other, ..] => Err(format!("unknown RobStride command {other:?}")),
}
}
fn knott_dynamics_command(args: &[String]) -> Result<String, String> {
let frame = match args {
[command, target, current_mode, requested_mode] if command == "nmt" => {
let target = match target.as_str() {
"all" | "broadcast" => None,
_ => Some(parse_u8(target, "node")?),
};
knott_dynamics_nmt_frame_from_mode(
target,
current_mode.parse::<KnottDynamicsMode>()?,
requested_mode.parse::<KnottDynamicsMode>()?,
)
}
[command, target] if command == "disable" => {
let target = match target.as_str() {
"all" | "broadcast" => None,
_ => Some(parse_u8(target, "node")?),
};
knott_dynamics_disable_frame(target)
}
[command, node] if command == "probe" => {
knott_dynamics_probe_frame(parse_u8(node, "node")?)
}
[command, node, position, velocity]
if command == "position-velocity" || command == "pv" =>
{
knott_dynamics_position_velocity_frame(
parse_u8(node, "node")?,
parse_f32(position, "position_rad")?,
parse_f32(velocity, "velocity_rad_s")?,
)
}
[command, node, position, torque] if command == "position-torque" || command == "pt" => {
knott_dynamics_position_torque_frame(
parse_u8(node, "node")?,
parse_f32(position, "position_rad")?,
parse_f32(torque, "torque_nm")?,
)
}
[command, node, bytes @ ..] if command == "watchdog" => {
if bytes.len() != 8 {
return Err("watchdog expects exactly 8 hex bytes".to_owned());
}
let mut data = [0_u8; 8];
for (index, value) in bytes.iter().enumerate() {
data[index] = parse_hex_byte(value)?;
}
knott_dynamics_opaque_watchdog_frame(parse_u8(node, "node")?, data)
}
[command, node] if command == "heartbeat" => {
knott_dynamics_heartbeat_frame(parse_u8(node, "node")?)
}
[command, node, current_mode, operation] if command == "flash" => {
let operation = match operation.as_str() {
"store" => KnottDynamicsFlashCommand::Store,
"load" => KnottDynamicsFlashCommand::Load,
other => return Err(format!("unknown flash operation {other:?}")),
};
knott_dynamics_flash_frame_in_mode(
parse_u8(node, "node")?,
current_mode.parse::<KnottDynamicsMode>()?,
operation,
)
}
[command, node, parameter] if command == "read" => knott_dynamics_parameter_read_frame(
parse_u8(node, "node")?,
parse_knott_dynamics_parameter(parameter)?,
),
[command, node, current_mode, parameter, value] if command == "write" => {
let parameter = parse_knott_dynamics_parameter(parameter)?;
let value = parse_knott_dynamics_parameter_value(parameter, value)?;
knott_dynamics_parameter_write_frame_in_mode(
parse_u8(node, "node")?,
current_mode.parse::<KnottDynamicsMode>()?,
parameter,
value,
)
}
[] => return Err("missing Knott Dynamics command".to_owned()),
[other, ..] => {
return Err(format!(
"unknown or incomplete Knott Dynamics command {other:?}"
));
}
};
frame
.map(|frame| format_frame(&frame))
.map_err(|err| err.to_string())
}
fn parse_knott_dynamics_parameter(value: &str) -> Result<KnottDynamicsParameter, String> {
value.parse().map_err(|err: kcan::Error| err.to_string())
}
fn parse_knott_dynamics_parameter_value(
parameter: KnottDynamicsParameter,
value: &str,
) -> Result<KnottDynamicsParameterValue, String> {
match parameter.descriptor().parameter_type {
KnottDynamicsParameterType::U32 => {
if parameter == KnottDynamicsParameter::Mode {
if let Ok(mode) = value.parse::<KnottDynamicsMode>() {
return Ok(KnottDynamicsParameterValue::U32(mode as u32));
}
}
Ok(KnottDynamicsParameterValue::U32(parse_u32(
value,
parameter.descriptor().name,
)?))
}
KnottDynamicsParameterType::I32 => Ok(KnottDynamicsParameterValue::I32(parse_i32(
value,
parameter.descriptor().name,
)?)),
KnottDynamicsParameterType::F32 => Ok(KnottDynamicsParameterValue::F32(parse_f32(
value,
parameter.descriptor().name,
)?)),
}
}
fn parse_model_and_id(args: &[String]) -> Result<(MotorModel, u8, &[String]), String> {
let [model, motor_id, rest @ ..] = args else {
return Err("expected model and id".to_owned());
};
let model = MotorModel::from_name(model)
.ok_or_else(|| format!("unsupported CubeMars model {model:?}"))?;
let motor_id = parse_u8(motor_id, "id")?;
Ok((model, motor_id, rest))
}
fn parse_robstride_model_and_id(
args: &[String],
) -> Result<(RobStrideModel, u8, &[String]), String> {
let [model, motor_id, rest @ ..] = args else {
return Err("expected model and id".to_owned());
};
let model = RobStrideModel::from_name(model)
.ok_or_else(|| format!("unsupported RobStride model {model:?}"))?;
let motor_id = parse_u8(motor_id, "id")?;
Ok((model, motor_id, rest))
}
fn parse_origin_mode(value: &str) -> Result<OriginMode, String> {
value.parse()
}
fn parse_robstride_kind(value: &str) -> Result<RobStrideFrameKind, String> {
value.parse()
}
fn parse_u8(value: &str, name: &str) -> Result<u8, String> {
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| format!("invalid {name} {value:?}: {err}"))
}
fn parse_i32(value: &str, name: &str) -> Result<i32, String> {
value
.parse::<i32>()
.map_err(|err| format!("invalid {name} {value:?}: {err}"))
}
fn parse_u32(value: &str, name: &str) -> Result<u32, String> {
let parsed = if let Some(hex) = value
.strip_prefix("0x")
.or_else(|| value.strip_prefix("0X"))
{
u32::from_str_radix(hex, 16)
} else {
value.parse::<u32>()
};
parsed.map_err(|err| format!("invalid {name} {value:?}: {err}"))
}
fn parse_f64(value: &str, name: &str) -> Result<f64, String> {
value
.parse::<f64>()
.map_err(|err| format!("invalid {name} {value:?}: {err}"))
}
fn parse_f32(value: &str, name: &str) -> Result<f32, String> {
value
.parse::<f32>()
.map_err(|err| format!("invalid {name} {value:?}: {err}"))
}
fn parse_hex_byte(value: &str) -> Result<u8, String> {
let value = value
.strip_prefix("0x")
.or_else(|| value.strip_prefix("0X"))
.unwrap_or(value);
u8::from_str_radix(value, 16).map_err(|err| format!("invalid hex byte {value:?}: {err}"))
}
fn format_frame(frame: &CanFrame) -> String {
frame.to_string()
}