kcan_core/
lib.rs

1//! No-std CAN protocol primitives for KCAN actuator tooling.
2//!
3//! This crate intentionally stays small: fixed-size CAN frames, CubeMars Servo
4//! Direct payload builders, and RobStride status parsing. Higher-level transport,
5//! dashboards, and analytics live in sibling crates or the main `kcan` crate.
6//!
7//! ```
8//! # fn main() -> kcan_core::Result<()> {
9//! use kcan_core::{CanId, DirectCommand, direct_command_frame};
10//!
11//! let id: CanId = "extended:0x301".parse()?;
12//! let frame = direct_command_frame(0x03, DirectCommand::Velocity(1_000))?;
13//!
14//! assert_eq!(id.raw(), 0x301);
15//! assert_eq!(frame.to_string(), "extended 0x303 len=4 data=00 00 03 E8");
16//! # Ok(())
17//! # }
18//! ```
19
20#![no_std]
21#![forbid(unsafe_code)]
22
23#[cfg(test)]
24extern crate std;
25
26use core::{fmt, str::FromStr};
27
28pub const MAX_STANDARD_ID: u16 = 0x07FF;
29pub const MAX_EXTENDED_ID: u32 = 0x1FFF_FFFF;
30pub const MAX_CAN_DATA_LEN: usize = 8;
31
32pub const DUTY_SCALE: f64 = 100_000.0;
33pub const CURRENT_SCALE: f64 = 1_000.0;
34pub const POSITION_SCALE: f64 = 10_000.0;
35pub const MAX_DUTY_CYCLE: f64 = 0.95;
36pub const MAX_CURRENT_AMPS: f64 = 60.0;
37pub const MAX_POSITION_DEGREES: f64 = 36_000.0;
38pub const MIN_POSITION_DEGREES: f64 = -36_000.0;
39pub const MAX_PROTOCOL_ERPM: i32 = 100_000;
40pub const MIN_PROTOCOL_ERPM: i32 = -100_000;
41pub const MAX_PROFILE_ERPM: i32 = 327_670;
42pub const MIN_PROFILE_ERPM: i32 = -327_680;
43pub const MAX_PROFILE_ACCELERATION_ERPM_PER_SEC: i32 = 327_670;
44pub const MIN_PROFILE_ACCELERATION_ERPM_PER_SEC: i32 = 0;
45pub const PROFILE_ERPM_PER_UNIT: i32 = 10;
46
47pub const ROBSTRIDE_MIT_BASE_ID: u32 = 0x200;
48pub const ROBSTRIDE_POSITION_BASE_ID: u32 = 0x300;
49pub const ROBSTRIDE_SPEED_BASE_ID: u32 = 0x400;
50pub const ROBSTRIDE_STATUS_BASE_ID: u32 = 0x500;
51pub const ROBSTRIDE_CONFIG_BASE_ID: u32 = 0x600;
52
53pub type Result<T> = core::result::Result<T, Error>;
54
55#[derive(Debug, Clone, Copy, PartialEq, Eq)]
56pub enum Error {
57    InvalidCanId(u32),
58    InvalidCanIdText,
59    PayloadTooLong { len: usize },
60    UnknownCanIdKind,
61}
62
63impl fmt::Display for Error {
64    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
65        match self {
66            Self::InvalidCanId(id) => write!(f, "invalid CAN id: 0x{id:X}"),
67            Self::InvalidCanIdText => write!(f, "invalid CAN id text"),
68            Self::PayloadTooLong { len } => write!(f, "CAN payload too long: {len} bytes"),
69            Self::UnknownCanIdKind => write!(f, "unknown CAN id kind"),
70        }
71    }
72}
73
74#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
75pub enum CanId {
76    Standard(u16),
77    Extended(u32),
78}
79
80impl CanId {
81    pub const fn standard(id: u16) -> Result<Self> {
82        if id > MAX_STANDARD_ID {
83            Err(Error::InvalidCanId(id as u32))
84        } else {
85            Ok(Self::Standard(id))
86        }
87    }
88
89    pub const fn extended(id: u32) -> Result<Self> {
90        if id > MAX_EXTENDED_ID {
91            Err(Error::InvalidCanId(id))
92        } else {
93            Ok(Self::Extended(id))
94        }
95    }
96
97    pub const fn raw(self) -> u32 {
98        match self {
99            Self::Standard(id) => id as u32,
100            Self::Extended(id) => id,
101        }
102    }
103
104    pub const fn is_extended(self) -> bool {
105        matches!(self, Self::Extended(_))
106    }
107
108    pub const fn kind_name(self) -> &'static str {
109        match self {
110            Self::Standard(_) => "standard",
111            Self::Extended(_) => "extended",
112        }
113    }
114}
115
116impl fmt::Display for CanId {
117    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
118        write!(f, "{} 0x{:X}", self.kind_name(), self.raw())
119    }
120}
121
122impl FromStr for CanId {
123    type Err = Error;
124
125    fn from_str(value: &str) -> Result<Self> {
126        let value = value.trim();
127        let (kind, raw) = if let Some((kind, raw)) = value.split_once(':') {
128            (kind.trim(), raw.trim())
129        } else {
130            let mut parts = value.split_whitespace();
131            match (parts.next(), parts.next(), parts.next()) {
132                (Some(kind), Some(raw), None) => (kind, raw),
133                _ => return Err(Error::InvalidCanIdText),
134            }
135        };
136
137        let raw = parse_can_id_number(raw)?;
138        if matches_kind(kind, &["standard", "std", "s"]) {
139            if raw > MAX_STANDARD_ID as u32 {
140                return Err(Error::InvalidCanId(raw));
141            }
142            CanId::standard(raw as u16)
143        } else if matches_kind(kind, &["extended", "ext", "e"]) {
144            CanId::extended(raw)
145        } else {
146            Err(Error::UnknownCanIdKind)
147        }
148    }
149}
150
151fn matches_kind(value: &str, candidates: &[&str]) -> bool {
152    candidates
153        .iter()
154        .any(|candidate| value.eq_ignore_ascii_case(candidate))
155}
156
157fn parse_can_id_number(value: &str) -> Result<u32> {
158    if let Some(hex) = value
159        .strip_prefix("0x")
160        .or_else(|| value.strip_prefix("0X"))
161    {
162        u32::from_str_radix(hex, 16).map_err(|_| Error::InvalidCanIdText)
163    } else {
164        value.parse::<u32>().map_err(|_| Error::InvalidCanIdText)
165    }
166}
167
168#[derive(Debug, Clone, Copy, PartialEq, Eq)]
169pub struct CanFrame {
170    id: CanId,
171    data: [u8; MAX_CAN_DATA_LEN],
172    len: u8,
173}
174
175impl CanFrame {
176    pub fn new(id: CanId, data: &[u8]) -> Result<Self> {
177        if data.len() > MAX_CAN_DATA_LEN {
178            return Err(Error::PayloadTooLong { len: data.len() });
179        }
180
181        let mut padded = [0_u8; MAX_CAN_DATA_LEN];
182        padded[..data.len()].copy_from_slice(data);
183        Ok(Self {
184            id,
185            data: padded,
186            len: data.len() as u8,
187        })
188    }
189
190    pub const fn from_payload(id: CanId, payload: DirectPayload) -> Self {
191        Self {
192            id,
193            data: payload.padded_data(),
194            len: payload.len as u8,
195        }
196    }
197
198    pub const fn id(&self) -> CanId {
199        self.id
200    }
201
202    pub fn data(&self) -> &[u8] {
203        &self.data[..self.len()]
204    }
205
206    pub const fn padded_data(&self) -> [u8; MAX_CAN_DATA_LEN] {
207        self.data
208    }
209
210    pub const fn len(&self) -> usize {
211        self.len as usize
212    }
213
214    pub const fn is_empty(&self) -> bool {
215        self.len == 0
216    }
217}
218
219impl fmt::Display for CanFrame {
220    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
221        write!(f, "{} len={} data=", self.id, self.len())?;
222        for (index, byte) in self.data().iter().enumerate() {
223            if index > 0 {
224                f.write_str(" ")?;
225            }
226            write!(f, "{byte:02X}")?;
227        }
228        Ok(())
229    }
230}
231
232#[derive(Debug, Clone, Copy, PartialEq, Eq)]
233#[repr(u8)]
234pub enum ControlMode {
235    DutyCycle = 0x00,
236    CurrentLoop = 0x01,
237    CurrentBrake = 0x02,
238    VelocityLoop = 0x03,
239    PositionLoop = 0x04,
240    SetOrigin = 0x05,
241    PositionVelocity = 0x06,
242    MitMode = 0x08,
243}
244
245impl ControlMode {
246    pub const fn extended_id(self, motor_id: u8) -> Result<CanId> {
247        CanId::extended(((self as u32) << 8) | motor_id as u32)
248    }
249}
250
251#[derive(Debug, Clone, Copy, PartialEq, Eq)]
252#[repr(u8)]
253pub enum OriginMode {
254    Temporary = 0x00,
255    Permanent = 0x01,
256}
257
258impl OriginMode {
259    pub const fn data(self) -> u8 {
260        self as u8
261    }
262}
263
264#[derive(Debug, Clone, Copy, PartialEq)]
265pub enum DirectCommand {
266    DutyCycle(f64),
267    Current(f64),
268    BrakeCurrent(f64),
269    Velocity(i32),
270    Position(f64),
271    SetOrigin(OriginMode),
272    PositionVelocity {
273        position_degrees: f64,
274        velocity_erpm: i32,
275        acceleration_erpm_per_sec: i32,
276    },
277}
278
279impl DirectCommand {
280    pub const fn mode(self) -> ControlMode {
281        match self {
282            Self::DutyCycle(_) => ControlMode::DutyCycle,
283            Self::Current(_) => ControlMode::CurrentLoop,
284            Self::BrakeCurrent(_) => ControlMode::CurrentBrake,
285            Self::Velocity(_) => ControlMode::VelocityLoop,
286            Self::Position(_) => ControlMode::PositionLoop,
287            Self::SetOrigin(_) => ControlMode::SetOrigin,
288            Self::PositionVelocity { .. } => ControlMode::PositionVelocity,
289        }
290    }
291
292    pub fn payload(self) -> DirectPayload {
293        match self {
294            Self::DutyCycle(duty) => DirectPayload::from_slice(&scaled_i32_bytes(
295                duty,
296                -MAX_DUTY_CYCLE,
297                MAX_DUTY_CYCLE,
298                DUTY_SCALE,
299            )),
300            Self::Current(current_amps) | Self::BrakeCurrent(current_amps) => {
301                DirectPayload::from_slice(&scaled_i32_bytes(
302                    current_amps,
303                    -MAX_CURRENT_AMPS,
304                    MAX_CURRENT_AMPS,
305                    CURRENT_SCALE,
306                ))
307            }
308            Self::Velocity(erpm) => DirectPayload::from_slice(
309                &erpm
310                    .clamp(MIN_PROTOCOL_ERPM, MAX_PROTOCOL_ERPM)
311                    .to_be_bytes(),
312            ),
313            Self::Position(position_degrees) => DirectPayload::from_slice(&scaled_i32_bytes(
314                position_degrees,
315                MIN_POSITION_DEGREES,
316                MAX_POSITION_DEGREES,
317                POSITION_SCALE,
318            )),
319            Self::SetOrigin(origin_mode) => DirectPayload::from_slice(&[origin_mode.data()]),
320            Self::PositionVelocity {
321                position_degrees,
322                velocity_erpm,
323                acceleration_erpm_per_sec,
324            } => {
325                let mut payload = [0_u8; MAX_CAN_DATA_LEN];
326                payload[..4].copy_from_slice(&scaled_i32_bytes(
327                    position_degrees,
328                    MIN_POSITION_DEGREES,
329                    MAX_POSITION_DEGREES,
330                    POSITION_SCALE,
331                ));
332                payload[4..6].copy_from_slice(&scaled_i16_units(
333                    velocity_erpm,
334                    MIN_PROFILE_ERPM,
335                    MAX_PROFILE_ERPM,
336                    PROFILE_ERPM_PER_UNIT,
337                ));
338                payload[6..8].copy_from_slice(&scaled_i16_units(
339                    acceleration_erpm_per_sec,
340                    MIN_PROFILE_ACCELERATION_ERPM_PER_SEC,
341                    MAX_PROFILE_ACCELERATION_ERPM_PER_SEC,
342                    PROFILE_ERPM_PER_UNIT,
343                ));
344                DirectPayload::from_array(payload, MAX_CAN_DATA_LEN)
345            }
346        }
347    }
348}
349
350#[derive(Debug, Clone, Copy, PartialEq, Eq)]
351pub struct DirectPayload {
352    data: [u8; MAX_CAN_DATA_LEN],
353    len: usize,
354}
355
356impl DirectPayload {
357    pub const fn len(&self) -> usize {
358        self.len
359    }
360
361    pub const fn is_empty(&self) -> bool {
362        self.len == 0
363    }
364
365    pub fn data(&self) -> &[u8] {
366        &self.data[..self.len]
367    }
368
369    pub const fn padded_data(&self) -> [u8; MAX_CAN_DATA_LEN] {
370        self.data
371    }
372
373    fn from_slice(data: &[u8]) -> Self {
374        let mut payload = [0_u8; MAX_CAN_DATA_LEN];
375        payload[..data.len()].copy_from_slice(data);
376        Self {
377            data: payload,
378            len: data.len(),
379        }
380    }
381
382    const fn from_array(data: [u8; MAX_CAN_DATA_LEN], len: usize) -> Self {
383        Self { data, len }
384    }
385}
386
387pub fn direct_command_frame(motor_id: u8, command: DirectCommand) -> Result<CanFrame> {
388    let payload = command.payload();
389    Ok(CanFrame::from_payload(
390        command.mode().extended_id(motor_id)?,
391        payload,
392    ))
393}
394
395#[derive(Debug, Clone, Copy, PartialEq, Eq)]
396pub enum RobStrideFrameKind {
397    Mit,
398    Position,
399    Speed,
400    Status,
401    Config,
402}
403
404impl RobStrideFrameKind {
405    pub const fn base_id(self) -> u32 {
406        match self {
407            Self::Mit => ROBSTRIDE_MIT_BASE_ID,
408            Self::Position => ROBSTRIDE_POSITION_BASE_ID,
409            Self::Speed => ROBSTRIDE_SPEED_BASE_ID,
410            Self::Status => ROBSTRIDE_STATUS_BASE_ID,
411            Self::Config => ROBSTRIDE_CONFIG_BASE_ID,
412        }
413    }
414
415    pub const fn can_id(self, motor_id: u8) -> Result<CanId> {
416        CanId::extended(self.base_id() + motor_id as u32)
417    }
418}
419
420#[derive(Debug, Clone, Copy, PartialEq)]
421pub struct RobStrideFeedback {
422    pub position_rad: f64,
423    pub velocity_rad_s: f64,
424    pub torque_nm: f64,
425}
426
427impl RobStrideFeedback {
428    pub fn parse(data: &[u8]) -> Option<Self> {
429        if data.len() < MAX_CAN_DATA_LEN {
430            return None;
431        }
432
433        let position_raw = i32::from_le_bytes([data[0], data[1], data[2], data[3]]);
434        let velocity_raw = i16::from_le_bytes([data[4], data[5]]);
435        let torque_raw = i16::from_le_bytes([data[6], data[7]]);
436
437        Some(Self {
438            position_rad: position_raw as f64 / 1_000.0,
439            velocity_rad_s: velocity_raw as f64 / 1_000.0,
440            torque_nm: torque_raw as f64 / 10.0,
441        })
442    }
443}
444
445fn scaled_i32_bytes(value: f64, min: f64, max: f64, scale: f64) -> [u8; 4] {
446    let scaled = round_f64(value.clamp(min, max) * scale);
447    (scaled.clamp(i32::MIN as f64, i32::MAX as f64) as i32).to_be_bytes()
448}
449
450fn round_f64(value: f64) -> f64 {
451    if value >= 0.0 {
452        (value + 0.5) as i64 as f64
453    } else {
454        (value - 0.5) as i64 as f64
455    }
456}
457
458fn scaled_i16_units(value: i32, min: i32, max: i32, units_per_lsb: i32) -> [u8; 2] {
459    let units = value.clamp(min, max) / units_per_lsb;
460    (units as i16).to_be_bytes()
461}
462
463#[cfg(test)]
464mod tests {
465    use super::*;
466    use std::string::ToString;
467
468    #[test]
469    fn direct_payloads_use_protocol_scaling() {
470        let current = DirectCommand::Current(1.0).payload();
471        assert_eq!(current.data(), &[0x00, 0x00, 0x03, 0xE8]);
472
473        let profile = DirectCommand::PositionVelocity {
474            position_degrees: 1.5,
475            velocity_erpm: -100,
476            acceleration_erpm_per_sec: 200,
477        }
478        .payload();
479        assert_eq!(profile.len(), 8);
480        assert_eq!(&profile.data()[..4], &15_000_i32.to_be_bytes());
481        assert_eq!(&profile.data()[4..6], &(-10_i16).to_be_bytes());
482        assert_eq!(&profile.data()[6..8], &20_i16.to_be_bytes());
483    }
484
485    #[test]
486    fn direct_frame_uses_natural_length() {
487        let frame =
488            direct_command_frame(0x03, DirectCommand::SetOrigin(OriginMode::Permanent)).unwrap();
489
490        assert_eq!(frame.id(), CanId::Extended(0x0503));
491        assert_eq!(frame.len(), 1);
492        assert_eq!(frame.data(), &[0x01]);
493    }
494
495    #[test]
496    fn can_ids_and_frames_format_and_parse() {
497        let standard = CanId::standard(0x123).unwrap();
498        let extended = CanId::extended(0x501).unwrap();
499        let frame = CanFrame::new(extended, &[0x0A, 0xB0]).unwrap();
500
501        assert_eq!(standard.kind_name(), "standard");
502        assert_eq!(extended.kind_name(), "extended");
503        assert_eq!(standard.to_string(), "standard 0x123");
504        assert_eq!(extended.to_string(), "extended 0x501");
505        assert_eq!(frame.to_string(), "extended 0x501 len=2 data=0A B0");
506        assert_eq!(standard.to_string().parse::<CanId>().unwrap(), standard);
507        assert_eq!(extended.to_string().parse::<CanId>().unwrap(), extended);
508        assert_eq!("s:0x123".parse::<CanId>().unwrap(), standard);
509        assert_eq!("e:0x501".parse::<CanId>().unwrap(), extended);
510        assert_eq!(
511            "0x123".parse::<CanId>().unwrap_err(),
512            Error::InvalidCanIdText
513        );
514        assert_eq!(
515            "standard 0x800".parse::<CanId>().unwrap_err(),
516            Error::InvalidCanId(0x800)
517        );
518    }
519
520    #[test]
521    fn robstride_status_feedback_decodes() {
522        let mut data = [0_u8; 8];
523        data[..4].copy_from_slice(&1_250_i32.to_le_bytes());
524        data[4..6].copy_from_slice(&(-500_i16).to_le_bytes());
525        data[6..8].copy_from_slice(&42_i16.to_le_bytes());
526
527        let feedback = RobStrideFeedback::parse(&data).unwrap();
528
529        assert_eq!(RobStrideFrameKind::Status.can_id(1).unwrap().raw(), 0x501);
530        assert_eq!(feedback.position_rad, 1.25);
531        assert_eq!(feedback.velocity_rad_s, -0.5);
532        assert_eq!(feedback.torque_nm, 4.2);
533    }
534}
kcan_core/lib.rs

kcan_core/
lib.rs
