#![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, MitCommand, MitHelperCommand, MotorModel, OriginMode,
RobStrideCommand, RobStrideFrameKind, RobStrideMitCommand, RobStrideModel,
RobStridePositionCommand, RobStrideRawCommand, RobStrideSpeedCommand,
};
const HELP: &str = "kcan - Knott Dynamics CAN actuator command
Usage:
kcan help
kcan models [cubemars|robstride]
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> <values...>
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>
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),
[other, ..] => Err(format!("unknown command {other:?}")),
}
}
fn models_command(args: &[String]) -> Result<String, String> {
match args {
[] => Ok(format!(
"CubeMars:\n{}\n\nRobStride:\n{}",
cubemars_models(),
robstride_models()
)),
[family] if family == "cubemars" || family == "cm" => Ok(cubemars_models()),
[family] if family == "robstride" || family == "rs" => Ok(robstride_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 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, command, and values".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")?,
}
}
(other, _) => return Err(format!("unknown or incomplete direct command {other:?}")),
};
direct_command_frame(motor_id, command).map_err(|err| err.to_string())
}
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 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_f64(value: &str, name: &str) -> Result<f64, String> {
value
.parse::<f64>()
.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 {
let id_kind = if frame.id().is_extended() {
"extended"
} else {
"standard"
};
format!(
"{} 0x{:X} len={} data={}",
id_kind,
frame.id().raw(),
frame.len(),
format_data(frame.data())
)
}
fn format_data(data: &[u8]) -> String {
data.iter()
.map(|byte| format!("{byte:02X}"))
.collect::<Vec<_>>()
.join(" ")
}