From 5409dddbf05ad27ccd9d82b81bdec78617d56b92 Mon Sep 17 00:00:00 2001 From: hacknus Date: Fri, 8 Sep 2023 20:32:57 +0200 Subject: [PATCH] extend DataPacket and implement SpiStatus --- src/lib.rs | 59 ++++++++++++++++++++++++++++++++++-------------- src/registers.rs | 41 +++++++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+), 17 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 75878f3..4706909 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -18,7 +18,7 @@ use embedded_hal::{ spi::{Mode, Phase, Polarity}, }; -use crate::registers::{Address, Registers}; +use crate::registers::{Address, Registers, SpiStatus}; mod registers; @@ -35,6 +35,8 @@ pub struct Tmc5160 { en: Option, /// the max velocity that is set pub v_max: f32, + /// status register of the driver + pub status: SpiStatus, _clock: f32, _step_count: f32, } @@ -50,11 +52,14 @@ pub enum Error { /// Data Exchange packet #[derive(Debug)] -pub struct DataPacket(u8, u32); +pub struct DataPacket { + status: SpiStatus, + data: u32 +} impl fmt::Display for DataPacket { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!(f, "0x{:x}:0x{:x}", self.0, self.1) + write!(f, "0x{:x}:0x{:x}", self.status.to_val(), self.data) } } @@ -71,6 +76,7 @@ impl Tmc5160 cs, en: None, v_max: 0.0, + status: SpiStatus::default(), _clock: 12000000.0, _step_count: 256.0, } @@ -121,7 +127,7 @@ impl Tmc5160 self.cs.set_high().ok(); - Ok(DataPacket(buffer[0], u32::from_be_bytes(ret_val))) + Ok(DataPacket{status: SpiStatus::from(buffer[0]), data: u32::from_be_bytes(ret_val)}) } /// write value to a specified register @@ -141,7 +147,7 @@ impl Tmc5160 self.cs.set_high().ok(); - Ok(DataPacket(buffer[0], u32::from_be_bytes(val))) + Ok(DataPacket{status: SpiStatus::from(buffer[0]), data: u32::from_be_bytes(val)}) } @@ -177,31 +183,44 @@ impl Tmc5160 /// set the position to 0 / home pub fn set_home(&mut self) -> Result> { self.write_register(Registers::XACTUAL, 0)?; - self.write_register(Registers::XTARGET, 0) + let packet = self.write_register(Registers::XTARGET, 0); + if let Ok(p) = &packet { + self.status = p.status; + } + packet } /// stop the motor now pub fn stop(&mut self) -> Result> { self.disable()?; self.write_register(Registers::VSTART, 0)?; - self.write_register(Registers::VMAX, 0) + let packet = self.write_register(Registers::VMAX, 0); + if let Ok(p) = &packet { + self.status = p.status; + } packet } /// check if the motor is moving pub fn is_moving(&mut self) -> Result> { - self.get_drv_status().map(|packet| (packet.0 & 0b1000) != 0b1000) + self.get_drv_status().map(|packet| packet.status.standstill) } /// get the value of the DRV STATUS register pub fn get_drv_status(&mut self) -> Result> { - self.read_register(Registers::DRV_STATUS) + let packet = self.read_register(Registers::DRV_STATUS); + if let Ok(p) = &packet { + self.status = p.status; + } packet } /// set the max velocity (VMAX) pub fn set_velocity(&mut self, velocity: f32) -> Result> { self.v_max = velocity; let v_max = self.speed_from_hz(velocity); - self.write_register(Registers::VMAX, v_max) + let packet = self.write_register(Registers::VMAX, v_max); + if let Ok(p) = &packet { + self.status = p.status; + } packet } /// set the max acceleration (AMAX, DMAX, A1, D1) @@ -210,19 +229,25 @@ impl Tmc5160 self.write_register(Registers::AMAX, a_max)?; self.write_register(Registers::DMAX, a_max)?; self.write_register(Registers::A_1, a_max)?; - self.write_register(Registers::D_1, a_max) + let packet = self.write_register(Registers::D_1, a_max); + if let Ok(p) = &packet { + self.status = p.status; + } packet } /// move to a specific location pub fn move_to(&mut self, target_signed: i32) -> Result> { self.enable()?; let target = (target_signed * self._step_count as i32) as u32; - self.write_register(Registers::XTARGET, target) + let packet = self.write_register(Registers::XTARGET, target); + if let Ok(p) = &packet { + self.status = p.status; + } packet } /// get the current position pub fn get_position(&mut self) -> Result> { - self.read_register(Registers::XACTUAL).map(|val| val.1 as f32 / self._step_count) + self.read_register(Registers::XACTUAL).map(|val| val.data as f32 / self._step_count) } /// set the current position @@ -234,10 +259,10 @@ impl Tmc5160 /// get the current velocity pub fn get_velocity(&mut self) -> Result> { self.read_register(Registers::VACTUAL).map(|target| { - if (target.1 & 0b100000000000000000000000) == 0b100000000000000000000000 { - ((16777216 - target.1 as i32) as f64 / self._step_count as f64) as f32 + if (target.data & 0b100000000000000000000000) == 0b100000000000000000000000 { + ((16777216 - target.data as i32) as f64 / self._step_count as f64) as f32 } else { - ((target.1 as i32) as f64 / self._step_count as f64) as f32 + ((target.data as i32) as f64 / self._step_count as f64) as f32 } }) } @@ -249,6 +274,6 @@ impl Tmc5160 /// get the current target position (XTARGET) pub fn get_target(&mut self) -> Result> { - self.read_register(Registers::XTARGET).map(|packet| packet.1 as i32) + self.read_register(Registers::XTARGET).map(|packet| packet.data as i32) } } diff --git a/src/registers.rs b/src/registers.rs index 954cdff..47ec9af 100644 --- a/src/registers.rs +++ b/src/registers.rs @@ -140,6 +140,47 @@ impl Address for Registers { } } +/// SPI status +#[derive(Debug, Default, Clone, Copy)] +#[allow(dead_code)] +pub struct SpiStatus { + pub status_stop_r: bool, + pub status_stop_l: bool, + pub position_reached: bool, + pub velocity_reached: bool, + pub standstill: bool, + pub sg2: bool, + pub driver_error: bool, + pub reset_flag: bool, +} +impl SpiStatus { + pub fn from(value: u8) -> Self { + Self { + status_stop_r: (value & 0b10000000) >> 7 == 1, + status_stop_l: (value & 0b1000000) >> 6 == 1, + position_reached: (value & 0b1000000) >> 5 == 1, + velocity_reached: (value & 0b100000) >> 4 == 1, + standstill: (value & 0b10000) >> 3 == 1, + sg2: (value & 0b1000) >> 2 == 1, + driver_error: (value & 0b10 >> 1) == 1, + reset_flag: (value & 0b1) == 1, + } + } + + pub fn to_val(&self) -> u8 { + let mut val = 0; + val |= (self.status_stop_r as u8) << 7; + val |= (self.status_stop_l as u8) << 7; + val |= (self.position_reached as u8) << 7; + val |= (self.velocity_reached as u8) << 7; + val |= (self.standstill as u8) << 7; + val |= (self.sg2 as u8) << 7; + val |= (self.driver_error as u8) << 7; + val |= (self.reset_flag as u8)<< 7; + val + } +} + /// Ramp Modes #[allow(dead_code)]