diff --git a/actuator/robstride/src/can.rs b/actuator/robstride/src/can.rs index 7760d95..50e8a3c 100644 --- a/actuator/robstride/src/can.rs +++ b/actuator/robstride/src/can.rs @@ -1,3 +1,4 @@ +use crate::types::MotorMode; use serde::{Deserialize, Serialize}; use serialport::TTYPort; use std::io::{Read, Write}; @@ -45,6 +46,109 @@ pub struct CanPack { pub data: Vec, } +#[derive(Debug, Default, Clone, Serialize, Deserialize)] +pub struct MotorFeedbackRaw { + pub can_id: u8, + pub pos_int: u16, + pub vel_int: u16, + pub torque_int: u16, + pub mode: MotorMode, + pub faults: u16, +} + +pub fn uint_to_float(x_int: u16, x_min: f32, x_max: f32, bits: u8) -> f32 { + let span = x_max - x_min; + let offset = x_min; + (x_int as f32) * span / ((1 << bits) - 1) as f32 + offset +} + +pub fn float_to_uint(x: f32, x_min: f32, x_max: f32, bits: u8) -> u16 { + let span = x_max - x_min; + let offset = x_min; + ((x - offset) * ((1 << bits) - 1) as f32 / span) as u16 +} + +pub fn pack_bits(values: &[u32], bit_lengths: &[u8]) -> u32 { + let mut result: u32 = 0; + let mut current_shift = 0; + + for (&value, &bits) in values.iter().zip(bit_lengths.iter()) { + let mask = (1 << bits) - 1; + result |= (value & mask) << current_shift; + current_shift += bits; + } + + result +} + +pub fn unpack_bits(value: u32, bit_lengths: &[u8]) -> Vec { + let mut result = Vec::new(); + let mut current_value = value; + + for &bits in bit_lengths.iter() { + let mask = (1 << bits) - 1; + result.push(current_value & mask); + current_value >>= bits; + } + + result +} + +pub fn pack_ex_id(ex_id: &ExId) -> [u8; 4] { + let addr = (pack_bits( + &[ + ex_id.id as u32, + ex_id.data as u32, + ex_id.mode as u32, + ex_id.res as u32, + ], + &[8, 16, 5, 3], + ) << 3) + | 0x00000004; + addr.to_be_bytes() +} + +pub fn unpack_ex_id(addr: [u8; 4]) -> ExId { + let addr = u32::from_be_bytes(addr); + let addr = unpack_bits(addr >> 3, &[8, 16, 5, 3]); + ExId { + id: addr[0] as u8, + data: addr[1] as u16, + mode: unsafe { std::mem::transmute(addr[2] as u8) }, + res: addr[3] as u8, + } +} + +pub fn unpack_raw_feedback(pack: &CanPack) -> MotorFeedbackRaw { + let can_id = (pack.ex_id.data & 0x00FF) as u8; + let faults = (pack.ex_id.data & 0x3F00) >> 8; + let mode = unsafe { std::mem::transmute(((pack.ex_id.data & 0xC000) >> 14) as u8) }; + + if pack.ex_id.mode != CanComMode::MotorFeedback { + return MotorFeedbackRaw { + can_id, + pos_int: 0, + vel_int: 0, + torque_int: 0, + mode, + faults, + }; + } + + let pos_int = u16::from_be_bytes([pack.data[0], pack.data[1]]); + let vel_int = u16::from_be_bytes([pack.data[2], pack.data[3]]); + let torque_int = u16::from_be_bytes([pack.data[4], pack.data[5]]); + + MotorFeedbackRaw { + can_id, + pos_int, + vel_int, + torque_int, + mode, + faults, + } +} + pub fn tx_packs( port: &mut TTYPort, packs: &[CanPack], diff --git a/actuator/robstride/src/lib.rs b/actuator/robstride/src/lib.rs index 3204e03..abb5755 100644 --- a/actuator/robstride/src/lib.rs +++ b/actuator/robstride/src/lib.rs @@ -1,19 +1,15 @@ mod can; mod config; mod motor; +mod port; mod supervisor; mod types; pub use can::{CanComMode, CanPack, ExId}; pub use config::{MotorConfig, ROBSTRIDE_CONFIGS}; -pub use motor::{Motor, MotorControlParams, MotorFeedback, MotorFeedbackRaw, Motors}; +pub use motor::{MotorControlParams, MotorFeedback, Motors}; pub use supervisor::MotorsSupervisor; -pub use types::{MotorMode, MotorType, RunMode}; - -pub const CAN_ID_MASTER: u8 = 0x00; -pub const CAN_ID_MOTOR_DEFAULT: u8 = 0x7F; -pub const CAN_ID_BROADCAST: u8 = 0xFE; -pub const CAN_ID_DEBUG_UI: u8 = 0xFD; +pub use types::{motor_type_from_str, MotorMode, MotorType, RunMode}; #[cfg(target_os = "linux")] pub const BAUD_RATE: nix::sys::termios::BaudRate = nix::sys::termios::BaudRate::B921600; diff --git a/actuator/robstride/src/motor.rs b/actuator/robstride/src/motor.rs index c6ae4d1..d73aaf8 100644 --- a/actuator/robstride/src/motor.rs +++ b/actuator/robstride/src/motor.rs @@ -1,11 +1,19 @@ +use crate::can::{float_to_uint, rx_unpack, tx_packs, uint_to_float, unpack_raw_feedback, ExId}; +use crate::can::{CanComMode, CanPack}; +use crate::config::MotorConfig; +use crate::config::ROBSTRIDE_CONFIGS; +use crate::port::init_serial_port; +use crate::types::{MotorMode, MotorType, RunMode}; use serde::{Deserialize, Serialize}; use serialport::TTYPort; use std::collections::HashMap; -use std::io; +use std::thread; +use std::time::Duration; -use crate::can::{CanComMode, CanPack}; -use crate::config::MotorConfig; -use crate::types::{MotorMode, MotorType, RunMode}; +pub const CAN_ID_MASTER: u8 = 0x00; +pub const CAN_ID_MOTOR_DEFAULT: u8 = 0x7F; +pub const CAN_ID_BROADCAST: u8 = 0xFE; +pub const CAN_ID_DEBUG_UI: u8 = 0xFD; #[derive(Debug, Clone, Copy, Serialize, Deserialize)] pub struct MotorControlParams { @@ -16,6 +24,18 @@ pub struct MotorControlParams { pub torque: f32, } +impl Default for MotorControlParams { + fn default() -> Self { + MotorControlParams { + position: 0.0, + velocity: 0.0, + kp: 0.0, + kd: 0.0, + torque: 0.0, + } + } +} + #[derive(Debug, Default, Clone, Serialize, Deserialize)] pub struct MotorFeedback { pub can_id: u8, @@ -26,22 +46,12 @@ pub struct MotorFeedback { pub faults: u16, } -#[derive(Debug, Default, Clone, Serialize, Deserialize)] -pub struct MotorFeedbackRaw { - pub can_id: u8, - pub pos_int: u16, - pub vel_int: u16, - pub torque_int: u16, - pub mode: MotorMode, - pub faults: u16, -} - pub struct Motors { - port: TTYPort, - motor_configs: HashMap, - mode: RunMode, - sleep_time: Duration, - verbose: bool, + pub port: TTYPort, + pub motor_configs: HashMap, + pub mode: RunMode, + pub sleep_time: Duration, + pub verbose: bool, } impl Motors { @@ -158,7 +168,7 @@ impl Motors { Ok(modes) } - fn send_set_mode( + pub fn send_set_mode( &mut self, mode: RunMode, ) -> Result, std::io::Error> { diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs index 829a0e8..28eabb3 100644 --- a/actuator/robstride/src/supervisor.rs +++ b/actuator/robstride/src/supervisor.rs @@ -4,7 +4,8 @@ use std::thread; use std::time::Duration; use crate::motor::{MotorControlParams, MotorFeedback, Motors}; -use crate::types::MotorType; +use crate::types::{MotorType, RunMode}; +use log::{error, info}; pub struct MotorsSupervisor { motors: Arc>,