fix vactual and xtarget readout

hacknus · Apr 23, 2024 · 57d3cfd · 57d3cfd
1 parent 0c2670e
commit 57d3cfd
Showing 1 changed file with 9 additions and 8 deletions.
diff --git a/src/lib.rs b/src/lib.rs
@@ -610,13 +610,14 @@ impl<SPI, CS, EN, E> Tmc5160<SPI, CS, EN>
 
     /// get the current velocity
     pub fn get_velocity(&mut self) -> Result<f32, Error<E>> {
-        self.read_register(Registers::VACTUAL).map(|target| {
-            if (target.data & 0b100000000000000000000000) == 0b100000000000000000000000 {
-                ((16777216 - target.data as i32) as f64 / self._step_count as f64) as f32
-            } else {
-                ((target.data as i32) as f64 / self._step_count as f64) as f32
-            }
-        })
+        // self.read_register(Registers::VACTUAL).map(|target| {
+        //     if (target.data & 0b100000000000000000000000) == 0b100000000000000000000000 {
+        //         ((16777216 - target.data as i32) as f64 / self._step_count as f64) as f32
+        //     } else {
+        //         ((target.data as i32) as f64 / self._step_count as f64) as f32
+        //     }
+        // })
+        self.read_register(Registers::VACTUAL).map(|val| (val.data as i32) as f32 / self._step_count)
     }
 
     /// get the set maximum velocity (VMAX)
@@ -626,6 +627,6 @@ impl<SPI, CS, EN, E> Tmc5160<SPI, CS, EN>
 
     /// get the current target position (XTARGET)
     pub fn get_target(&mut self) -> Result<f32, Error<E>> {
-        self.read_register(Registers::XTARGET).map(|packet| packet.data as f32 / self._step_count)
+        self.read_register(Registers::XTARGET).map(|val| (val.data as i32) as f32 / self._step_count)
     }
 }