From 9f2fd2e2c684b6a7c09bfa5c6f5ae38a44e21d1b Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Sun, 15 Oct 2023 13:41:08 +0800
Subject: [PATCH] update

---
 .../step_motor_ctrl_module/step_motor_ctrl_module.cpp       |  2 +-
 components/tmc/basic/tmc_ic_interface.hpp                   |  2 +-
 components/tmc/ic/ztmc4361A.cpp                             |  4 ++--
 components/tmc/ic/ztmc5130.cpp                              | 13 +++++++------
 components/tmc/ic/ztmc5130.hpp                              |  4 ++--
 5 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
index de8f138..df01cb3 100644
--- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
+++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
@@ -613,5 +613,5 @@ int32_t StepMotorCtrlModule::factory_reset() {
 }
 void StepMotorCtrlModule::active_cfg() {
   m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay);
-  m_stepM1->setScale(m_param.distance_scale / 1000.0);
+  m_stepM1->setScale(m_param.distance_scale);
 }
diff --git a/components/tmc/basic/tmc_ic_interface.hpp b/components/tmc/basic/tmc_ic_interface.hpp
index bc738e0..69b446d 100644
--- a/components/tmc/basic/tmc_ic_interface.hpp
+++ b/components/tmc/basic/tmc_ic_interface.hpp
@@ -58,7 +58,7 @@ class IStepperMotor {
   virtual bool isStoped()      = 0;
 
   virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0;
-  virtual void setScale(float scale)                                   = 0;
+  virtual void setScale(int32_t scale)                                   = 0;
 
   virtual void enable(bool enable) = 0;
 };
diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp
index 438ac57..b0d69ab 100644
--- a/components/tmc/ic/ztmc4361A.cpp
+++ b/components/tmc/ic/ztmc4361A.cpp
@@ -406,11 +406,11 @@ int32_t TMC4361A::to_motor_pos(int32_t pos) {  //
 
 int32_t TMC4361A::to_user_pos(int32_t pos) {  //
   int32_t val = pos / 51200.0 * m_scale;
-  return 0;
+  return val;
 }  //
 int32_t TMC4361A::to_user_vel(int32_t vel) {  //
   int32_t val = vel * 60.0 / 51200.0;
-  return 0;
+  return val;
 }
 
 #endif
diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp
index 3c5d3d4..5a136d3 100644
--- a/components/tmc/ic/ztmc5130.cpp
+++ b/components/tmc/ic/ztmc5130.cpp
@@ -116,7 +116,7 @@ Tmc5130RampStat TMC5130::getTMC5130_RAMPSTAT2() {
 void TMC5130::stop() { rotate(0); }
 void TMC5130::rotate(int32_t velocity) {
   // velocity *= m_scale;
-  to_motor_vel(velocity);
+  velocity = to_motor_vel(velocity);
   writeInt(TMC5130_VMAX, abs(velocity));
   writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG);
 }
@@ -125,8 +125,9 @@ void TMC5130::left(int32_t velocity) { rotate(-velocity); }
 void TMC5130::moveTo(int32_t position, uint32_t velocityMax) {
   // position *= m_scale;
   // velocityMax *= m_scale;
-  to_motor_pos(position);
-  to_motor_vel(velocityMax);
+  position    = to_motor_pos(position);
+  velocityMax = to_motor_vel(velocityMax);
+  // ZLOGI("TMC5130", "moveTo %d %d", position, velocityMax);
   writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
   writeInt(TMC5130_VMAX, velocityMax);
   writeInt(TMC5130_XTARGET, position);
@@ -159,7 +160,7 @@ bool TMC5130::isReachTarget() {
   }
 }
 
-void TMC5130::setScale(float scale) { m_scale = scale; }
+void TMC5130::setScale(int32_t scale) { m_scale = scale; }
 
 /*******************************************************************************
  *                                    basic                                    *
@@ -225,9 +226,9 @@ int32_t TMC5130::to_motor_pos(int32_t pos) {  //
 
 int32_t TMC5130::to_user_pos(int32_t pos) {  //
   int32_t val = pos / 51200.0 * m_scale;
-  return 0;
+  return val;
 }  //
 int32_t TMC5130::to_user_vel(int32_t vel) {  //
   int32_t val = vel * 60.0 / 51200.0;
-  return 0;
+  return val;
 }
\ No newline at end of file
diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp
index 1770d57..92a0995 100644
--- a/components/tmc/ic/ztmc5130.hpp
+++ b/components/tmc/ic/ztmc5130.hpp
@@ -88,7 +88,7 @@ class TMC5130 : public IStepperMotor {
   const uint8_t *m_registerAccessTable;
   const int32_t *m_defaultRegisterResetState;
 
-  float m_scale = 1.0;
+  int32_t m_scale = 10000;
 
  public:
   TMC5130(/* args */);
@@ -115,7 +115,7 @@ class TMC5130 : public IStepperMotor {
   virtual int32_t getENC_POS() { return 0; }    // TODO impl it
   virtual void    setENC_POS(int32_t value) {}  // TODO impl it
 
-  virtual void setScale(float scale);
+  virtual void setScale(int32_t scale);
 
   virtual void setAcceleration(float accelerationpps2);
   virtual void setDeceleration(float accelerationpps2);