use kcan::protocol::{
AK60_6_MIT_LIMITS, AK80_6_MIT_LIMITS, DirectCommand, FaultCode, MitCommand, MitHelperCommand,
MotorModel, OriginMode, degrees_to_radians, direct_command_frame, erpm_to_rad_s, float_to_uint,
mit_command_frame, mit_helper_frame, pack_mit_command, rad_s_to_erpm, radians_to_degrees,
uint_to_float,
};
use kcan::{CanId, Error};
fn assert_close(left: f64, right: f64, tolerance: f64) {
assert!(
(left - right).abs() <= tolerance,
"left={left} right={right} tolerance={tolerance}"
);
}
#[test]
fn float_to_uint_clamps_to_range() {
assert_eq!(float_to_uint(-999.0, -1.0, 1.0, 12).unwrap(), 0);
assert_eq!(float_to_uint(999.0, -1.0, 1.0, 12).unwrap(), 4095);
}
#[test]
fn uint_to_float_round_trips_midpoint() {
let raw = float_to_uint(0.0, -12.56, 12.56, 16).unwrap();
let value = uint_to_float(raw, -12.56, 12.56, 16).unwrap();
assert_close(value, 0.0, 0.001);
}
#[test]
fn invalid_bit_width_is_rejected() {
assert_eq!(
float_to_uint(0.0, -1.0, 1.0, 0).unwrap_err(),
Error::InvalidBitWidth(0)
);
}
#[test]
fn ak60_mit_frame_uses_extended_layout_byte_order() {
let limits = AK60_6_MIT_LIMITS;
let p_int = float_to_uint(1.0, limits.position_min_rad, limits.position_max_rad, 16).unwrap();
let v_int = float_to_uint(
2.0,
limits.velocity_min_rad_s,
limits.velocity_max_rad_s,
12,
)
.unwrap();
let kp_int = float_to_uint(30.0, limits.kp_min, limits.kp_max, 12).unwrap();
let kd_int = float_to_uint(1.5, limits.kd_min, limits.kd_max, 12).unwrap();
let t_int = float_to_uint(3.0, limits.torque_min_nm, limits.torque_max_nm, 12).unwrap();
let expected = [
(kp_int >> 4) as u8,
(((kp_int & 0xF) << 4) | (kd_int >> 8)) as u8,
(kd_int & 0xFF) as u8,
(p_int >> 8) as u8,
(p_int & 0xFF) as u8,
(v_int >> 4) as u8,
(((v_int & 0xF) << 4) | (t_int >> 8)) as u8,
(t_int & 0xFF) as u8,
];
let payload = pack_mit_command(
MotorModel::Ak60_6,
MitCommand {
position_rad: 1.0,
velocity_rad_s: 2.0,
kp: 30.0,
kd: 1.5,
torque_nm: 3.0,
},
)
.unwrap();
assert_eq!(payload, expected);
}
#[test]
fn ak80_mit_frame_uses_standard_layout() {
let payload = pack_mit_command(
MotorModel::Ak80_6,
MitCommand {
position_rad: 0.0,
velocity_rad_s: 0.0,
kp: 0.0,
kd: 0.0,
torque_nm: 0.0,
},
)
.unwrap();
assert_eq!(payload.len(), 8);
assert_eq!(payload[0], 0x7F);
assert_eq!(payload[1], 0xFF);
}
#[test]
fn mit_command_frame_uses_model_specific_id() {
let ak60 = mit_command_frame(MotorModel::Ak60_6, 0x03, MitCommand::neutral()).unwrap();
let ak80 = mit_command_frame(MotorModel::Ak80_6, 0x03, MitCommand::neutral()).unwrap();
assert_eq!(ak60.id(), CanId::Extended(0x0803));
assert_eq!(ak80.id(), CanId::Standard(0x0003));
assert_eq!(ak60.len(), 8);
}
#[test]
fn helper_frame_uses_raw_motor_id() {
let frame = mit_helper_frame(MotorModel::Ak60_6, 0x03, MitHelperCommand::ZeroPosition).unwrap();
assert_eq!(frame.id(), CanId::Extended(0x03));
assert_eq!(
frame.data(),
&[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE]
);
}
#[test]
fn feedback_decodes_ak60_status_frame() {
let data = [0x00, 0x64, 0x00, 0x0A, 0xFF, 0x9C, 0x19, 0x02];
let feedback = MotorModel::Ak60_6.parse_feedback(&data).unwrap();
assert_close(feedback.position_degrees, 10.0, f64::EPSILON);
assert_eq!(feedback.speed_erpm, 100);
assert_close(feedback.current_amps, -1.0, f64::EPSILON);
assert_eq!(feedback.temperature_celsius, 25);
assert_eq!(feedback.error_code, 2);
assert_eq!(feedback.fault_code(), Some(FaultCode::OverCurrent));
assert!(feedback.has_fault());
assert_eq!(feedback.error_description(), "Over-current");
}
#[test]
fn fault_codes_map_known_errors() {
assert_eq!(FaultCode::from_code(0), Some(FaultCode::None));
assert_eq!(FaultCode::from_code(7), Some(FaultCode::MotorLockUp));
assert_eq!(FaultCode::from_code(99), None);
assert_eq!(FaultCode::UnderVoltage.code(), 4);
assert_eq!(FaultCode::UnderVoltage.description(), "Under-voltage");
}
#[test]
fn direct_duty_frame_uses_extended_mode_id_and_scaled_i32() {
let frame = direct_command_frame(0x03, DirectCommand::DutyCycle(0.5)).unwrap();
assert_eq!(frame.id(), CanId::Extended(0x0003));
assert_eq!(frame.len(), 4);
assert_eq!(&frame.data()[..4], &50_000_i32.to_be_bytes());
}
#[test]
fn direct_current_and_brake_frames_use_milliamp_scaling() {
let current = direct_command_frame(0x03, DirectCommand::Current(2.5)).unwrap();
let brake = direct_command_frame(0x03, DirectCommand::BrakeCurrent(-1.5)).unwrap();
assert_eq!(current.id(), CanId::Extended(0x0103));
assert_eq!(¤t.data()[..4], &2_500_i32.to_be_bytes());
assert_eq!(brake.id(), CanId::Extended(0x0203));
assert_eq!(&brake.data()[..4], &(-1_500_i32).to_be_bytes());
}
#[test]
fn direct_velocity_frame_clamps_protocol_range() {
let frame = direct_command_frame(0x03, DirectCommand::Velocity(999_999)).unwrap();
assert_eq!(frame.id(), CanId::Extended(0x0303));
assert_eq!(frame.len(), 4);
assert_eq!(&frame.data()[..4], &100_000_i32.to_be_bytes());
}
#[test]
fn direct_position_frame_uses_degree_scaling() {
let frame = direct_command_frame(0x03, DirectCommand::Position(10.0)).unwrap();
assert_eq!(frame.id(), CanId::Extended(0x0403));
assert_eq!(frame.len(), 4);
assert_eq!(&frame.data()[..4], &100_000_i32.to_be_bytes());
}
#[test]
fn direct_origin_frame_uses_origin_mode_byte() {
let frame =
direct_command_frame(0x03, DirectCommand::SetOrigin(OriginMode::Permanent)).unwrap();
assert_eq!(frame.id(), CanId::Extended(0x0503));
assert_eq!(frame.len(), 1);
assert_eq!(frame.data()[0], 0x01);
}
#[test]
fn direct_position_velocity_frame_uses_profile_layout() {
let frame = direct_command_frame(
0x03,
DirectCommand::PositionVelocity {
position_degrees: 1.5,
velocity_erpm: -100,
acceleration_erpm_per_sec: 200,
},
)
.unwrap();
assert_eq!(frame.id(), CanId::Extended(0x0603));
assert_eq!(&frame.data()[..4], &15_000_i32.to_be_bytes());
assert_eq!(&frame.data()[4..6], &(-10_i16).to_be_bytes());
assert_eq!(&frame.data()[6..8], &20_i16.to_be_bytes());
}
#[test]
fn direct_payload_bytes_match_frame_payload_without_vec() {
let command = DirectCommand::PositionVelocity {
position_degrees: 1.5,
velocity_erpm: -100,
acceleration_erpm_per_sec: 200,
};
let payload = command.payload_bytes();
let frame = direct_command_frame(0x03, command).unwrap();
assert_eq!(payload.len(), 8);
assert!(!payload.is_empty());
assert_eq!(payload.data(), frame.data());
assert_eq!(payload.data(), command.payload().as_slice());
assert_eq!(payload.padded_data(), frame.padded_data());
let origin = DirectCommand::SetOrigin(OriginMode::Temporary).payload_bytes();
assert_eq!(origin.len(), 1);
assert_eq!(origin.data(), &[0x00]);
assert_eq!(origin.padded_data()[1], 0x00);
}
#[test]
fn model_limits_follow_cube_mars_ranges() {
assert_eq!(AK60_6_MIT_LIMITS.velocity_max_rad_s, 60.0);
assert_eq!(AK60_6_MIT_LIMITS.torque_max_nm, 12.0);
assert_eq!(AK80_6_MIT_LIMITS.position_min_rad, -12.5);
assert_eq!(AK80_6_MIT_LIMITS.position_max_rad, 12.5);
assert_eq!(MotorModel::Ak60_6.spec().max_velocity_erpm, 48_128);
assert_eq!(MotorModel::Ak80_6.spec().max_velocity_erpm, 91_444);
}
#[test]
fn cubemars_supported_models_pack_neutral_mit_frames() {
assert!(MotorModel::all().len() > 10);
assert_eq!(MotorModel::from_name("ak90-9"), Some(MotorModel::Ak90_9));
assert_eq!(
MotorModel::from_name("AK100_33"),
Some(MotorModel::Ak100_33)
);
assert_eq!(MotorModel::from_name("unknown"), None);
assert_eq!(MotorModel::Ak60_6.to_string(), "AK60-6");
assert_eq!("ak60-6".parse::<MotorModel>().unwrap(), MotorModel::Ak60_6);
assert_eq!(
"AK100_33".parse::<MotorModel>().unwrap(),
MotorModel::Ak100_33
);
assert_eq!(
"unknown".parse::<MotorModel>().unwrap_err(),
Error::UnknownMotorLabel("unknown".to_owned())
);
for model in MotorModel::all() {
assert!(!model.name().is_empty());
let frame = mit_command_frame(*model, 0x03, MitCommand::neutral()).unwrap();
assert_eq!(frame.len(), 8);
if model.uses_extended_mit_layout() {
assert_eq!(frame.id(), CanId::Extended(0x0803));
} else {
assert_eq!(frame.id(), CanId::Standard(0x0003));
}
}
}
#[test]
fn unit_conversions_round_trip() {
let spec = MotorModel::Ak60_6.spec();
let rad_s = erpm_to_rad_s(7_840, spec);
assert_eq!(rad_s_to_erpm(rad_s, spec), 7_840);
assert_close(radians_to_degrees(degrees_to_radians(90.0)), 90.0, 1e-12);
}