Merge pull request #26 from pollen-robotics/3dof_zero

3dof zero

build.rs

@@ -1,5 +1,48 @@
+use std::process::Command;
+use std::{env, fs::File, io::Write, path::Path};
 fn main() {
     println!("cargo:rustc-link-arg-bins=--nmagic");
     println!("cargo:rustc-link-arg-bins=-Tlink.x");
     println!("cargo:rustc-link-arg-bins=-Tdefmt.x");
+
+    // Create a build time file for constants
+    let out_dir = env::var("OUT_DIR").expect("No out dir");
+    let dest_path = Path::new(&out_dir).join("constants.rs");
+    let mut f = File::create(&dest_path).expect("Could not create file");
+
+    // Trick to get the current commit hash and pass it to the firmware
+    let output = Command::new("git")
+        .args(&["rev-parse", "HEAD"])
+        .output()
+        .unwrap();
+    let mut git_hash = String::from_utf8(output.stdout).unwrap();
+    git_hash.pop(); //remove trainling '\n'
+                    // println!("cargo:rustc-env=GIT_HASH={}", git_hash);
+    write!(&mut f, "pub const GIT_HASH: &str = \"{}\";", git_hash).expect("Could not write file");
+    // Get firmware zero values
+    let zeros = option_env!("ZEROS");
+    if let Some(zeros) = zeros {
+        let zeros: Vec<f32> = zeros
+            .split(",")
+            .filter_map(|s| s.parse::<f32>().ok())
+            .collect();
+        writeln!(
+            &mut f,
+            "pub const HARDWARE_ZEROS: [f32;3] = [{:.32}, {:.32}, {:.32}];",
+            zeros[0], zeros[1], zeros[2]
+        )
+        .expect("Could not write file"); // {:.32} to be sure to print the full precision. It counts...
+    } else {
+        writeln!(&mut f, "pub const HARDWARE_ZEROS: [f32;3] = [0.0,0.0,0.0];")
+            .expect("Could not write file");
+    }
+
+    // Get Dynamixel id
+    let id = option_env!("DXL_ID");
+    if let Some(id) = id {
+        let id = id.parse::<u8>().ok().unwrap();
+        writeln!(&mut f, "pub static DXL_ID: u8 = {:?};", id).expect("Could not write file");
+    } else {
+        writeln!(&mut f, "pub static DXL_ID: u8 = 42;").expect("Could not write file");
+    }
 }

src/config/mod.rs

@@ -9,7 +9,7 @@ pub const N_AXIS: usize = 2;
 #[cfg(feature = "orbita3d")]
 pub const N_AXIS: usize = 3;
 
-pub static DXL_ID: u8 = 52;
+// pub static DXL_ID: u8 = 42;
 
 pub type DynamixelUart = Uart<'static, p::USART1, p::DMA1_CH0, p::DMA1_CH1>;
 
@@ -48,6 +48,9 @@ pub type AD5047Bot<'d> = AD5047Sensor<'d, p::SPI4, p::PA15>;
 // pub type DonutHall<'d> = I2cHallSensor<'d, p::I2C1, p::PB6, p::PB7>;
 pub type DonutHall<'d> = I2cHallSensor<p::I2C1>;
 
+// from build.rs (should contain DXL_ID, HARDWARE_ZEROS and GIT_HASH)
+include!(concat!(env!("OUT_DIR"), "/constants.rs"));
+
 pub struct ActuatorConfig {
     #[cfg(feature = "orbita3d")]
     pub a: VentouseAConfig,

src/dynamixel/conversion.rs

@@ -38,7 +38,6 @@ pub fn float_to_bytes<const N: usize>(data: [f32; N]) -> [u8; 4 * N] {
     result
 }
 
-
 pub fn u32_to_bytes<const N: usize>(data: [u32; N]) -> [u8; 4 * N] {
     let mut result = [0; 4 * N];
 
@@ -49,7 +48,6 @@ pub fn u32_to_bytes<const N: usize>(data: [u32; N]) -> [u8; 4 * N] {
     result
 }
 
-
 pub fn bytes_to_u32<const N: usize>(data: &[u8]) -> [u32; N] {
     assert!(data.len() == N * 4);
     let mut result = [0; N];
@@ -59,8 +57,6 @@ pub fn bytes_to_u32<const N: usize>(data: &[u8]) -> [u32; N] {
     result
 }
 
-
-
 pub fn u16_to_bytes<const N: usize>(data: [u16; N]) -> [u8; 2 * N] {
     let mut result = [0; 2 * N];
 
@@ -71,7 +67,6 @@ pub fn u16_to_bytes<const N: usize>(data: [u16; N]) -> [u8; 2 * N] {
     result
 }
 
-
 pub fn bytes_to_u16<const N: usize>(data: &[u8]) -> [u16; N] {
     assert!(data.len() == N * 2);
     let mut result = [0; N];
@@ -81,7 +76,6 @@ pub fn bytes_to_u16<const N: usize>(data: &[u8]) -> [u16; N] {
     result
 }
 
-
 pub fn i16_to_bytes<const N: usize>(data: [i16; N]) -> [u8; 2 * N] {
     let mut result = [0; 2 * N];
 
@@ -92,7 +86,6 @@ pub fn i16_to_bytes<const N: usize>(data: [i16; N]) -> [u8; 2 * N] {
     result
 }
 
-
 pub fn bytes_to_i16<const N: usize>(data: &[u8]) -> [i16; N] {
     assert!(data.len() == N * 2);
     let mut result = [0; N];
@@ -102,31 +95,27 @@ pub fn bytes_to_i16<const N: usize>(data: &[u8]) -> [i16; N] {
     result
 }
 
-
-pub fn pid_to_bytes<const N:usize>(pid: [Pid;N]) -> [u8; 4*N] {
+pub fn pid_to_bytes<const N: usize>(pid: [Pid; N]) -> [u8; 4 * N] {
     let mut result = [0; 4 * N];
     for i in 0..N {
-	let rawpid=(pid[i].p as u32) << 16 | (pid[i].i as u32) ;
-	result[i * 4..(i + 1) * 4].copy_from_slice(&rawpid.to_le_bytes());
+        let rawpid = (pid[i].p as u32) << 16 | (pid[i].i as u32);
+        result[i * 4..(i + 1) * 4].copy_from_slice(&rawpid.to_le_bytes());
     }
     result
-
 }
 
-pub fn bytes_to_pid<const N:usize>(data: &[u8]) -> [Pid;N] {
+pub fn bytes_to_pid<const N: usize>(data: &[u8]) -> [Pid; N] {
     assert!(data.len() == N * 4); //FIXME: remove assert!!
-    let mut result = [Pid{p:0,i:0}; N];
+    let mut result = [Pid { p: 0, i: 0 }; N];
 
     for i in 0..N {
-
-	let rawpid = u32::from_le_bytes(data[i * 4..(i + 1) * 4].try_into().unwrap());
-	let pid=Pid {
-		p: ((rawpid >> 16) as i16) & 0x7FFF,
-		i: (rawpid as i16) & 0x7FFF,
-	};
+        let rawpid = u32::from_le_bytes(data[i * 4..(i + 1) * 4].try_into().unwrap());
+        let pid = Pid {
+            p: ((rawpid >> 16) as i16) & 0x7FFF,
+            i: (rawpid as i16) & 0x7FFF,
+        };
 
         result[i] = pid;
     }
     result
-
 }

src/dynamixel/packet/instruction_packet.rs

@@ -1,4 +1,4 @@
-use defmt::{Format, trace};
+use defmt::{trace, Format};
 
 use super::{crc, ParsingError};
 
@@ -16,79 +16,70 @@ impl<'d> InstructionPacketKind<'d> {
             return Err(ParsingError::InvalidPacket);
         }
 
-/*
-        if bytes[0] != 0xFF || bytes[1] != 0xFF {
-            return Err(ParsingError::InvalidPacket);
-        }
+        /*
+                if bytes[0] != 0xFF || bytes[1] != 0xFF {
+                    return Err(ParsingError::InvalidPacket);
+                }
 
-        let id = bytes[2];
-        if id != receiver_id {
-            return Err(ParsingError::IgnorePacket(receiver_id, id));
-        }
+                let id = bytes[2];
+                if id != receiver_id {
+                    return Err(ParsingError::IgnorePacket(receiver_id, id));
+                }
 
-        let length = bytes[3];
-        if length as usize != bytes.len() - 4 {
-            return Err(ParsingError::InvalidPacket);
-        }
+                let length = bytes[3];
+                if length as usize != bytes.len() - 4 {
+                    return Err(ParsingError::InvalidPacket);
+                }
 
-        let instruction = bytes[4];
+                let instruction = bytes[4];
 
-        let params = &bytes[5..bytes.len() - 1];
+                let params = &bytes[5..bytes.len() - 1];
 
-        let received_crc = *bytes.last().unwrap();
-        let calculated_crc = crc(&bytes[2..bytes.len() - 1]);
-*/
+                let received_crc = *bytes.last().unwrap();
+                let calculated_crc = crc(&bytes[2..bytes.len() - 1]);
+        */
 
-	//At least it is easy to find a complete packet inside a buffer
-	let mut idx:usize = 0;
-	let mut buffer_checked=false;
+        //At least it is easy to find a complete packet inside a buffer
+        let mut idx: usize = 0;
+        let mut buffer_checked = false;
         let mut id = 0;
         let mut length = 0;
 
-	while !buffer_checked
-	{
-	    while  !(bytes[idx] == 0xFF && bytes[idx+1] == 0xFF)
-	    {
-		idx+=1;
-		if bytes.len() - idx < 6 {
-		    return Err(ParsingError::InvalidPacket);
-		}
-
-	    }
-
-            id = bytes[idx+2];
-            length = bytes[idx+3];
-
-
-            if id != receiver_id{
-		if length as usize == bytes.len()-idx - 4{
-		    //If the id is wrong and there is only one packet in the buffer, we can ignore it and return
-		    return Err(ParsingError::IgnorePacket(receiver_id, id));
-		}
-		else{
-		    //If the id is wrong and there are more packets in the buffer, we need to continue searching
-		    idx+=1;
-		    continue;
-
-		}
+        while !buffer_checked {
+            while !(bytes[idx] == 0xFF && bytes[idx + 1] == 0xFF) {
+                idx += 1;
+                if bytes.len() - idx < 6 {
+                    return Err(ParsingError::InvalidPacket);
+                }
             }
 
-
-            if length as usize > bytes.len()-idx - 4 {
-		//if the packet is not complete, we can ignore it and return
-		return Err(ParsingError::InvalidPacket);
+            id = bytes[idx + 2];
+            length = bytes[idx + 3];
+
+            if id != receiver_id {
+                if length as usize == bytes.len() - idx - 4 {
+                    //If the id is wrong and there is only one packet in the buffer, we can ignore it and return
+                    return Err(ParsingError::IgnorePacket(receiver_id, id));
+                } else {
+                    //If the id is wrong and there are more packets in the buffer, we need to continue searching
+                    idx += 1;
+                    continue;
+                }
             }
 
-	    //If we are here, we have a complete packet
-	    buffer_checked=true;
-
+            if length as usize > bytes.len() - idx - 4 {
+                //if the packet is not complete, we can ignore it and return
+                return Err(ParsingError::InvalidPacket);
+            }
 
-	}
+            //If we are here, we have a complete packet
+            buffer_checked = true;
+        }
 
-        let instruction = bytes[idx+4];
-        let params = &bytes[idx+5..idx+5+(length as usize) -2];
-	let received_crc = bytes[idx+5+(length as usize) -2];
-        let calculated_crc = crc(&bytes[idx+2..idx+5+(length as usize) - 2]);
+        let instruction = bytes[idx + 4];
+        let params = &bytes[idx + 5..idx + 5 + (length as usize) - 2];
+        let received_crc = bytes[idx + 5 + (length as usize) - 2];
+        let calculated_crc = crc(&bytes[idx + 2..idx + 5 + (length as usize) - 2]);
 
         if received_crc != calculated_crc {
             return Err(ParsingError::InvalidChecksum);

src/dynamixel/register.rs

@@ -3,7 +3,6 @@ pub enum DynamixelRegister {
     FirmwareVersion,
     Id,
 
-
     FluxPID,
     TorquePID,
     VelocityPID,
@@ -31,50 +30,41 @@ pub enum DynamixelRegister {
     IndexSensor,
 
     FullState,
-
-
 }
 
 impl DynamixelRegister {
     pub fn with_address(address: u8) -> Option<Self> {
         match address {
-	    0 => Some(DynamixelRegister::ModelNumber),
-	    6 => Some(DynamixelRegister::FirmwareVersion),
-	    7 => Some(DynamixelRegister::Id),
-
-	    10 => Some(DynamixelRegister::VelocityLimit),
-	    14 => Some(DynamixelRegister::TorqueFluxLimit),
-	    18 => Some(DynamixelRegister::UqUdLimit),
-
-	    20 => Some(DynamixelRegister::FluxPID),
-	    24 => Some(DynamixelRegister::TorquePID),
-	    28 => Some(DynamixelRegister::VelocityPID),
-	    32 => Some(DynamixelRegister::PositionPID),
+            0 => Some(DynamixelRegister::ModelNumber),
+            6 => Some(DynamixelRegister::FirmwareVersion),
+            7 => Some(DynamixelRegister::Id),
 
+            10 => Some(DynamixelRegister::VelocityLimit),
+            14 => Some(DynamixelRegister::TorqueFluxLimit),
+            18 => Some(DynamixelRegister::UqUdLimit),
 
+            20 => Some(DynamixelRegister::FluxPID),
+            24 => Some(DynamixelRegister::TorquePID),
+            28 => Some(DynamixelRegister::VelocityPID),
+            32 => Some(DynamixelRegister::PositionPID),
 
             40 => Some(DynamixelRegister::TorqueEnable),
             51 => Some(DynamixelRegister::CurrentVelocity),
             50 => Some(DynamixelRegister::CurrentPosition),
-	    54 => Some(DynamixelRegister::FeedforwardVelocity),
+            54 => Some(DynamixelRegister::FeedforwardVelocity),
             52 => Some(DynamixelRegister::CurrentTorque),
             60 => Some(DynamixelRegister::TargetPosition),
             62 => Some(DynamixelRegister::TargetPositionWithVelocityFF),
             64 => Some(DynamixelRegister::TargetPositionEstimateVelocityFF),
 
-	    90 => Some(DynamixelRegister::AxisSensor),
-
-
-	    #[cfg(feature = "orbita3d")]
-	    99 => Some(DynamixelRegister::IndexSensor),
-
-
-	    100 => Some(DynamixelRegister::FullState),
-
+            90 => Some(DynamixelRegister::AxisSensor),
 
-	    _ => None
+            #[cfg(feature = "orbita3d")]
+            99 => Some(DynamixelRegister::IndexSensor),
 
+            100 => Some(DynamixelRegister::FullState),
 
+            _ => None,
         }
     }
 }

src/dynamixel/task.rs

@@ -24,7 +24,7 @@ pub async fn messsage_handler(usart: config::DynamixelUart, dir_pin: AnyPin) {
         match dxl.read().await {
             Ok(packet) => {
                 debug!("Got packet: {:?}", packet);
-                dxl_error = {SHARED_MEMORY.lock().await.get_error_state()} as u8;
+                dxl_error = { SHARED_MEMORY.lock().await.get_error_state() } as u8;
                 match packet {
                     InstructionPacketKind::Ping(_) => {
                         let sp = StatusPacket::ack(id, dxl_error);