use crate::bus::{CanFrame, CanId};
use crate::error::Result;
pub const ROBSTRIDE_DEFAULT_BITRATE: u32 = 1_000_000;
pub const ROBSTRIDE_MIT_BASE_ID: u32 = 0x200;
pub const ROBSTRIDE_POSITION_BASE_ID: u32 = 0x300;
pub const ROBSTRIDE_SPEED_BASE_ID: u32 = 0x400;
pub const ROBSTRIDE_STATUS_BASE_ID: u32 = 0x500;
pub const ROBSTRIDE_CONFIG_BASE_ID: u32 = 0x600;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum RobStrideModel {
Rs00,
Rs01,
Rs02,
Rs03,
Rs04,
Rs05,
Rs06,
}
pub const ROBSTRIDE_MODELS: [RobStrideModel; 7] = [
RobStrideModel::Rs00,
RobStrideModel::Rs01,
RobStrideModel::Rs02,
RobStrideModel::Rs03,
RobStrideModel::Rs04,
RobStrideModel::Rs05,
RobStrideModel::Rs06,
];
impl RobStrideModel {
pub const fn name(self) -> &'static str {
match self {
Self::Rs00 => "RS-00",
Self::Rs01 => "RS-01",
Self::Rs02 => "RS-02",
Self::Rs03 => "RS-03",
Self::Rs04 => "RS-04",
Self::Rs05 => "RS-05",
Self::Rs06 => "RS-06",
}
}
pub const fn spec(self) -> RobStrideSpec {
match self {
Self::Rs00 => RobStrideSpec::new(self, 14.0, 315.0, 0.0, 500.0, 0.0, 5.0),
Self::Rs01 => RobStrideSpec::new(self, 17.0, 315.0, 0.0, 500.0, 0.0, 5.0),
Self::Rs02 => RobStrideSpec::new(self, 17.0, 410.0, 0.0, 500.0, 0.0, 5.0),
Self::Rs03 => RobStrideSpec::new(self, 60.0, 195.0, 0.0, 5_000.0, 0.0, 100.0),
Self::Rs04 => RobStrideSpec::new(self, 120.0, 200.0, 0.0, 5_000.0, 0.0, 100.0),
Self::Rs05 => RobStrideSpec::new(self, 5.5, 480.0, 0.0, 500.0, 0.0, 5.0),
Self::Rs06 => RobStrideSpec::new(self, 36.0, 480.0, 0.0, 5_000.0, 0.0, 100.0),
}
}
pub const fn all() -> &'static [Self] {
&ROBSTRIDE_MODELS
}
pub fn from_name(value: &str) -> Option<Self> {
let compact = value
.trim()
.to_ascii_lowercase()
.replace(['-', '_', ' '], "");
match compact.as_str() {
"rs00" | "rs0" => Some(Self::Rs00),
"rs01" | "rs1" => Some(Self::Rs01),
"rs02" | "rs2" => Some(Self::Rs02),
"rs03" | "rs3" => Some(Self::Rs03),
"rs04" | "rs4" => Some(Self::Rs04),
"rs05" | "rs5" => Some(Self::Rs05),
"rs06" | "rs6" => Some(Self::Rs06),
_ => None,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RobStrideSpec {
pub model: RobStrideModel,
pub max_torque_nm: f64,
pub max_speed_rpm: f64,
pub kp_min: f64,
pub kp_max: f64,
pub kd_min: f64,
pub kd_max: f64,
}
impl RobStrideSpec {
pub const fn new(
model: RobStrideModel,
max_torque_nm: f64,
max_speed_rpm: f64,
kp_min: f64,
kp_max: f64,
kd_min: f64,
kd_max: f64,
) -> Self {
Self {
model,
max_torque_nm,
max_speed_rpm,
kp_min,
kp_max,
kd_min,
kd_max,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum RobStrideFrameKind {
Mit,
Position,
Speed,
Status,
Config,
}
impl RobStrideFrameKind {
pub const fn base_id(self) -> u32 {
match self {
Self::Mit => ROBSTRIDE_MIT_BASE_ID,
Self::Position => ROBSTRIDE_POSITION_BASE_ID,
Self::Speed => ROBSTRIDE_SPEED_BASE_ID,
Self::Status => ROBSTRIDE_STATUS_BASE_ID,
Self::Config => ROBSTRIDE_CONFIG_BASE_ID,
}
}
pub fn can_id(self, motor_id: u8) -> Result<CanId> {
CanId::extended(self.base_id() + u32::from(motor_id))
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStrideMitCommand {
pub position_rad: f64,
pub velocity_rad_s: f64,
pub kp: f64,
pub kd: f64,
pub torque_nm: f64,
}
impl RobStrideMitCommand {
pub const fn neutral() -> Self {
Self {
position_rad: 0.0,
velocity_rad_s: 0.0,
kp: 0.0,
kd: 0.0,
torque_nm: 0.0,
}
}
pub fn pack(self, spec: RobStrideSpec) -> [u8; 8] {
let mut data = [0_u8; 8];
data[..4].copy_from_slice(&scaled_i32_le(
self.position_rad,
-100_000.0,
100_000.0,
1_000.0,
));
data[4] = scaled_i8_byte(
self.velocity_rad_s,
-255.0 / 1_000.0,
255.0 / 1_000.0,
1_000.0,
);
data[5] = scaled_u8(self.kp, spec.kp_min, spec.kp_max, 5.0);
data[6] = scaled_u8(self.kd, spec.kd_min, spec.kd_max, 500.0);
data[7] = scaled_i8_byte(
self.torque_nm,
-spec.max_torque_nm,
spec.max_torque_nm,
10.0,
);
data
}
}
impl Default for RobStrideMitCommand {
fn default() -> Self {
Self::neutral()
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStridePositionCommand {
pub position_rad: f64,
pub velocity_rad_s: f64,
pub max_torque_nm: f64,
}
impl RobStridePositionCommand {
pub fn pack(self, spec: RobStrideSpec) -> [u8; 8] {
let mut data = [0_u8; 8];
data[..4].copy_from_slice(&scaled_i32_le(
self.position_rad,
-100_000.0,
100_000.0,
1_000.0,
));
data[4..6].copy_from_slice(&scaled_i16_le(
self.velocity_rad_s,
-1_000.0,
1_000.0,
1_000.0,
));
data[6..8].copy_from_slice(&scaled_i16_le(
self.max_torque_nm,
0.0,
spec.max_torque_nm,
10.0,
));
data
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStrideSpeedCommand {
pub velocity_rad_s: f64,
pub max_torque_nm: f64,
}
impl RobStrideSpeedCommand {
pub fn pack(self, spec: RobStrideSpec) -> [u8; 8] {
let mut data = [0_u8; 8];
data[..4].copy_from_slice(&scaled_i32_le(
self.velocity_rad_s,
-1_000.0,
1_000.0,
1_000.0,
));
data[4..6].copy_from_slice(&scaled_i16_le(
self.max_torque_nm,
0.0,
spec.max_torque_nm,
10.0,
));
data
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStrideRawCommand {
pub kind: RobStrideFrameKind,
pub data: [u8; 8],
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum RobStrideCommand {
Mit(RobStrideMitCommand),
Position(RobStridePositionCommand),
Speed(RobStrideSpeedCommand),
StatusQuery,
Config(RobStrideRawCommand),
Raw(RobStrideRawCommand),
}
impl RobStrideCommand {
pub const fn kind(self) -> RobStrideFrameKind {
match self {
Self::Mit(_) => RobStrideFrameKind::Mit,
Self::Position(_) => RobStrideFrameKind::Position,
Self::Speed(_) => RobStrideFrameKind::Speed,
Self::StatusQuery => RobStrideFrameKind::Status,
Self::Config(_) => RobStrideFrameKind::Config,
Self::Raw(raw) => raw.kind,
}
}
pub fn payload(self, spec: RobStrideSpec) -> [u8; 8] {
match self {
Self::Mit(command) => command.pack(spec),
Self::Position(command) => command.pack(spec),
Self::Speed(command) => command.pack(spec),
Self::StatusQuery => [0_u8; 8],
Self::Config(raw) | Self::Raw(raw) => raw.data,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct RobStrideFeedback {
pub position_rad: f64,
pub velocity_rad_s: f64,
pub torque_nm: f64,
pub mode: Option<u8>,
pub error_code: Option<u8>,
}
impl RobStrideFeedback {
pub fn parse(data: &[u8]) -> Option<Self> {
if data.len() < 8 {
return None;
}
let position_raw = i32::from_le_bytes([data[0], data[1], data[2], data[3]]);
let velocity_raw = i16::from_le_bytes([data[4], data[5]]);
let torque_raw = i16::from_le_bytes([data[6], data[7]]);
Some(Self {
position_rad: f64::from(position_raw) / 1_000.0,
velocity_rad_s: f64::from(velocity_raw) / 1_000.0,
torque_nm: f64::from(torque_raw) / 10.0,
mode: data.get(8).copied(),
error_code: data.get(9).copied(),
})
}
pub const fn has_error(&self) -> bool {
matches!(self.error_code, Some(code) if code != 0)
}
}
pub fn robstride_command_frame(
model: RobStrideModel,
motor_id: u8,
command: RobStrideCommand,
) -> Result<CanFrame> {
let spec = model.spec();
CanFrame::new_padded(command.kind().can_id(motor_id)?, &command.payload(spec))
}
pub fn robstride_status_query_frame(motor_id: u8) -> Result<CanFrame> {
CanFrame::new_padded(RobStrideFrameKind::Status.can_id(motor_id)?, &[0_u8; 8])
}
fn scaled_i32_le(value: f64, min: f64, max: f64, scale: f64) -> [u8; 4] {
let scaled = (value.clamp(min, max) * scale).round();
(scaled.clamp(f64::from(i32::MIN), f64::from(i32::MAX)) as i32).to_le_bytes()
}
fn scaled_i16_le(value: f64, min: f64, max: f64, scale: f64) -> [u8; 2] {
let scaled = (value.clamp(min, max) * scale).round();
(scaled.clamp(f64::from(i16::MIN), f64::from(i16::MAX)) as i16).to_le_bytes()
}
fn scaled_i8_byte(value: f64, min: f64, max: f64, scale: f64) -> u8 {
let scaled = (value.clamp(min, max) * scale).round();
let signed = scaled.clamp(f64::from(i8::MIN), f64::from(i8::MAX)) as i8;
signed as u8
}
fn scaled_u8(value: f64, min: f64, max: f64, scale: f64) -> u8 {
let scaled = (value.clamp(min, max) * scale).round();
scaled.clamp(0.0, f64::from(u8::MAX)) as u8
}