kcan 0.1.2

CAN controller primitives for actuator and motor control.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151

use kcan::protocols::robstride::{
    RobStrideCommand, RobStrideFeedback, RobStrideFrameKind, RobStrideMitCommand, RobStrideModel,
    RobStridePositionCommand, RobStrideRawCommand, RobStrideSpeedCommand, robstride_command_frame,
    robstride_status_query_frame,
};
use kcan::{ActuatorConfig, ActuatorProtocol, CanId};

#[test]
fn robstride_models_expose_specs() {
    let spec = RobStrideModel::Rs04.spec();

    assert_eq!(RobStrideModel::Rs04.name(), "RS-04");
    assert_eq!(RobStrideModel::all().len(), 7);
    assert_eq!(
        RobStrideModel::from_name("rs-01"),
        Some(RobStrideModel::Rs01)
    );
    assert_eq!(
        RobStrideModel::from_name("RS_06"),
        Some(RobStrideModel::Rs06)
    );
    assert_eq!(RobStrideModel::from_name("unknown"), None);
    assert_eq!(spec.max_torque_nm, 120.0);
    assert_eq!(spec.kp_max, 5_000.0);
    assert_eq!(spec.kd_max, 100.0);
}

#[test]
fn robstride_frame_kind_uses_documented_base_ids() {
    assert_eq!(
        RobStrideFrameKind::Mit.can_id(1).unwrap(),
        CanId::Extended(0x201)
    );
    assert_eq!(
        RobStrideFrameKind::Position.can_id(1).unwrap(),
        CanId::Extended(0x301)
    );
    assert_eq!(
        RobStrideFrameKind::Speed.can_id(1).unwrap(),
        CanId::Extended(0x401)
    );
    assert_eq!(
        RobStrideFrameKind::Status.can_id(1).unwrap(),
        CanId::Extended(0x501)
    );
    assert_eq!(
        RobStrideFrameKind::Config.can_id(1).unwrap(),
        CanId::Extended(0x601)
    );
}

#[test]
fn robstride_mit_command_follows_seeed_simple_layout() {
    let frame = robstride_command_frame(
        RobStrideModel::Rs01,
        1,
        RobStrideCommand::Mit(RobStrideMitCommand {
            position_rad: 1.25,
            velocity_rad_s: 0.1,
            kp: 30.0,
            kd: 0.5,
            torque_nm: -1.2,
        }),
    )
    .unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x201));
    assert_eq!(&frame.data()[0..4], &1_250_i32.to_le_bytes());
    assert_eq!(frame.data()[4], 100);
    assert_eq!(frame.data()[5], 150);
    assert_eq!(frame.data()[6], 250);
    assert_eq!(frame.data()[7], (-12_i8) as u8);
}

#[test]
fn robstride_position_command_uses_eight_byte_layout() {
    let frame = robstride_command_frame(
        RobStrideModel::Rs02,
        2,
        RobStrideCommand::Position(RobStridePositionCommand {
            position_rad: -2.0,
            velocity_rad_s: 0.25,
            max_torque_nm: 6.0,
        }),
    )
    .unwrap();

    assert_eq!(frame.id(), CanId::Extended(0x302));
    assert_eq!(&frame.data()[0..4], &(-2_000_i32).to_le_bytes());
    assert_eq!(&frame.data()[4..6], &250_i16.to_le_bytes());
    assert_eq!(&frame.data()[6..8], &60_i16.to_le_bytes());
}

#[test]
fn robstride_speed_status_and_raw_frames_build() {
    let speed = robstride_command_frame(
        RobStrideModel::Rs03,
        3,
        RobStrideCommand::Speed(RobStrideSpeedCommand {
            velocity_rad_s: 1.5,
            max_torque_nm: 12.0,
        }),
    )
    .unwrap();
    let status = robstride_status_query_frame(3).unwrap();
    let raw = robstride_command_frame(
        RobStrideModel::Rs03,
        3,
        RobStrideCommand::Raw(RobStrideRawCommand {
            kind: RobStrideFrameKind::Config,
            data: [1, 2, 3, 4, 5, 6, 7, 8],
        }),
    )
    .unwrap();

    assert_eq!(speed.id(), CanId::Extended(0x403));
    assert_eq!(&speed.data()[0..4], &1_500_i32.to_le_bytes());
    assert_eq!(status.id(), CanId::Extended(0x503));
    assert_eq!(raw.id(), CanId::Extended(0x603));
    assert_eq!(raw.data(), &[1, 2, 3, 4, 5, 6, 7, 8]);
}

#[test]
fn robstride_feedback_parses_documented_numeric_fields() {
    let mut data = [0_u8; 8];
    data[0..4].copy_from_slice(&1_250_i32.to_le_bytes());
    data[4..6].copy_from_slice(&(-500_i16).to_le_bytes());
    data[6..8].copy_from_slice(&42_i16.to_le_bytes());

    let feedback = RobStrideFeedback::parse(&data).unwrap();

    assert_eq!(feedback.position_rad, 1.25);
    assert_eq!(feedback.velocity_rad_s, -0.5);
    assert_eq!(feedback.torque_nm, 4.2);
    assert_eq!(feedback.mode, None);
    assert_eq!(feedback.error_code, None);
    assert!(!feedback.has_error());
}

#[test]
fn actuator_config_formats_protocol_family() {
    let config = ActuatorConfig {
        id: 1,
        label: Some("left-knee".to_owned()),
        protocol: ActuatorProtocol::RobStride(RobStrideModel::Rs01),
    };

    assert_eq!(config.protocol.family_name(), "RobStride/Seeed");
    assert_eq!(config.protocol.model_name(), "RS-01");
    assert_eq!(config.display_name(), "left-knee (0x01)");
}