fix lint

kscalelabs · Nov 8, 2024 · 10ac71c · 10ac71c
1 parent 8f6f347
commit 10ac71c
Show file tree

Hide file tree

Showing 4 changed files with 139 additions and 28 deletions.
diff --git 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<u8>,
 }
 
+#[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<u32> {
+    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
@@ -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
@@ -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<u8, &'static MotorConfig>,
-    mode: RunMode,
-    sleep_time: Duration,
-    verbose: bool,
+    pub port: TTYPort,
+    pub motor_configs: HashMap<u8, &'static MotorConfig>,
+    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<HashMap<u8, MotorFeedback>, std::io::Error> {

diff --git 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<Mutex<Motors>>,