diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 5b8e8e4..6228d71 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -129,6 +129,124 @@ pub struct MotorFeedback { pub faults: u16, } +fn init_serial_port(device: &str) -> Result, serialport::Error> { + let port = serialport::new(device, BAUDRATE) + .data_bits(serialport::DataBits::Eight) + .flow_control(serialport::FlowControl::None) + .parity(serialport::Parity::None) + .stop_bits(serialport::StopBits::One) + .timeout(Duration::from_millis(10)) + .open()?; + println!("Serial port initialized successfully"); + Ok(port) +} + +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() { + let mask = (1 << bits) - 1; + result |= (value & mask) << current_shift; + current_shift += bits; + } + + 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 txd_pack(port: &mut Box, pack: &CanPack) -> Result<(), std::io::Error> { + let mut buffer = Vec::new(); + buffer.extend_from_slice(b"AT"); + + 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, + ], + &[3, 5, 16, 8], + ) << 3) + | 0x00000004; + + buffer.extend_from_slice(&addr.to_be_bytes()); + buffer.push(pack.len); + buffer.extend_from_slice(&pack.data[..pack.len as usize]); + buffer.extend_from_slice(b"\r\n"); + + println!("tx {:02X?}", buffer); + + port.write_all(&buffer)?; + port.flush()?; + Ok(()) +} + +fn read_bytes( + port: &mut Box, + config: &MotorConfig, +) -> Result { + let mut buffer = [0u8; 17]; + let bytes_read = port.read(&mut buffer)?; + + println!("rx {:02X?}", &buffer[..bytes_read]); + + if bytes_read == 17 && 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, + }; + + let can_id = ex_id.data & 0x00FF; + let faults = (ex_id.data & 0x3F00) >> 8; + let mode = unsafe { std::mem::transmute(((ex_id.data & 0xC000) >> 14) as u8) }; + + let pos_int_get = u16::from_be_bytes([buffer[7], buffer[8]]); + let vel_int_get = u16::from_be_bytes([buffer[9], buffer[10]]); + let torque_int_get = u16::from_be_bytes([buffer[11], buffer[12]]); + + let position = uint_to_float(pos_int_get, config.p_min, config.p_max, 16); + let velocity = uint_to_float(vel_int_get, config.v_min, config.v_max, 16); + let torque = uint_to_float(torque_int_get, config.t_min, config.t_max, 16); + + let feedback = MotorFeedback { + can_id, + position, + velocity, + torque, + mode, + faults, + }; + + println!("Parsed data:"); + println!(" Motor ID: {}", feedback.can_id); + println!(" Position: {}", feedback.position); + println!(" Velocity: {}", feedback.velocity); + println!(" Torque: {}", feedback.torque); + println!(" Mode: {:?}", feedback.mode); + println!(" Faults: {:?}", feedback.faults); + + Ok(feedback) + } else { + Ok(MotorFeedback { + can_id: 0, + position: 0.0, + velocity: 0.0, + torque: 0.0, + mode: MotorMode::Reset, + faults: 0, + }) + } +} + pub struct Motor { port: Box, config: MotorConfig, @@ -145,90 +263,6 @@ impl Motor { Ok(Motor { port, config, id }) } - fn txd_pack(&mut self, pack: &CanPack) -> Result<(), std::io::Error> { - let mut buffer = Vec::new(); - buffer.extend_from_slice(b"AT"); - - 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, - ], - &[3, 5, 16, 8], - ) << 3) - | 0x00000004; - - buffer.extend_from_slice(&addr.to_be_bytes()); - buffer.push(pack.len); - buffer.extend_from_slice(&pack.data[..pack.len as usize]); - buffer.extend_from_slice(b"\r\n"); - - println!("tx {:02X?}", buffer); - - self.port.write_all(&buffer)?; - self.port.flush()?; - Ok(()) - } - - fn read_bytes(&mut self) -> Result { - let mut buffer = [0u8; 17]; - let bytes_read = self.port.read(&mut buffer)?; - - println!("rx {:02X?}", &buffer[..bytes_read]); - - if bytes_read == 17 && 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, - }; - - let can_id = ex_id.data & 0x00FF; - let faults = (ex_id.data & 0x3F00) >> 8; - let mode = unsafe { std::mem::transmute(((ex_id.data & 0xC000) >> 14) as u8) }; - - let pos_int_get = u16::from_be_bytes([buffer[7], buffer[8]]); - let vel_int_get = u16::from_be_bytes([buffer[9], buffer[10]]); - let torque_int_get = u16::from_be_bytes([buffer[11], buffer[12]]); - - let position = uint_to_float(pos_int_get, self.config.p_min, self.config.p_max, 16); - let velocity = uint_to_float(vel_int_get, self.config.v_min, self.config.v_max, 16); - let torque = uint_to_float(torque_int_get, self.config.t_min, self.config.t_max, 16); - - let feedback = MotorFeedback { - can_id, - position, - velocity, - torque, - mode, - faults, - }; - - println!("Parsed data:"); - println!(" Motor ID: {}", feedback.can_id); - println!(" Position: {}", feedback.position); - println!(" Velocity: {}", feedback.velocity); - println!(" Torque: {}", feedback.torque); - println!(" Mode: {:?}", feedback.mode); - println!(" Faults: {:?}", feedback.faults); - - Ok(feedback) - } else { - Ok(MotorFeedback { - can_id: 0, - position: 0.0, - velocity: 0.0, - torque: 0.0, - mode: MotorMode::Reset, - faults: 0, - }) - } - } - pub fn send_set_mode(&mut self, run_mode: RunMode) -> Result { let mut pack = CanPack { ex_id: ExId { @@ -245,8 +279,8 @@ impl Motor { pack.data[..2].copy_from_slice(&index.to_le_bytes()); pack.data[4] = run_mode as u8; - self.txd_pack(&pack)?; - self.read_bytes() + txd_pack(&mut self.port, &pack)?; + read_bytes(&mut self.port, &self.config) } pub fn send_reset(&mut self) -> Result { @@ -261,8 +295,8 @@ impl Motor { data: [0; 8], }; - self.txd_pack(&pack)?; - self.read_bytes() + txd_pack(&mut self.port, &pack)?; + read_bytes(&mut self.port, &self.config) } pub fn send_start(&mut self) -> Result { @@ -277,8 +311,8 @@ impl Motor { data: [0; 8], }; - self.txd_pack(&pack)?; - self.read_bytes() + txd_pack(&mut self.port, &pack)?; + read_bytes(&mut self.port, &self.config) } pub fn send_set_speed_limit(&mut self, speed: f32) -> Result { @@ -297,8 +331,8 @@ impl Motor { pack.data[..2].copy_from_slice(&index.to_le_bytes()); pack.data[4..8].copy_from_slice(&speed.to_le_bytes()); - self.txd_pack(&pack)?; - self.read_bytes() + txd_pack(&mut self.port, &pack)?; + read_bytes(&mut self.port, &self.config) } pub fn send_set_location(&mut self, location: f32) -> Result { @@ -317,38 +351,7 @@ impl Motor { pack.data[..2].copy_from_slice(&index.to_le_bytes()); pack.data[4..8].copy_from_slice(&location.to_le_bytes()); - self.txd_pack(&pack)?; - self.read_bytes() + txd_pack(&mut self.port, &pack)?; + read_bytes(&mut self.port, &self.config) } } - -fn init_serial_port(device: &str) -> Result, serialport::Error> { - let port = serialport::new(device, BAUDRATE) - .data_bits(serialport::DataBits::Eight) - .flow_control(serialport::FlowControl::None) - .parity(serialport::Parity::None) - .stop_bits(serialport::StopBits::One) - .timeout(Duration::from_millis(10)) - .open()?; - println!("Serial port initialized successfully"); - Ok(port) -} - -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() { - let mask = (1 << bits) - 1; - result |= (value & mask) << current_shift; - current_shift += bits; - } - - 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 -}