use std::f64::consts::PI;
use crate::bus::{CanFrame, CanId, MAX_CAN_DATA_LEN};
use crate::error::{Error, Result};
pub const DEFAULT_INTERFACE: &str = "can0";
pub const DEFAULT_BITRATE: u32 = 1_000_000;
pub const DEFAULT_MOTOR_ID: u8 = 0x03;
pub const DEFAULT_SECONDARY_MOTOR_ID: u8 = 0x04;
pub const DEFAULT_CONTROL_RATE_HZ: f64 = 100.0;
pub const DUTY_SCALE: f64 = 100_000.0;
pub const CURRENT_SCALE: f64 = 1_000.0;
pub const POSITION_SCALE: f64 = 10_000.0;
pub const PROFILE_POSITION_SCALE: f64 = POSITION_SCALE;
pub const MAX_DUTY_CYCLE: f64 = 0.95;
pub const MAX_CURRENT_AMPS: f64 = 60.0;
pub const MAX_POSITION_DEGREES: f64 = 36_000.0;
pub const MIN_POSITION_DEGREES: f64 = -36_000.0;
pub const MAX_PROTOCOL_ERPM: i32 = 100_000;
pub const MIN_PROTOCOL_ERPM: i32 = -100_000;
pub const MAX_PROFILE_ERPM: i32 = 327_670;
pub const MIN_PROFILE_ERPM: i32 = -327_680;
pub const MAX_PROFILE_ACCELERATION_ERPM_PER_SEC: i32 = 327_670;
pub const MIN_PROFILE_ACCELERATION_ERPM_PER_SEC: i32 = 0;
pub const PROFILE_ERPM_PER_UNIT: i32 = 10;
pub const MIT_HELPER_ENABLE: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC];
pub const MIT_HELPER_DISABLE: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD];
pub const MIT_HELPER_ZERO_POSITION: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE];
pub const MIT_HELPER_ENABLE_LEGACY: [u8; 8] = [0xFF; 8];
pub const MIT_HELPER_DISABLE_LEGACY: [u8; 8] = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE];
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u8)]
pub enum ControlMode {
DutyCycle = 0x00,
CurrentLoop = 0x01,
CurrentBrake = 0x02,
VelocityLoop = 0x03,
PositionLoop = 0x04,
SetOrigin = 0x05,
PositionVelocity = 0x06,
MitMode = 0x08,
}
impl ControlMode {
pub fn extended_id(self, motor_id: u8) -> Result<CanId> {
CanId::extended(((self as u32) << 8) | u32::from(motor_id))
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u8)]
pub enum OriginMode {
Temporary = 0x00,
Permanent = 0x01,
}
impl OriginMode {
pub const fn data(self) -> u8 {
self as u8
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum DirectCommand {
DutyCycle(f64),
Current(f64),
BrakeCurrent(f64),
Velocity(i32),
Position(f64),
SetOrigin(OriginMode),
PositionVelocity {
position_degrees: f64,
velocity_erpm: i32,
acceleration_erpm_per_sec: i32,
},
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct DirectPayload {
data: [u8; MAX_CAN_DATA_LEN],
len: u8,
}
impl DirectPayload {
pub const fn len(&self) -> usize {
self.len as usize
}
pub const fn is_empty(&self) -> bool {
self.len == 0
}
pub fn data(&self) -> &[u8] {
&self.data[..self.len()]
}
pub const fn padded_data(&self) -> [u8; MAX_CAN_DATA_LEN] {
self.data
}
fn from_slice(data: &[u8]) -> Self {
debug_assert!(data.len() <= MAX_CAN_DATA_LEN);
let mut payload = [0_u8; MAX_CAN_DATA_LEN];
payload[..data.len()].copy_from_slice(data);
Self {
data: payload,
len: data.len() as u8,
}
}
const fn from_array(data: [u8; MAX_CAN_DATA_LEN], len: u8) -> Self {
Self { data, len }
}
}
impl DirectCommand {
pub const fn mode(self) -> ControlMode {
match self {
Self::DutyCycle(_) => ControlMode::DutyCycle,
Self::Current(_) => ControlMode::CurrentLoop,
Self::BrakeCurrent(_) => ControlMode::CurrentBrake,
Self::Velocity(_) => ControlMode::VelocityLoop,
Self::Position(_) => ControlMode::PositionLoop,
Self::SetOrigin(_) => ControlMode::SetOrigin,
Self::PositionVelocity { .. } => ControlMode::PositionVelocity,
}
}
pub fn payload(self) -> Vec<u8> {
self.payload_bytes().data().to_vec()
}
pub fn payload_bytes(self) -> DirectPayload {
match self {
Self::DutyCycle(duty) => DirectPayload::from_slice(&scaled_i32_bytes(
duty,
-MAX_DUTY_CYCLE,
MAX_DUTY_CYCLE,
DUTY_SCALE,
)),
Self::Current(current_amps) | Self::BrakeCurrent(current_amps) => {
DirectPayload::from_slice(&scaled_i32_bytes(
current_amps,
-MAX_CURRENT_AMPS,
MAX_CURRENT_AMPS,
CURRENT_SCALE,
))
}
Self::Velocity(erpm) => DirectPayload::from_slice(
&erpm
.clamp(MIN_PROTOCOL_ERPM, MAX_PROTOCOL_ERPM)
.to_be_bytes(),
),
Self::Position(position_degrees) => DirectPayload::from_slice(&scaled_i32_bytes(
position_degrees,
MIN_POSITION_DEGREES,
MAX_POSITION_DEGREES,
POSITION_SCALE,
)),
Self::SetOrigin(origin_mode) => DirectPayload::from_slice(&[origin_mode.data()]),
Self::PositionVelocity {
position_degrees,
velocity_erpm,
acceleration_erpm_per_sec,
} => {
let mut payload = [0_u8; MAX_CAN_DATA_LEN];
payload[..4].copy_from_slice(&scaled_i32_bytes(
position_degrees,
MIN_POSITION_DEGREES,
MAX_POSITION_DEGREES,
PROFILE_POSITION_SCALE,
));
payload[4..6].copy_from_slice(&scaled_i16_units(
velocity_erpm,
MIN_PROFILE_ERPM,
MAX_PROFILE_ERPM,
PROFILE_ERPM_PER_UNIT,
));
payload[6..8].copy_from_slice(&scaled_i16_units(
acceleration_erpm_per_sec,
MIN_PROFILE_ACCELERATION_ERPM_PER_SEC,
MAX_PROFILE_ACCELERATION_ERPM_PER_SEC,
PROFILE_ERPM_PER_UNIT,
));
DirectPayload::from_array(payload, MAX_CAN_DATA_LEN as u8)
}
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum MotorModel {
Ak10_9,
Ak45_10,
Ak60_6,
Ak70_10,
Ak80_6,
Ak80_8,
Ak80_9,
Ak80_64,
Ak90_6,
Ak90_9,
Ak90_12,
Ak90_30,
Ak100_9,
Ak100_15,
Ak100_25,
Ak100_33,
Ak110_9,
Ak120_12,
Ak125_9,
Ak140_10,
Ak150_9,
}
pub const SUPPORTED_MOTOR_MODELS: [MotorModel; 21] = [
MotorModel::Ak10_9,
MotorModel::Ak45_10,
MotorModel::Ak60_6,
MotorModel::Ak70_10,
MotorModel::Ak80_6,
MotorModel::Ak80_8,
MotorModel::Ak80_9,
MotorModel::Ak80_64,
MotorModel::Ak90_6,
MotorModel::Ak90_9,
MotorModel::Ak90_12,
MotorModel::Ak90_30,
MotorModel::Ak100_9,
MotorModel::Ak100_15,
MotorModel::Ak100_25,
MotorModel::Ak100_33,
MotorModel::Ak110_9,
MotorModel::Ak120_12,
MotorModel::Ak125_9,
MotorModel::Ak140_10,
MotorModel::Ak150_9,
];
impl MotorModel {
pub const fn name(self) -> &'static str {
match self {
Self::Ak10_9 => "AK10-9",
Self::Ak45_10 => "AK45-10",
Self::Ak60_6 => "AK60-6",
Self::Ak70_10 => "AK70-10",
Self::Ak80_6 => "AK80-6",
Self::Ak80_8 => "AK80-8",
Self::Ak80_9 => "AK80-9",
Self::Ak80_64 => "AK80-64",
Self::Ak90_6 => "AK90-6",
Self::Ak90_9 => "AK90-9",
Self::Ak90_12 => "AK90-12",
Self::Ak90_30 => "AK90-30",
Self::Ak100_9 => "AK100-9",
Self::Ak100_15 => "AK100-15",
Self::Ak100_25 => "AK100-25",
Self::Ak100_33 => "AK100-33",
Self::Ak110_9 => "AK110-9",
Self::Ak120_12 => "AK120-12",
Self::Ak125_9 => "AK125-9",
Self::Ak140_10 => "AK140-10",
Self::Ak150_9 => "AK150-9",
}
}
pub const fn all() -> &'static [Self] {
&SUPPORTED_MOTOR_MODELS
}
pub fn from_name(value: &str) -> Option<Self> {
let compact = value
.trim()
.to_ascii_lowercase()
.replace(['-', '_', ' '], "");
match compact.as_str() {
"ak10" | "ak109" => Some(Self::Ak10_9),
"ak45" | "ak4510" => Some(Self::Ak45_10),
"ak60" | "ak606" => Some(Self::Ak60_6),
"ak70" | "ak7010" => Some(Self::Ak70_10),
"ak80" | "ak806" => Some(Self::Ak80_6),
"ak808" => Some(Self::Ak80_8),
"ak809" => Some(Self::Ak80_9),
"ak8064" => Some(Self::Ak80_64),
"ak906" => Some(Self::Ak90_6),
"ak90" | "ak909" => Some(Self::Ak90_9),
"ak9012" => Some(Self::Ak90_12),
"ak9030" => Some(Self::Ak90_30),
"ak100" | "ak1009" => Some(Self::Ak100_9),
"ak10015" => Some(Self::Ak100_15),
"ak10025" => Some(Self::Ak100_25),
"ak10033" => Some(Self::Ak100_33),
"ak110" | "ak1109" => Some(Self::Ak110_9),
"ak120" | "ak12012" => Some(Self::Ak120_12),
"ak125" | "ak1259" => Some(Self::Ak125_9),
"ak140" | "ak14010" => Some(Self::Ak140_10),
"ak150" | "ak1509" => Some(Self::Ak150_9),
_ => None,
}
}
pub const fn spec(self) -> MotorSpec {
match self {
Self::Ak10_9 => generic_ak_spec(self, 9),
Self::Ak45_10 => generic_ak_spec(self, 10),
Self::Ak60_6 => AK60_6_SPEC,
Self::Ak70_10 => generic_ak_spec(self, 10),
Self::Ak80_6 => AK80_6_SPEC,
Self::Ak80_8 => generic_ak_spec(self, 8),
Self::Ak80_9 => generic_ak_spec(self, 9),
Self::Ak80_64 => generic_ak_spec(self, 64),
Self::Ak90_6 => generic_ak_spec(self, 6),
Self::Ak90_9 => generic_ak_spec(self, 9),
Self::Ak90_12 => generic_ak_spec(self, 12),
Self::Ak90_30 => generic_ak_spec(self, 30),
Self::Ak100_9 => generic_ak_spec(self, 9),
Self::Ak100_15 => generic_ak_spec(self, 15),
Self::Ak100_25 => generic_ak_spec(self, 25),
Self::Ak100_33 => generic_ak_spec(self, 33),
Self::Ak110_9 => generic_ak_spec(self, 9),
Self::Ak120_12 => generic_ak_spec(self, 12),
Self::Ak125_9 => generic_ak_spec(self, 9),
Self::Ak140_10 => generic_ak_spec(self, 10),
Self::Ak150_9 => generic_ak_spec(self, 9),
}
}
pub const fn uses_extended_mit_layout(self) -> bool {
matches!(self, Self::Ak60_6)
}
pub fn mit_id(self, motor_id: u8) -> Result<CanId> {
if self.uses_extended_mit_layout() {
ControlMode::MitMode.extended_id(motor_id)
} else {
CanId::standard(motor_id.into())
}
}
pub fn helper_id(self, motor_id: u8) -> Result<CanId> {
if self.uses_extended_mit_layout() {
CanId::extended(motor_id.into())
} else {
CanId::standard(motor_id.into())
}
}
pub fn pack_mit(self, command: MitCommand) -> Result<[u8; 8]> {
if self.uses_extended_mit_layout() {
pack_mit_ak60(command, self.spec().mit_limits)
} else {
pack_mit_ak80(command, self.spec().mit_limits)
}
}
pub fn parse_feedback(self, data: &[u8]) -> Result<MotorFeedback> {
if self.uses_extended_mit_layout() {
parse_ak60_feedback(data)
} else {
parse_ak80_feedback(data, self.spec())
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MitLimits {
pub position_min_rad: f64,
pub position_max_rad: f64,
pub velocity_min_rad_s: f64,
pub velocity_max_rad_s: f64,
pub torque_min_nm: f64,
pub torque_max_nm: f64,
pub kp_min: f64,
pub kp_max: f64,
pub kd_min: f64,
pub kd_max: f64,
}
pub const AK60_6_MIT_LIMITS: MitLimits = MitLimits {
position_min_rad: -12.56,
position_max_rad: 12.56,
velocity_min_rad_s: -60.0,
velocity_max_rad_s: 60.0,
torque_min_nm: -12.0,
torque_max_nm: 12.0,
kp_min: 0.0,
kp_max: 500.0,
kd_min: 0.0,
kd_max: 5.0,
};
pub const AK80_6_MIT_LIMITS: MitLimits = MitLimits {
position_min_rad: -12.5,
position_max_rad: 12.5,
velocity_min_rad_s: -76.0,
velocity_max_rad_s: 76.0,
torque_min_nm: -12.0,
torque_max_nm: 12.0,
kp_min: 0.0,
kp_max: 500.0,
kd_min: 0.0,
kd_max: 5.0,
};
pub const AK_DEFAULT_MIT_LIMITS: MitLimits = MitLimits {
position_min_rad: -12.5,
position_max_rad: 12.5,
velocity_min_rad_s: -60.0,
velocity_max_rad_s: 60.0,
torque_min_nm: -12.0,
torque_max_nm: 12.0,
kp_min: 0.0,
kp_max: 500.0,
kd_min: 0.0,
kd_max: 5.0,
};
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MotorSpec {
pub model: MotorModel,
pub rated_voltage: &'static str,
pub pole_pairs: u16,
pub gear_ratio: u16,
pub rated_torque_nm: f64,
pub peak_torque_nm: f64,
pub rated_current_amps: f64,
pub peak_current_amps: f64,
pub max_output_speed_rpm: u16,
pub max_velocity_erpm: i32,
pub min_velocity_erpm: i32,
pub mit_position_kp: f64,
pub mit_position_kd: f64,
pub mit_velocity_kd: f64,
pub mit_limits: MitLimits,
}
pub const AK60_6_SPEC: MotorSpec = MotorSpec {
model: MotorModel::Ak60_6,
rated_voltage: "24/48V",
pole_pairs: 14,
gear_ratio: 6,
rated_torque_nm: 3.0,
peak_torque_nm: 9.0,
rated_current_amps: 3.8,
peak_current_amps: 11.2,
max_output_speed_rpm: 640,
max_velocity_erpm: 48_128,
min_velocity_erpm: -48_128,
mit_position_kp: 20.0,
mit_position_kd: 1.0,
mit_velocity_kd: 0.2,
mit_limits: AK60_6_MIT_LIMITS,
};
pub const AK80_6_SPEC: MotorSpec = MotorSpec {
model: MotorModel::Ak80_6,
rated_voltage: "48V",
pole_pairs: 21,
gear_ratio: 6,
rated_torque_nm: 6.0,
peak_torque_nm: 12.0,
rated_current_amps: 9.7,
peak_current_amps: 20.0,
max_output_speed_rpm: 800,
max_velocity_erpm: 91_444,
min_velocity_erpm: -91_444,
mit_position_kp: 1.0,
mit_position_kd: 1.0,
mit_velocity_kd: 0.5,
mit_limits: AK80_6_MIT_LIMITS,
};
const fn generic_ak_spec(model: MotorModel, gear_ratio: u16) -> MotorSpec {
MotorSpec {
model,
rated_voltage: "24/48V",
pole_pairs: 21,
gear_ratio,
rated_torque_nm: 0.0,
peak_torque_nm: AK_DEFAULT_MIT_LIMITS.torque_max_nm,
rated_current_amps: 0.0,
peak_current_amps: 20.0,
max_output_speed_rpm: 500,
max_velocity_erpm: 100_000,
min_velocity_erpm: -100_000,
mit_position_kp: 1.0,
mit_position_kd: 1.0,
mit_velocity_kd: 0.5,
mit_limits: AK_DEFAULT_MIT_LIMITS,
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MitCommand {
pub position_rad: f64,
pub velocity_rad_s: f64,
pub kp: f64,
pub kd: f64,
pub torque_nm: f64,
}
impl MitCommand {
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,
}
}
}
impl Default for MitCommand {
fn default() -> Self {
Self::neutral()
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum MitHelperCommand {
Enable,
Disable,
ZeroPosition,
EnableLegacy,
DisableLegacy,
}
impl MitHelperCommand {
pub const fn data(self) -> [u8; 8] {
match self {
Self::Enable => MIT_HELPER_ENABLE,
Self::Disable => MIT_HELPER_DISABLE,
Self::ZeroPosition => MIT_HELPER_ZERO_POSITION,
Self::EnableLegacy => MIT_HELPER_ENABLE_LEGACY,
Self::DisableLegacy => MIT_HELPER_DISABLE_LEGACY,
}
}
}
#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MotorFeedback {
pub reported_motor_id: Option<u8>,
pub position_degrees: f64,
pub speed_erpm: i32,
pub current_amps: f64,
pub temperature_celsius: i16,
pub error_code: u8,
}
impl MotorFeedback {
pub fn fault_code(&self) -> Option<FaultCode> {
FaultCode::from_code(self.error_code)
}
pub const fn has_fault(&self) -> bool {
self.error_code != 0
}
pub const fn error_description(&self) -> &'static str {
match FaultCode::from_code(self.error_code) {
Some(fault) => fault.description(),
None => "Unknown fault",
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[repr(u8)]
pub enum FaultCode {
None = 0,
MotorOverTemperature = 1,
OverCurrent = 2,
OverVoltage = 3,
UnderVoltage = 4,
EncoderFault = 5,
MosfetOverTemperature = 6,
MotorLockUp = 7,
}
impl FaultCode {
pub const fn from_code(code: u8) -> Option<Self> {
match code {
0 => Some(Self::None),
1 => Some(Self::MotorOverTemperature),
2 => Some(Self::OverCurrent),
3 => Some(Self::OverVoltage),
4 => Some(Self::UnderVoltage),
5 => Some(Self::EncoderFault),
6 => Some(Self::MosfetOverTemperature),
7 => Some(Self::MotorLockUp),
_ => None,
}
}
pub const fn code(self) -> u8 {
self as u8
}
pub const fn description(self) -> &'static str {
match self {
Self::None => "No fault",
Self::MotorOverTemperature => "Motor over-temperature",
Self::OverCurrent => "Over-current",
Self::OverVoltage => "Over-voltage",
Self::UnderVoltage => "Under-voltage",
Self::EncoderFault => "Encoder fault",
Self::MosfetOverTemperature => "MOSFET over-temperature",
Self::MotorLockUp => "Motor lock-up",
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct FeedbackFilter {
extended_ids: Vec<u32>,
standard_ids: Vec<u16>,
}
impl FeedbackFilter {
pub fn for_motor(model: MotorModel, motor_id: u8) -> Self {
if model.uses_extended_mit_layout() {
Self {
extended_ids: vec![0x2900 | u32::from(motor_id), 0x2900],
standard_ids: Vec::new(),
}
} else {
Self {
extended_ids: Vec::new(),
standard_ids: vec![u16::from(motor_id)],
}
}
}
pub fn with_explicit_id(mut self, id: CanId) -> Self {
match id {
CanId::Standard(id) => self.standard_ids.push(id),
CanId::Extended(id) => self.extended_ids.push(id),
}
self.dedup();
self
}
pub fn with_legacy_ids(mut self, motor_id: u8) -> Self {
self.standard_ids.extend([
u16::from(motor_id),
u16::from(motor_id.saturating_add(1)),
0x0080 | u16::from(motor_id),
]);
self.extended_ids.extend([
u32::from(motor_id),
u32::from(motor_id.saturating_add(1)),
0x0080 | u32::from(motor_id),
]);
self.dedup();
self
}
pub fn matches(&self, frame: &CanFrame) -> bool {
match frame.id() {
CanId::Standard(id) => self.standard_ids.contains(&id),
CanId::Extended(id) => self.extended_ids.contains(&id),
}
}
pub fn extended_ids(&self) -> &[u32] {
&self.extended_ids
}
pub fn standard_ids(&self) -> &[u16] {
&self.standard_ids
}
fn dedup(&mut self) {
self.extended_ids.sort_unstable();
self.extended_ids.dedup();
self.standard_ids.sort_unstable();
self.standard_ids.dedup();
}
}
pub fn float_to_uint(value: f64, min: f64, max: f64, bits: u8) -> Result<u32> {
if max <= min {
return Err(Error::InvalidRange { min, max });
}
if bits == 0 || bits > 32 {
return Err(Error::InvalidBitWidth(bits));
}
let max_int = if bits == 32 {
u32::MAX
} else {
(1_u32 << bits) - 1
};
let clamped = value.clamp(min, max);
if clamped <= min {
return Ok(0);
}
if clamped >= max {
return Ok(max_int);
}
let raw = ((clamped - min) * f64::from(max_int) / (max - min)) as u32;
Ok(raw.min(max_int))
}
pub fn uint_to_float(raw: u32, min: f64, max: f64, bits: u8) -> Result<f64> {
if max <= min {
return Err(Error::InvalidRange { min, max });
}
if bits == 0 || bits > 32 {
return Err(Error::InvalidBitWidth(bits));
}
let max_int = if bits == 32 {
u32::MAX
} else {
(1_u32 << bits) - 1
};
let raw = raw.min(max_int);
Ok((f64::from(raw) * (max - min) / f64::from(max_int)) + min)
}
pub fn pack_mit_command(model: MotorModel, command: MitCommand) -> Result<[u8; 8]> {
model.pack_mit(command)
}
pub fn mit_command_frame(model: MotorModel, motor_id: u8, command: MitCommand) -> Result<CanFrame> {
CanFrame::new_padded(model.mit_id(motor_id)?, &model.pack_mit(command)?)
}
pub fn mit_helper_frame(
model: MotorModel,
motor_id: u8,
helper: MitHelperCommand,
) -> Result<CanFrame> {
CanFrame::new_padded(model.helper_id(motor_id)?, &helper.data())
}
pub fn direct_command_frame(motor_id: u8, command: DirectCommand) -> Result<CanFrame> {
let payload = command.payload_bytes();
CanFrame::new(command.mode().extended_id(motor_id)?, payload.data())
}
pub fn erpm_to_rad_s(erpm: i32, spec: MotorSpec) -> f64 {
f64::from(erpm) * (2.0 * PI) / (60.0 * f64::from(spec.pole_pairs * spec.gear_ratio))
}
pub fn rad_s_to_erpm(rad_s: f64, spec: MotorSpec) -> i32 {
(rad_s * (60.0 * f64::from(spec.pole_pairs * spec.gear_ratio)) / (2.0 * PI)).round() as i32
}
pub fn degrees_to_radians(degrees: f64) -> f64 {
degrees * PI / 180.0
}
pub fn radians_to_degrees(radians: f64) -> f64 {
radians * 180.0 / PI
}
pub fn clamp_erpm(erpm: i32, spec: MotorSpec) -> i32 {
erpm.clamp(spec.min_velocity_erpm, spec.max_velocity_erpm)
}
fn scaled_i32_bytes(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_be_bytes()
}
fn scaled_i16_units(value: i32, min: i32, max: i32, units_per_lsb: i32) -> [u8; 2] {
let units = value.clamp(min, max) / units_per_lsb;
(units as i16).to_be_bytes()
}
fn pack_mit_ak60(command: MitCommand, limits: MitLimits) -> Result<[u8; 8]> {
let p_int = float_to_uint(
command.position_rad,
limits.position_min_rad,
limits.position_max_rad,
16,
)?;
let v_int = float_to_uint(
command.velocity_rad_s,
limits.velocity_min_rad_s,
limits.velocity_max_rad_s,
12,
)?;
let kp_int = float_to_uint(command.kp, limits.kp_min, limits.kp_max, 12)?;
let kd_int = float_to_uint(command.kd, limits.kd_min, limits.kd_max, 12)?;
let t_int = float_to_uint(
command.torque_nm,
limits.torque_min_nm,
limits.torque_max_nm,
12,
)?;
Ok([
(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,
])
}
fn pack_mit_ak80(command: MitCommand, limits: MitLimits) -> Result<[u8; 8]> {
let p_int = float_to_uint(
command.position_rad,
limits.position_min_rad,
limits.position_max_rad,
16,
)?;
let v_int = float_to_uint(
command.velocity_rad_s,
limits.velocity_min_rad_s,
limits.velocity_max_rad_s,
12,
)?;
let kp_int = float_to_uint(command.kp, limits.kp_min, limits.kp_max, 12)?;
let kd_int = float_to_uint(command.kd, limits.kd_min, limits.kd_max, 12)?;
let t_int = float_to_uint(
command.torque_nm,
limits.torque_min_nm,
limits.torque_max_nm,
12,
)?;
Ok([
(p_int >> 8) as u8,
(p_int & 0xFF) as u8,
(v_int >> 4) as u8,
(((v_int & 0x0F) << 4) | ((kp_int >> 8) & 0x0F)) as u8,
(kp_int & 0xFF) as u8,
(kd_int >> 4) as u8,
(((kd_int & 0x0F) << 4) | ((t_int >> 8) & 0x0F)) as u8,
(t_int & 0xFF) as u8,
])
}
fn parse_ak60_feedback(data: &[u8]) -> Result<MotorFeedback> {
if data.len() < 8 {
return Err(Error::PayloadTooShort {
expected: 8,
actual: data.len(),
});
}
let position_raw = i16::from_be_bytes([data[0], data[1]]);
let speed_raw = i16::from_be_bytes([data[2], data[3]]);
let current_raw = i16::from_be_bytes([data[4], data[5]]);
let temperature_raw = i8::from_be_bytes([data[6]]);
Ok(MotorFeedback {
reported_motor_id: None,
position_degrees: f64::from(position_raw) * 0.1,
speed_erpm: i32::from(speed_raw) * 10,
current_amps: f64::from(current_raw) * 0.01,
temperature_celsius: i16::from(temperature_raw),
error_code: data[7],
})
}
fn parse_ak80_feedback(data: &[u8], spec: MotorSpec) -> Result<MotorFeedback> {
if data.len() < 7 {
return Err(Error::PayloadTooShort {
expected: 7,
actual: data.len(),
});
}
let motor_id = data[0];
let p_int = (u32::from(data[1]) << 8) | u32::from(data[2]);
let v_int = (u32::from(data[3]) << 4) | u32::from(data[4] >> 4);
let i_int = (u32::from(data[4] & 0x0F) << 8) | u32::from(data[5]);
let temp_raw = data[6];
let error_code = data.get(7).copied().unwrap_or(0);
let position_rad = uint_to_float(
p_int,
spec.mit_limits.position_min_rad,
spec.mit_limits.position_max_rad,
16,
)?;
let velocity_rad_s = uint_to_float(
v_int,
spec.mit_limits.velocity_min_rad_s,
spec.mit_limits.velocity_max_rad_s,
12,
)?;
let current_amps = uint_to_float(i_int, -12.0, 12.0, 12)?;
Ok(MotorFeedback {
reported_motor_id: Some(motor_id),
position_degrees: radians_to_degrees(position_rad),
speed_erpm: rad_s_to_erpm(velocity_rad_s, spec),
current_amps,
temperature_celsius: i16::from(temp_raw) - 40,
error_code,
})
}