diff --git a/actuator/rust/bindings/bindings.pyi b/actuator/rust/bindings/bindings.pyi index 9c2b632..4c76afb 100644 --- a/actuator/rust/bindings/bindings.pyi +++ b/actuator/rust/bindings/bindings.pyi @@ -26,7 +26,7 @@ class PyRobstrideMotorFeedback: class PyRobstrideMotors: - def __new__(cls,port_name:str, motor_infos:typing.Mapping[int, str]): ... + def __new__(cls,port_name,motor_infos,verbose = ...): ... def send_get_mode(self) -> dict[int, str]: ... @@ -53,7 +53,7 @@ class PyRobstrideMotors: class PyRobstrideMotorsSupervisor: - def __new__(cls,port_name:str, motor_infos:typing.Mapping[int, str]): ... + def __new__(cls,port_name:str, motor_infos:typing.Mapping[int, str], verbose:bool): ... def set_position(self, motor_id:int, position:float) -> None: ... diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs index 487cb6f..bb559e3 100644 --- a/actuator/rust/bindings/src/lib.rs +++ b/actuator/rust/bindings/src/lib.rs @@ -9,6 +9,7 @@ use robstride::{ }; use std::collections::HashMap; use std::time::Duration; + #[gen_stub_pyclass] #[pyclass] struct PyRobstrideMotors { @@ -19,7 +20,8 @@ struct PyRobstrideMotors { #[pymethods] impl PyRobstrideMotors { #[new] - fn new(port_name: String, motor_infos: HashMap) -> PyResult { + #[pyo3(signature = (port_name, motor_infos, verbose = false))] + fn new(port_name: String, motor_infos: HashMap, verbose: bool) -> PyResult { let motor_infos = motor_infos .into_iter() .map(|(id, type_str)| { @@ -28,7 +30,7 @@ impl PyRobstrideMotors { }) .collect::>>()?; - let motors = RobstrideMotors::new(&port_name, &motor_infos) + let motors = RobstrideMotors::new(&port_name, &motor_infos, verbose) .map_err(|e| PyErr::new::(e.to_string()))?; Ok(PyRobstrideMotors { inner: motors }) @@ -225,7 +227,7 @@ struct PyRobstrideMotorsSupervisor { #[pymethods] impl PyRobstrideMotorsSupervisor { #[new] - fn new(port_name: String, motor_infos: HashMap) -> PyResult { + fn new(port_name: String, motor_infos: HashMap, verbose: bool) -> PyResult { let motor_infos = motor_infos .into_iter() .map(|(id, type_str)| { @@ -234,7 +236,7 @@ impl PyRobstrideMotorsSupervisor { }) .collect::>>()?; - let controller = RobstrideMotorsSupervisor::new(&port_name, &motor_infos) + let controller = RobstrideMotorsSupervisor::new(&port_name, &motor_infos, verbose) .map_err(|e| PyErr::new::(e.to_string()))?; Ok(PyRobstrideMotorsSupervisor { inner: controller }) diff --git a/actuator/rust/robstride/Cargo.toml b/actuator/rust/robstride/Cargo.toml index 8a93fb3..5186522 100644 --- a/actuator/rust/robstride/Cargo.toml +++ b/actuator/rust/robstride/Cargo.toml @@ -17,3 +17,4 @@ serialport = "4.5.1" ctrlc = "3.4.5" lazy_static = "1.4.0" spin_sleep = "1.2.1" +clap = { version = "4.3", features = ["derive"] } diff --git a/actuator/rust/robstride/src/bin/supervisor.rs b/actuator/rust/robstride/src/bin/supervisor.rs index 3157f63..9ffa33a 100644 --- a/actuator/rust/robstride/src/bin/supervisor.rs +++ b/actuator/rust/robstride/src/bin/supervisor.rs @@ -1,8 +1,18 @@ +use clap::Parser; use robstride::{motor_type_from_str, MotorType, MotorsSupervisor}; use std::collections::HashMap; use std::io::{self, Write}; +#[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] +struct Args { + #[arg(short, long, help = "Enable verbose output")] + verbose: bool, +} + fn main() -> Result<(), Box> { + let args = Args::parse(); + print!("Enter the TEST_ID (u8): "); io::stdout().flush()?; let mut input = String::new(); @@ -32,7 +42,7 @@ fn main() -> Result<(), Box> { }; let motor_type = motor_type_from_str(motor_type_input.as_str())?; let motor_infos: HashMap = HashMap::from([(test_id, motor_type)]); - let controller = MotorsSupervisor::new(&port_name, &motor_infos)?; + let controller = MotorsSupervisor::new(&port_name, &motor_infos, args.verbose)?; println!("Motor Controller Test CLI"); println!("Available commands:"); @@ -44,6 +54,7 @@ fn main() -> Result<(), Box> { println!(" zero / z"); println!(" get_feedback / g"); println!(" pause / w"); + println!(" reset / r"); println!(" quit / q"); loop { @@ -116,6 +127,10 @@ fn main() -> Result<(), Box> { controller.toggle_pause(); println!("Toggled pause state"); } + "reset" | "r" => { + controller.reset(); + println!("Reset motors"); + } "quit" | "q" => { controller.stop(); println!("Exiting..."); diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 80911f4..15b4b20 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -43,7 +43,7 @@ lazy_static! { v_max: 44.0, kp_min: 0.0, kp_max: 500.0, - kp_default: 20.0, + kp_default: 10.0, kd_min: 0.0, kd_max: 5.0, kd_default: 1.0, @@ -80,10 +80,10 @@ lazy_static! { v_max: 20.0, kp_min: 0.0, kp_max: 5000.0, - kp_default: 1000.0, + kp_default: 10.0, kd_min: 0.0, kd_max: 100.0, - kd_default: 10.0, + kd_default: 1.0, t_min: -60.0, t_max: 60.0, zero_on_init: false, @@ -98,10 +98,10 @@ lazy_static! { v_max: 15.0, kp_min: 0.0, kp_max: 5000.0, - kp_default: 1000.0, + kp_default: 10.0, kd_min: 0.0, kd_max: 100.0, - kd_default: 10.0, + kd_default: 1.0, t_min: -120.0, t_max: 120.0, zero_on_init: false, @@ -112,7 +112,7 @@ lazy_static! { } #[repr(u8)] -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, PartialEq)] pub enum CanComMode { AnnounceDevId = 0, MotorCtrl, @@ -206,11 +206,23 @@ fn init_serial_port(device: &str) -> Result, serialport::Err Ok(port) } +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 +} + +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 +} + 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()).rev() { + for (&value, &bits) in values.iter().zip(bit_lengths.iter()) { let mask = (1 << bits) - 1; result |= (value & mask) << current_shift; current_shift += bits; @@ -219,94 +231,98 @@ fn pack_bits(values: &[u32], bit_lengths: &[u8]) -> u32 { result } -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 -} +fn unpack_bits(value: u32, bit_lengths: &[u8]) -> Vec { + let mut result = Vec::new(); + let mut current_value = value; -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 -} + for &bits in bit_lengths.iter() { + let mask = (1 << bits) - 1; + result.push(current_value & mask); + current_value >>= bits; + } -fn tx_pack(port: &mut Box, pack: &CanPack) -> Result<(), std::io::Error> { - let mut buffer = Vec::new(); - buffer.extend_from_slice(b"AT"); + result +} +fn pack_ex_id(ex_id: &ExId) -> [u8; 4] { let addr = (pack_bits( &[ - pack.ex_id.res as u32, - pack.ex_id.mode as u32, - pack.ex_id.data as u32, - pack.ex_id.id as u32, + ex_id.id as u32, + ex_id.data as u32, + ex_id.mode as u32, + ex_id.res as u32, ], - &[3, 5, 16, 8], + &[8, 16, 5, 3], ) << 3) | 0x00000004; + addr.to_be_bytes() +} - buffer.extend_from_slice(&addr.to_be_bytes()); +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, + } +} + +fn tx_pack( + port: &mut Box, + pack: &CanPack, + verbose: bool, +) -> Result<(), std::io::Error> { + let mut buffer = Vec::new(); + buffer.extend_from_slice(b"AT"); + + buffer.extend_from_slice(&pack_ex_id(&pack.ex_id)); buffer.push(pack.len); buffer.extend_from_slice(&pack.data[..pack.len as usize]); buffer.extend_from_slice(b"\r\n"); - // println!( - // "TX: {}", - // buffer - // .iter() - // .map(|b| format!("{:02X}", b)) - // .collect::>() - // .join(" ") - // ); + if verbose { + println!( + "TX: {}", + buffer + .iter() + .map(|b| format!("{:02X}", b)) + .collect::>() + .join(" ") + ); + } port.write_all(&buffer)?; port.flush()?; Ok(()) } -fn rx_unpack(port: &mut Box) -> Result { - let mut buffer = [0u8; 7]; - port.read_exact(&mut buffer)?; - - // print!( - // "RX: {} ", - // buffer - // .iter() - // .map(|b| format!("{:02X}", b)) - // .collect::>() - // .join(" ") - // ); - - if buffer[0] == b'A' && buffer[1] == b'T' { - let addr = u32::from_be_bytes([buffer[2], buffer[3], buffer[4], buffer[5]]) >> 3; - let ex_id = ExId { - id: (addr & 0xFF) as u8, - data: ((addr >> 8) & 0xFFFF) as u16, - mode: unsafe { std::mem::transmute((addr >> 24) as u8) }, - res: 0, - }; +fn rx_unpack(port: &mut Box, verbose: bool) -> Result { + let mut buffer = [0u8; 17]; + let bytes_read = port.read(&mut buffer)?; + + if verbose { + println!( + "RX: {} ", + buffer + .iter() + .map(|b| format!("{:02X}", b)) + .collect::>() + .join(" ") + ); + } + + if bytes_read == 17 && buffer[0] == b'A' && buffer[1] == b'T' { + let ex_id = unpack_ex_id([buffer[2], buffer[3], buffer[4], buffer[5]]); let len = buffer[6]; - let mut buffer = vec![0u8; len as usize + 2]; - port.read_exact(&mut buffer)?; - - // println!( - // "{}", - // buffer - // .iter() - // .map(|b| format!("{:02X}", b)) - // .collect::>() - // .join(" ") - // ); Ok(CanPack { ex_id, len, - data: buffer, + data: buffer[7..(7 + len as usize)].to_vec(), }) } else { - println!("Failed to read CAN packet"); - Err(std::io::Error::new( std::io::ErrorKind::UnexpectedEof, "Failed to read CAN packet", @@ -314,13 +330,27 @@ fn rx_unpack(port: &mut Box) -> Result } } -fn rx_unpack_feedback(port: &mut Box) -> Result { - match rx_unpack(port) { +fn rx_unpack_feedback( + port: &mut Box, + verbose: bool, +) -> Result { + match rx_unpack(port, verbose) { Ok(pack) => { 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 Ok(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]]); @@ -387,12 +417,14 @@ pub struct Motors { pending_responses: usize, mode: RunMode, sleep_time: Duration, + verbose: bool, } impl Motors { pub fn new( port_name: &str, motor_infos: &HashMap, + verbose: bool, ) -> Result> { let port = init_serial_port(port_name)?; let motor_configs: HashMap = motor_infos @@ -412,11 +444,12 @@ impl Motors { pending_responses: 0, mode: RunMode::UnsetMode, sleep_time: Duration::from_millis(50), + verbose, }) } fn send_command(&mut self, pack: &CanPack) -> Result<(), std::io::Error> { - tx_pack(&mut self.port, pack)?; + tx_pack(&mut self.port, pack, self.verbose)?; self.pending_responses += 1; Ok(()) } @@ -438,10 +471,10 @@ impl Motors { let index: u16 = 0x7005; pack.data[..2].copy_from_slice(&index.to_le_bytes()); - tx_pack(&mut self.port, &pack)?; + tx_pack(&mut self.port, &pack, self.verbose)?; } - match rx_unpack(&mut self.port) { + match rx_unpack(&mut self.port, self.verbose) { Ok(pack) => { let mode = unsafe { std::mem::transmute(pack.data[4] as u8) }; Ok(HashMap::from([(pack.ex_id.id, mode)])) @@ -591,11 +624,11 @@ impl Motors { let index: u16 = index; pack.data[..2].copy_from_slice(&index.to_le_bytes()); - tx_pack(&mut self.port, &pack)?; + tx_pack(&mut self.port, &pack, self.verbose)?; let mut packs = Vec::new(); for _ in 0..num_packs { - packs.push(rx_unpack(&mut self.port)?); + packs.push(rx_unpack(&mut self.port, self.verbose)?); } let name = packs @@ -607,7 +640,7 @@ impl Motors { Ok(name) } - fn read_uint_param(&mut self, motor_id: u8, index: u16) -> Result { + fn read_uint16_param(&mut self, motor_id: u8, index: u16) -> Result { let mut pack = CanPack { ex_id: ExId { id: motor_id, @@ -621,10 +654,10 @@ impl Motors { let index: u16 = index; pack.data[..2].copy_from_slice(&index.to_le_bytes()); - tx_pack(&mut self.port, &pack)?; + tx_pack(&mut self.port, &pack, self.verbose)?; - let pack = rx_unpack(&mut self.port)?; - let value = u32::from_le_bytes(pack.data[4..8].try_into().unwrap()); + let pack = rx_unpack(&mut self.port, self.verbose)?; + let value = u16::from_le_bytes(pack.data[4..6].try_into().unwrap()); Ok(value) } @@ -662,13 +695,13 @@ impl Motors { Ok(names) } - pub fn read_can_timeout(&mut self) -> Result, std::io::Error> { + pub fn read_can_timeouts(&mut self) -> Result, std::io::Error> { let motor_ids = self.motor_configs.keys().cloned().collect::>(); let mut timeouts = HashMap::new(); for id in motor_ids { - let timeout = self.read_uint_param(id, 0x200c)?; - timeouts.insert(id, timeout); + let timeout = self.read_uint16_param(id, 0x200c)?; + timeouts.insert(id, timeout as f32 / 20.0); } Ok(timeouts) } @@ -817,7 +850,7 @@ impl Motors { fn read_all_pending_responses(&mut self) -> Result, std::io::Error> { while self.pending_responses > 0 { - match rx_unpack_feedback(&mut self.port) { + match rx_unpack_feedback(&mut self.port, self.verbose) { Ok(raw_feedback) => { if let Some(config) = self.motor_configs.get(&raw_feedback.can_id) { let position = @@ -867,15 +900,17 @@ pub struct MotorsSupervisor { motors_to_zero: Arc>>, sleep_duration: Arc>, paused: Arc>, + restart: Arc>, } impl MotorsSupervisor { pub fn new( port_name: &str, motor_infos: &HashMap, + verbose: bool, ) -> Result> { // Initialize Motors - let motors = Motors::new(port_name, motor_infos)?; + let motors = Motors::new(port_name, motor_infos, verbose)?; // Get default KP/KD values for all motors. let target_params = motors @@ -908,6 +943,7 @@ impl MotorsSupervisor { let target_params = Arc::new(Mutex::new(target_params)); let running = Arc::new(Mutex::new(true)); let paused = Arc::new(Mutex::new(false)); + let restart = Arc::new(Mutex::new(false)); let controller = MotorsSupervisor { motors, @@ -917,6 +953,7 @@ impl MotorsSupervisor { motors_to_zero, sleep_duration: Arc::new(Mutex::new(Duration::from_micros(10))), paused, + restart, }; controller.start_control_thread(); @@ -932,12 +969,14 @@ impl MotorsSupervisor { let motors_to_zero = Arc::clone(&self.motors_to_zero); let sleep_duration = Arc::clone(&self.sleep_duration); let paused = Arc::clone(&self.paused); + let restart = Arc::clone(&self.restart); thread::spawn(move || { let mut motors = motors.lock().unwrap(); + let _ = motors.send_reset(); - let _ = motors.send_can_timeout(100); // If motor doesn't receive a command for 100ms, it will stop. let _ = motors.send_start(); + // let _ = motors.send_can_timeout(100); // If motor doesn't receive a command for 100ms, it will stop. while *running.lock().unwrap() { // If paused, just wait 100ms without sending any commands. @@ -946,6 +985,12 @@ impl MotorsSupervisor { continue; } + if *restart.lock().unwrap() { + *restart.lock().unwrap() = false; + let _ = motors.send_reset(); + let _ = motors.send_start(); + } + // Read latest feedback from motors. { let latest_feedback_from_motors = motors.get_latest_feedback(); @@ -1042,6 +1087,11 @@ impl MotorsSupervisor { } pub fn add_motor_to_zero(&self, motor_id: u8) { + // We need to set the motor parameters to zero to avoid the motor + // rapidly changing to the new target after it is zeroed. + self.set_torque(motor_id, 0.0); + self.set_position(motor_id, 0.0); + self.set_velocity(motor_id, 0.0); let mut motors_to_zero = self.motors_to_zero.lock().unwrap(); motors_to_zero.insert(motor_id); } @@ -1056,6 +1106,11 @@ impl MotorsSupervisor { *paused = !*paused; } + pub fn reset(&self) { + let mut restart = self.restart.lock().unwrap(); + *restart = true; + } + pub fn stop(&self) { { let mut running = self.running.lock().unwrap();