use std::collections::VecDeque;
use std::time::Duration;
use kcan::protocol::{AK60_6_MIT_LIMITS, erpm_to_rad_s, float_to_uint};
use kcan::{
CanBus, CanFrame, CanId, DirectCommand, HelperPolicy, MitHelperCommand, MotorConfig,
MotorController, MotorManager, MotorModel, OriginMode, Result,
};
#[derive(Debug, Default)]
struct RecordingBus {
sent: Vec<CanFrame>,
received: VecDeque<CanFrame>,
}
impl RecordingBus {
fn with_received(frames: impl IntoIterator<Item = CanFrame>) -> Self {
Self {
sent: Vec::new(),
received: frames.into_iter().collect(),
}
}
}
impl CanBus for RecordingBus {
fn send(&mut self, frame: &CanFrame) -> Result<()> {
self.sent.push(frame.clone());
Ok(())
}
fn receive(&mut self, _timeout: Duration) -> Result<Option<CanFrame>> {
Ok(self.received.pop_front())
}
}
fn strict_config() -> MotorConfig {
MotorConfig {
helper_policy: HelperPolicy::Strict,
..MotorConfig::default()
}
}
#[test]
fn set_velocity_sends_enable_then_velocity_command() {
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.set_velocity_erpm(1_000).unwrap();
let sent = &motor.bus().sent;
assert_eq!(sent.len(), 2);
assert_eq!(sent[0].id(), CanId::Extended(0x0803));
assert_eq!(sent[1].id(), CanId::Extended(0x0803));
assert!(motor.mit_enabled());
let active = motor.active_command().unwrap();
assert_eq!(active.kp, 0.0);
assert_eq!(active.kd, MotorModel::Ak60_6.spec().mit_velocity_kd);
assert_eq!(
active.velocity_rad_s,
erpm_to_rad_s(1_000, MotorModel::Ak60_6.spec())
);
}
#[test]
fn set_velocity_clamps_to_motor_spec() {
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.set_velocity_erpm(100_000).unwrap();
let active = motor.active_command().unwrap();
assert_eq!(
active.velocity_rad_s,
erpm_to_rad_s(
MotorModel::Ak60_6.spec().max_velocity_erpm,
MotorModel::Ak60_6.spec()
)
);
}
#[test]
fn zero_position_sends_helper_frame() {
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.zero_position().unwrap();
assert_eq!(motor.bus().sent.len(), 1);
assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x03));
assert_eq!(
motor.bus().sent[0].data(),
MitHelperCommand::ZeroPosition.data()
);
}
#[test]
fn receive_feedback_filters_and_decodes_frames() {
let unrelated = CanFrame::new_padded(CanId::Extended(0x9999), &[0; 8]).unwrap();
let feedback_frame = CanFrame::new_padded(
CanId::Extended(0x2903),
&[0x00, 0x64, 0x00, 0x0A, 0xFF, 0x9C, 0x19, 0x00],
)
.unwrap();
let bus = RecordingBus::with_received([unrelated, feedback_frame]);
let mut motor = MotorController::new(bus, strict_config()).unwrap();
let feedback = motor
.receive_feedback(Duration::from_millis(10))
.unwrap()
.unwrap();
assert_eq!(feedback.position_degrees, 10.0);
assert_eq!(feedback.speed_erpm, 100);
assert_eq!(feedback.current_amps, -1.0);
assert!(motor.last_feedback().is_some());
}
#[test]
fn refresh_once_resends_active_command_and_captures_feedback() {
let feedback_frame = CanFrame::new_padded(
CanId::Extended(0x2903),
&[0x00, 0x64, 0x00, 0x0A, 0xFF, 0x9C, 0x19, 0x00],
)
.unwrap();
let bus = RecordingBus::with_received([feedback_frame]);
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.set_velocity_erpm(1_000).unwrap();
motor.bus_mut().sent.clear();
let feedback = motor
.refresh_once(Duration::from_millis(1))
.unwrap()
.unwrap();
assert_eq!(motor.bus().sent.len(), 1);
assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x0803));
assert_eq!(feedback.speed_erpm, 100);
}
#[test]
fn refresh_for_reports_iterations_and_missed_feedback() {
let mut config = strict_config();
config.control_rate_hz = 1_000.0;
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, config).unwrap();
let stats = motor
.refresh_for(Duration::from_millis(2), Duration::ZERO)
.unwrap();
assert!(stats.iterations >= 1);
assert_eq!(stats.feedback_frames, 0);
assert_eq!(stats.missed_feedback, stats.iterations);
assert!(stats.effective_hz().is_some());
assert_eq!(stats.feedback_ratio(), Some(0.0));
}
#[test]
fn position_command_uses_profile_gains() {
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.set_position_degrees(90.0).unwrap();
let active = motor.active_command().unwrap();
assert_eq!(active.kp, MotorModel::Ak60_6.spec().mit_position_kp);
assert_eq!(active.kd, MotorModel::Ak60_6.spec().mit_position_kd);
assert!((active.position_rad - 90_f64.to_radians()).abs() < 1e-12);
}
#[test]
fn torque_command_is_saturated_by_packer() {
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.set_torque_nm(999.0).unwrap();
let payload = motor.bus().sent.last().unwrap().data();
let torque_raw = (u32::from(payload[6] & 0x0F) << 8) | u32::from(payload[7]);
assert_eq!(
torque_raw,
float_to_uint(
999.0,
AK60_6_MIT_LIMITS.torque_min_nm,
AK60_6_MIT_LIMITS.torque_max_nm,
12
)
.unwrap()
);
}
#[test]
fn direct_current_method_sends_current_loop_frame() {
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.set_current_amps(3.0).unwrap();
assert_eq!(motor.bus().sent.len(), 1);
assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x0103));
assert_eq!(motor.bus().sent[0].len(), 4);
assert_eq!(&motor.bus().sent[0].data()[..4], &3_000_i32.to_be_bytes());
}
#[test]
fn direct_origin_method_sends_origin_frame() {
let bus = RecordingBus::default();
let mut motor = MotorController::new(bus, strict_config()).unwrap();
motor.set_origin(OriginMode::Temporary).unwrap();
assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x0503));
assert_eq!(motor.bus().sent[0].len(), 1);
assert_eq!(motor.bus().sent[0].data()[0], 0x00);
}
#[test]
fn manager_rejects_duplicate_ids() {
let result = MotorManager::new(
RecordingBus::default(),
[MotorConfig::default(), MotorConfig::default()],
);
assert!(result.is_err());
}
#[test]
fn manager_routes_keepalive_to_all_motors() {
let mut right = MotorConfig::default();
right.motor_id = 0x04;
let mut manager = MotorManager::new(RecordingBus::default(), [MotorConfig::default(), right])
.unwrap()
.with_labels([("left".to_owned(), 0x03), ("right".to_owned(), 0x04)])
.unwrap();
manager.send_keepalive_all().unwrap();
assert_eq!(manager.id_for_label("left").unwrap(), 0x03);
assert_eq!(manager.bus().sent.len(), 2);
assert_eq!(manager.bus().sent[0].id(), CanId::Extended(0x0803));
assert_eq!(manager.bus().sent[1].id(), CanId::Extended(0x0804));
}
#[test]
fn manager_routes_direct_commands_by_motor_id() {
let mut right = MotorConfig::default();
right.motor_id = 0x04;
let mut manager =
MotorManager::new(RecordingBus::default(), [MotorConfig::default(), right]).unwrap();
manager
.send_direct_command(0x04, DirectCommand::Velocity(1_234))
.unwrap();
assert_eq!(manager.bus().sent.len(), 1);
assert_eq!(manager.bus().sent[0].id(), CanId::Extended(0x0304));
assert_eq!(&manager.bus().sent[0].data()[..4], &1_234_i32.to_be_bytes());
}