update

components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -4,7 +4,6 @@
 #include <string.h>
 
 #include "a8000_protocol\protocol.hpp"
-#include "sdk/components/tmc/ic/ztmc4361A.hpp"
 #include "sdk\components\flash\znvs.hpp"
 #include "sdk\components\tmc\ic\ztmc5130.hpp"
 using namespace iflytop;
@@ -30,19 +29,6 @@ void StepMotorCtrlModule::initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable
   if (tmcmotor) {
     tmcmotor->getGState();  // 读取状态，清空下复位标识
   }
-
-  TMC4361A* tmc4361motor = dynamic_cast<TMC4361A*>(m_stepM1);
-  if (tmc4361motor) {
-    auto tmc4361state  = tmc4361motor->getTMC4361AMotorEventState();
-    auto tmc4361status = tmc4361motor->getTMC4361AMotorStatus();
-    auto tmc2160Status = tmc4361motor->getTMC2160MotorDriverStatus();
-    auto tmc2160State  = tmc4361motor->getTMC2160MotorGstate();
-
-    dump(tmc4361state);
-    dump(tmc4361status);
-    dump(tmc2160Status);
-    dump(tmc2160State);
-  }
 }
 void StepMotorCtrlModule::create_default_cfg(config_t& cfg) {
   memset(&cfg, 0, sizeof(cfg));
@@ -62,12 +48,42 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) {
 int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) {
   int32_t suc = pri_module_xxx_reg(param_id, read, val);
   if (suc != 0) return suc;
+  if (!read) {
+    switch (param_id) {
+      case kreg_step_motor_shaft:
+      case kreg_step_motor_one_circle_pulse:
+      case kreg_step_motor_one_circle_pulse_denominator:
+      case kreg_step_motor_ihold:
+      case kreg_step_motor_irun:
+      case kreg_step_motor_iholddelay:
+      case kreg_step_motor_max_d:
+      case kreg_step_motor_min_d:
+      case kreg_step_motor_iglobalscaler:
+      case kreg_step_motor_in_debug_mode:
+      case kreg_step_motor_vstart:
+      case kreg_step_motor_a1:
+      case kreg_step_motor_amax:
+      case kreg_step_motor_v1:
+      case kreg_step_motor_dmax:
+      case kreg_step_motor_d1:
+      case kreg_step_motor_vstop:
+      case kreg_step_motor_tzerowait:
+      case kreg_step_motor_enc_resolution:
+        step_motor_active_cfg();
+        break;
+      default:
+        break;
+    }
+  }
   return suc;
 }
 int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val) {
   switch (param_id) {
     MODULE_COMMON_PROCESS_REG_CB();
     PROCESS_REG(kreg_step_motor_pos, inter_get_pos(val), ACTION_NONE);
+    PROCESS_REG(kreg_step_motor_enc_val, read_enc_val(val), write_enc_val(val));
+    PROCESS_REG(kreg_step_motor_denc_val, REG_GET(m_state.denc), ACTION_NONE);
+
     PROCESS_REG(kreg_step_motor_dpos, REG_GET(m_state.dpos), ACTION_NONE);
     PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_cfg.motor_shaft), REG_SET(m_cfg.motor_shaft));
     PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_cfg.motor_one_circle_pulse), REG_SET(m_cfg.motor_one_circle_pulse));
@@ -91,6 +107,8 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
     PROCESS_REG(kreg_step_motor_d1, REG_GET(m_cfg.motor_d1), REG_SET(m_cfg.motor_d1));
     PROCESS_REG(kreg_step_motor_vstop, REG_GET(m_cfg.motor_vstop), REG_SET(m_cfg.motor_vstop));
     PROCESS_REG(kreg_step_motor_tzerowait, REG_GET(m_cfg.motor_tzerowait), REG_SET(m_cfg.motor_tzerowait));
+    PROCESS_REG(kreg_step_motor_enc_resolution, REG_GET(m_cfg.motor_enc_resolution), REG_SET(m_cfg.motor_enc_resolution));
+    PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc_resolution), REG_SET(m_cfg.motor_enable_enc_resolution));
 
     default:
       return err::kmodule_not_find_reg;
@@ -121,6 +139,7 @@ int32_t StepMotorCtrlModule::step_motor_easy_set_current_pos(int32_t pos) {
   int32_t motor_pos = 0;
   inter_forward_kinematics(pos, motor_pos);
   m_stepM1->setXACTUAL(motor_pos);
+  m_stepM1->set_enc_val(motor_pos);
   return 0;
 }
 
@@ -141,6 +160,7 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() {
   m_stepM1->set_d1(m_cfg.motor_d1);
   m_stepM1->set_vstop(m_cfg.motor_vstop);
   m_stepM1->set_tzerowait(m_cfg.motor_tzerowait);
+  m_stepM1->set_enc_resolution(m_cfg.motor_enc_resolution);
 
   // stepmotor_iglobalscaler
   if (m_state.enable) {
@@ -177,38 +197,23 @@ int32_t StepMotorCtrlModule::step_motor_read_tmc5130_state(int32_t* status) {
   }
   return 0;
 };
-int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_status(int32_t* status) {
-  TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
-  if (tmc4361) {
-    auto devStatus = tmc4361->getTMC4361AMotorStatus();
-    memcpy(status, &devStatus, sizeof(int32_t));
-  }
-  return 0;
-};
-int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_state(int32_t* gstate) {
-  TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
-  if (tmc4361) {
-    auto state = tmc4361->getTMC4361AMotorEventState();
-    memcpy(gstate, &state, sizeof(int32_t));
-  }
-  return 0;
-};
-int32_t StepMotorCtrlModule::step_motor_read_tmc2160_status(int32_t* status) {
-  TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
-  if (tmc4361) {
-    auto devStatus = tmc4361->getTMC2160MotorDriverStatus();
-    memcpy(status, &devStatus, sizeof(int32_t));
+
+int32_t StepMotorCtrlModule::step_motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) {
+  TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
+  if (tmc5130) {
+    tmc5130->writeInt(reg_addr, reg_val);
+    return 0;
   }
-  return 0;
-};
-int32_t StepMotorCtrlModule::step_motor_read_tmc2160_state(int32_t* status) {
-  TMC4361A* tmc4361 = dynamic_cast<TMC4361A*>(m_stepM1);
-  if (tmc4361) {
-    auto state = tmc4361->getTMC2160MotorGstate();
-    memcpy(status, &state, sizeof(int32_t));
+  return err::kcmd_not_support;
+}
+int32_t StepMotorCtrlModule::step_motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) {
+  TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
+  if (tmc5130) {
+    *reg_val = tmc5130->readInt(reg_addr);
+    return 0;
   }
-  return 0;
-};
+  return err::kcmd_not_support;
+}
 
 int32_t StepMotorCtrlModule::step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) {
   if (ioindex < 0 || ioindex >= m_nio) {
@@ -235,15 +240,18 @@ int StepMotorCtrlModule::inter_get_pos(int32_t& x) {
 }
 void StepMotorCtrlModule::befor_motor_move() {  //
   m_state.before_move_pos = m_stepM1->getXACTUAL();
-  creg.module_status    = 1;
+  m_state.before_move_enc = m_stepM1->read_enc_val();
+  creg.module_status      = 1;
   creg.module_errorcode   = 0;
 
-  step_motor_active_cfg();
+  //
   m_stepM1->enable(1);
 }
 void StepMotorCtrlModule::after_motor_move() {
   m_state.after_move_pos = m_stepM1->getXACTUAL();
+  m_state.after_move_enc = m_stepM1->read_enc_val();
   m_state.dpos           = m_state.after_move_pos - m_state.before_move_pos;
+  m_state.denc           = m_state.after_move_enc - m_state.before_move_enc;
   if (creg.module_errorcode != 0) {
     creg.module_status = 2;
   }
@@ -264,109 +272,28 @@ void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) {
   ZLOGI(TAG, "olb:                 %d", status->olb);
   ZLOGI(TAG, "stst:                %d", status->stst);
 }
-void StepMotorCtrlModule::dump(TMC2160MotorDriverStatus_t val) {
-  ZLOGI(TAG, "------------ dump TMC2160MotorDriverStatus_t ---------------");
-  ZLOGI(TAG, "sg_result:           %d", val.sg_result);
-  ZLOGI(TAG, "s2vsa:               %d", val.s2vsa);
-  ZLOGI(TAG, "s2vsb:               %d", val.s2vsb);
-  ZLOGI(TAG, "stealth:             %d", val.stealth);
-  ZLOGI(TAG, "fsactive:            %d", val.fsactive);
-  ZLOGI(TAG, "cs_actual:           %d", val.cs_actual);
-  ZLOGI(TAG, "reserved2:           %d", val.reserved2);
-  ZLOGI(TAG, "stallguard:          %d", val.stallguard);
-  ZLOGI(TAG, "ot:                  %d", val.ot);
-  ZLOGI(TAG, "otpw:                %d", val.otpw);
-  ZLOGI(TAG, "s2ga:                %d", val.s2ga);
-  ZLOGI(TAG, "s2gb:                %d", val.s2gb);
-  ZLOGI(TAG, "ola:                 %d", val.ola);
-  ZLOGI(TAG, "olb:                 %d", val.olb);
-  ZLOGI(TAG, "stst:                %d", val.stst);
-}
-void StepMotorCtrlModule::dump(TMC4361AMotorStatus_t val) {
-  ZLOGI(TAG, "------------ dump TMC4361AMotorStatus_t ---------------");
-
-  ZLOGI(TAG, "TARGET_REACHED_F:    %d", val.TARGET_REACHED_F);
-  ZLOGI(TAG, "POS_COMP_REACHED_F:  %d", val.POS_COMP_REACHED_F);
-  ZLOGI(TAG, "VEL_REACHED_F:       %d", val.VEL_REACHED_F);
-  ZLOGI(TAG, "VEL_STATE_F:         %d", val.VEL_STATE_F);
-  ZLOGI(TAG, "RAMP_STATE_F:        %d", val.RAMP_STATE_F);
-  ZLOGI(TAG, "STOPL_ACTIVE_F:      %d", val.STOPL_ACTIVE_F);
-  ZLOGI(TAG, "STOPR_ACTIVE_F:      %d", val.STOPR_ACTIVE_F);
-  ZLOGI(TAG, "VSTOPL_ACTIVE_F:     %d", val.VSTOPL_ACTIVE_F);
-  ZLOGI(TAG, "VSTOPR_ACTIVE_F:     %d", val.VSTOPR_ACTIVE_F);
-  ZLOGI(TAG, "ACTIVE_STALL_F:      %d", val.ACTIVE_STALL_F);
-  ZLOGI(TAG, "HOME_ERROR_F:        %d", val.HOME_ERROR_F);
-  ZLOGI(TAG, "FS_ACTIVE_F:         %d", val.FS_ACTIVE_F);
-  ZLOGI(TAG, "ENC_FAIL_F:          %d", val.ENC_FAIL_F);
-  ZLOGI(TAG, "N_ACTIVE_F:          %d", val.N_ACTIVE_F);
-  ZLOGI(TAG, "ENC_LATCH_F:         %d", val.ENC_LATCH_F);
-  ZLOGI(TAG, "MULTI_CYCLE_FAIL_F:  %d", val.MULTI_CYCLE_FAIL_F);
-  ZLOGI(TAG, "___:                 %d", val.___);
-  ZLOGI(TAG, "CL_FIT_F:            %d", val.CL_FIT_F);
-  ZLOGI(TAG, "SERIAL_ENC_FLAGS:    %d", val.SERIAL_ENC_FLAGS);
-  ZLOGI(TAG, "SG:                  %d", val.SG);
-  ZLOGI(TAG, "OT:                  %d", val.OT);
-  ZLOGI(TAG, "OTPW:                %d", val.OTPW);
-  ZLOGI(TAG, "S2GA:                %d", val.S2GA);
-  ZLOGI(TAG, "S2GB:                %d", val.S2GB);
-  ZLOGI(TAG, "OLA:                 %d", val.OLA);
-  ZLOGI(TAG, "OLB:                 %d", val.OLB);
-  ZLOGI(TAG, "STST:                %d", val.STST);
-}
-void StepMotorCtrlModule::dump(TMC2160MotorGstate_t val) {
-  ZLOGI(TAG, "------------ dump TMC2160MotorGstate_t ---------------");
-
-  ZLOGI(TAG, "reset:               %d", val.reset);
-  ZLOGI(TAG, "drv_err:             %d", val.drv_err);
-  ZLOGI(TAG, "uv_cp:               %d", val.uv_cp);
-}
-void StepMotorCtrlModule::dump(TMC4361AMotorEventState_t val) {
-  ZLOGI(TAG, "------------ dump TMC4361AMotorEventState_t ---------------");
-
-  ZLOGI(TAG, "TARGET_REACHED :              %d", val.TARGET_REACHED);
-  ZLOGI(TAG, "POS_COMP_REACHED :            %d", val.POS_COMP_REACHED);
-  ZLOGI(TAG, "VEL_REACHED :                 %d", val.VEL_REACHED);
-  ZLOGI(TAG, "VEL_STATE_EQ_0 :              %d", val.VEL_STATE_EQ_0);
-  ZLOGI(TAG, "VEL_STATE_EQ_1 :              %d", val.VEL_STATE_EQ_1);
-  ZLOGI(TAG, "VEL_STATE_EQ_2 :              %d", val.VEL_STATE_EQ_2);
-  ZLOGI(TAG, "RAMP_STATE_EQ_0 :             %d", val.RAMP_STATE_EQ_0);
-  ZLOGI(TAG, "RAMP_STATE_EQ_1 :             %d", val.RAMP_STATE_EQ_1);
-  ZLOGI(TAG, "RAMP_STATE_EQ_2 :             %d", val.RAMP_STATE_EQ_2);
-  ZLOGI(TAG, "MAX_PHASE_TRAP :              %d", val.MAX_PHASE_TRAP);
-  ZLOGI(TAG, "FROZEN :                      %d", val.FROZEN);
-  ZLOGI(TAG, "STOPL :                       %d", val.STOPL);
-  ZLOGI(TAG, "STOPR :                       %d", val.STOPR);
-  ZLOGI(TAG, "VSTOPL_ACTIVE :               %d", val.VSTOPL_ACTIVE);
-  ZLOGI(TAG, "VSTOPR_ACTIVE :               %d", val.VSTOPR_ACTIVE);
-  ZLOGI(TAG, "HOME_ERROR :                  %d", val.HOME_ERROR);
-  ZLOGI(TAG, "XLATCH_DONE :                 %d", val.XLATCH_DONE);
-  ZLOGI(TAG, "FS_ACTIVE :                   %d", val.FS_ACTIVE);
-  ZLOGI(TAG, "ENC_FAIL :                    %d", val.ENC_FAIL);
-  ZLOGI(TAG, "N_ACTIVE :                    %d", val.N_ACTIVE);
-  ZLOGI(TAG, "ENC_DONE :                    %d", val.ENC_DONE);
-  ZLOGI(TAG, "SER_ENC_DATA_FAIL :           %d", val.SER_ENC_DATA_FAIL);
-  ZLOGI(TAG, "___ :                         %d", val.___);
-  ZLOGI(TAG, "SER_DATA_DONE :               %d", val.SER_DATA_DONE);
-  ZLOGI(TAG, "SERIAL_ENC_Flags :            %d", val.SERIAL_ENC_Flags);
-  ZLOGI(TAG, "COVER_DONE :                  %d", val.COVER_DONE);
-  ZLOGI(TAG, "ENC_VEL0 :                    %d", val.ENC_VEL0);
-  ZLOGI(TAG, "CL_MAX :                      %d", val.CL_MAX);
-  ZLOGI(TAG, "CL_FIT :                      %d", val.CL_FIT);
-  ZLOGI(TAG, "STOP_ON_STALL :               %d", val.STOP_ON_STALL);
-  ZLOGI(TAG, "MOTOR_EV :                    %d", val.MOTOR_EV);
-  ZLOGI(TAG, "RST_EV :                      %d", val.RST_EV);
-}
-
-void StepMotorCtrlModule::setErrorFlag(int32_t ecode) {
-  creg.module_errorcode = ecode;
-}
 
-bool StepMotorCtrlModule::check_when_run() {
-  //
+void StepMotorCtrlModule::setErrorFlag(int32_t ecode) { creg.module_errorcode = ecode; }
 
-  if (m_state.debugmode) {
-    return true;
+int32_t StepMotorCtrlModule::read_enc_val(int32_t& enc_val) {
+  auto tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
+  if (tmc5130) {
+    tmc5130->read_enc_val(enc_val);
+    return 0;
+  }
+  return err::kcmd_not_support;
+}
+int32_t StepMotorCtrlModule::write_enc_val(int32_t enc_val) {
+  auto tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
+  if (tmc5130) {
+    tmc5130->set_enc_val(enc_val);
+    return 0;
   }
+  return err::kcmd_not_support;
+}
+bool StepMotorCtrlModule::check_when_run() {
+  //
+  if (m_state.debugmode) return true;
 
   TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
   if (tmc5130 && tmc5130->isTMC5130()) {
@@ -671,6 +598,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_zero() {
         exec_move_to_io_task(0, -1);
         after_motor_move();
         m_stepM1->setXACTUAL(0);
+        m_stepM1->set_enc_val(0);
       },
       [this]() { m_stepM1->stop(); });
   return 0;

components/step_motor_ctrl_module/step_motor_ctrl_module.hpp

@@ -30,12 +30,17 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
     int32_t motor_d1;
     int32_t motor_vstop;
     int32_t motor_tzerowait;
+    int32_t motor_enc_resolution;
+    int32_t motor_enable_enc_resolution;
   } config_t;
 
   typedef struct {
     int32_t dpos;
+    int32_t denc;
     int32_t after_move_pos;
     int32_t before_move_pos;
+    int32_t after_move_enc;
+    int32_t before_move_enc;
     int32_t enable;
     int32_t debugmode;
   } state_t;
@@ -55,7 +60,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   void initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable[], int nio, config_t* cfg);
 
   static void create_default_cfg(config_t& cfg);
-  config_t* get_cfg() { return &m_cfg; }
+  config_t*   get_cfg() { return &m_cfg; }
 
   /***********************************************************************************************************************
    *                                                      ZIModule                                                       *
@@ -89,10 +94,12 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
 
   virtual int32_t step_motor_read_tmc5130_status(int32_t* errorflag) override;
   virtual int32_t step_motor_read_tmc5130_state(int32_t* status) override;
-  virtual int32_t step_motor_read_tmc4361a_status(int32_t* status) override;
-  virtual int32_t step_motor_read_tmc4361a_state(int32_t* gstate) override;
-  virtual int32_t step_motor_read_tmc2160_status(int32_t* status) override;
-  virtual int32_t step_motor_read_tmc2160_state(int32_t* status) override;
+
+  // virtual int32_t step_motor_read_enc_val(int32_t* enc_val) override;
+  // virtual int32_t step_motor_set_enc_resolution(int32_t enc_resolution) override;
+  // virtual int32_t step_motor_get_enc_resolution(int32_t* enc_resolution) override;
+  virtual int32_t step_motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) override;
+  virtual int32_t step_motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) override;
 
   virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) override;
 
@@ -132,9 +139,9 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status);
   void setErrorFlag(int32_t ecode);
 
-  void dump(TMC2160MotorDriverStatus_t val);
-  void dump(TMC4361AMotorStatus_t val);
-  void dump(TMC2160MotorGstate_t val);
-  void dump(TMC4361AMotorEventState_t val);
+  int32_t read_enc_val(int32_t& enc_val);
+  int32_t write_enc_val(int32_t enc_val);
+
+  // virtual int32_t step_motor_set_enc_resolution(int32_t enc_resolution);
 };
 }  // namespace iflytop

components/tmc/ic/ztmc5130.cpp

@@ -327,4 +327,79 @@ void TMC51X0::readMotorState(bool *stoped, int32_t *error) {
   *stoped = isReachTarget();
 }
 
+void TMC51X0::read_enc_val(int32_t &enc_val) {
+  enc_val = to_user_pos(readInt(TMC5130_XENC));
+  if (m_enc_resolution < 0) {
+    enc_val = -enc_val;
+  }
+}
+int32_t TMC51X0::read_enc_val() {
+  int32_t enc_val;
+  read_enc_val(enc_val);
+  return enc_val;
+}
+
+void TMC51X0::set_enc_val(int32_t enc_val) {
+  int32_t enc_set_val = to_motor_pos(enc_val);
+  if (m_enc_resolution < 0) {
+    enc_set_val = -enc_set_val;
+  }
+  writeInt(TMC5130_XENC, enc_set_val);
+}
+bool TMC51X0::set_enc_resolution(int32_t enc_resolution) {
+  /**
+   * @brief
+   *
+   *      1.假设电机是256细分
+   *      2.假设TMC5130_ENC_CONST是十进制模式
+   *      3.只支持指定分辨率的编码器
+   */
+
+  int32_t enc_resolution_tmp   = enc_resolution * 4;
+  int16_t enc_const_integral   = 0;
+  int16_t enc_const_fractional = 0;
+
+  switch (abs(enc_resolution_tmp)) {
+    case 1000:
+      enc_const_integral   = 51;
+      enc_const_fractional = 0.2 * 10000;
+      break;
+    case 1024:
+      enc_const_integral   = 50;
+      enc_const_fractional = 0 * 10000;
+      break;
+    case 4000:
+      enc_const_integral   = 12;
+      enc_const_fractional = 0.8 * 10000;
+      break;
+    case 4096:
+      enc_const_integral   = 12;
+      enc_const_fractional = 0.5 * 10000;
+      break;
+    case 16384:
+      enc_const_integral   = 3;
+      enc_const_fractional = 0.125 * 10000;
+      break;
+    case 0:
+      m_enc_resolution = 0;
+      return true;
+
+    default:
+      return false;
+      break;
+  }
+
+  m_enc_resolution = enc_resolution;
+  uint32_t setval  = 0;
+  uint8_t *psetval = (uint8_t *)&setval;
+
+  memcpy(psetval + 2, &enc_const_integral, 2);
+  memcpy(psetval, &enc_const_fractional, 2);
+
+  writeInt(TMC5130_ENCMODE, 0x1 << 10);
+  writeInt(TMC5130_ENC_CONST, setval);
+  return true;
+}
+void TMC51X0::get_enc_resolution(int32_t *enc_resolution) { *enc_resolution = m_enc_resolution; }
+
 #endif

components/tmc/ic/ztmc5130.hpp

@@ -79,17 +79,17 @@ class TMC51X0 : public IStepperMotor {
 
   typedef struct {
     SPI_HandleTypeDef *spi;
-    Pin_t              csgpio          = PinNull;  //
-    Pin_t              ennPin          = PinNull;  //
+    Pin_t              csgpio = PinNull;  //
+    Pin_t              ennPin = PinNull;  //
 
   } cfg_t;
 
  protected:
   cfg_t m_cfg;
 
-  ZGPIO             *m_csnpin               = NULL;
-  ZGPIO             *m_ennpin               = NULL;
-  SPI_HandleTypeDef *m_hspi                 = NULL;
+  ZGPIO             *m_csnpin = NULL;
+  ZGPIO             *m_ennpin = NULL;
+  SPI_HandleTypeDef *m_hspi   = NULL;
 
   // uint8_t  m_channel;
   uint32_t m_lastCallPeriodicJobTick;
@@ -104,6 +104,8 @@ class TMC51X0 : public IStepperMotor {
   mres_type_t m_MRES           = kmres_256;
   double      m_onecirclepulse = 51200;
 
+  int32_t m_enc_resolution = 0;  // 编码器分辨率，默认只有在256细分的情况下有效
+
  public:
   TMC51X0(/* args */);
 
@@ -130,6 +132,14 @@ class TMC51X0 : public IStepperMotor {
   virtual void setScale(int32_t scale);
   virtual void setScaleDenominator(int32_t scale);
 
+  virtual void read_enc_val(int32_t &enc_val);
+  virtual void set_enc_val(int32_t enc_val);
+  virtual int32_t read_enc_val();
+
+
+  virtual bool set_enc_resolution(int32_t enc_resolution);
+  virtual void get_enc_resolution(int32_t *enc_resolution);
+
   virtual bool isReachTarget();  // 是否到达目标位置
   virtual bool isStoped() { return isReachTarget(); }
   virtual void setNoAccLimit(bool enable) override;

components/zcancmder/zcan_protocol_parser.cpp

@@ -38,11 +38,9 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
 
   REGFN(step_motor_read_tmc5130_status);
   REGFN(step_motor_read_tmc5130_state);
-  REGFN(step_motor_read_tmc4361a_status);
-  REGFN(step_motor_read_tmc4361a_state);
-  REGFN(step_motor_read_tmc2160_status);
-  REGFN(step_motor_read_tmc2160_state);
   REGFN(step_motor_read_io_index_in_stm32);
+  REGFN(step_motor_set_subdevice_reg);
+  REGFN(step_motor_get_subdevice_reg);
 
   REGFN(mini_servo_enable);
   REGFN(mini_servo_read_pos);
@@ -318,35 +316,23 @@ int32_t ZCanProtocolParser::step_motor_read_tmc5130_state(cmdcontxt_t* cxt) {
   cxt->acklen  = 4;
   return module->step_motor_read_tmc5130_state(&ack[0]);
 }
-int32_t ZCanProtocolParser::step_motor_read_tmc4361a_status(cmdcontxt_t* cxt) {
-  CHECK_AND_GET_MODULE(0);
-  int32_t* ack = (int32_t*)cxt->ackbuf;
-  cxt->acklen  = 4;
-  return module->step_motor_read_tmc4361a_status(&ack[0]);
-}
-int32_t ZCanProtocolParser::step_motor_read_tmc4361a_state(cmdcontxt_t* cxt) {
-  CHECK_AND_GET_MODULE(0);
-  int32_t* ack = (int32_t*)cxt->ackbuf;
-  cxt->acklen  = 4;
-  return module->step_motor_read_tmc4361a_state(&ack[0]);
-}
-int32_t ZCanProtocolParser::step_motor_read_tmc2160_status(cmdcontxt_t* cxt) {
-  CHECK_AND_GET_MODULE(0);
+int32_t ZCanProtocolParser::step_motor_read_io_index_in_stm32(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(1);
   int32_t* ack = (int32_t*)cxt->ackbuf;
   cxt->acklen  = 4;
-  return module->step_motor_read_tmc2160_status(&ack[0]);
+  return module->step_motor_read_io_index_in_stm32(cxt->params[0], ack);
 }
-int32_t ZCanProtocolParser::step_motor_read_tmc2160_state(cmdcontxt_t* cxt) {
-  CHECK_AND_GET_MODULE(0);
-  int32_t* ack = (int32_t*)cxt->ackbuf;
-  cxt->acklen  = 4;
-  return module->step_motor_read_tmc2160_state(&ack[0]);
+
+int32_t ZCanProtocolParser::step_motor_set_subdevice_reg(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(2);
+  return module->step_motor_set_subdevice_reg(cxt->params[0], cxt->params[1]);
 }
-int32_t ZCanProtocolParser::step_motor_read_io_index_in_stm32(cmdcontxt_t* cxt) {
+
+int32_t ZCanProtocolParser::step_motor_get_subdevice_reg(cmdcontxt_t* cxt) {
   CHECK_AND_GET_MODULE(1);
   int32_t* ack = (int32_t*)cxt->ackbuf;
   cxt->acklen  = 4;
-  return module->step_motor_read_io_index_in_stm32(cxt->params[0], ack);
+  return module->step_motor_get_subdevice_reg(cxt->params[0], ack);
 }
 
 #undef MODULE_CLASS

components/zcancmder/zcan_protocol_parser.hpp

@@ -74,11 +74,9 @@ class ZCanProtocolParser : public IZCanReceiverListener {
   CMDFN(step_motor_easy_move_to_end_point);
   CMDFN(step_motor_read_tmc5130_status);
   CMDFN(step_motor_read_tmc5130_state);
-  CMDFN(step_motor_read_tmc4361a_status);
-  CMDFN(step_motor_read_tmc4361a_state);
-  CMDFN(step_motor_read_tmc2160_status);
-  CMDFN(step_motor_read_tmc2160_state);
   CMDFN(step_motor_read_io_index_in_stm32);
+  CMDFN(step_motor_set_subdevice_reg);
+  CMDFN(step_motor_get_subdevice_reg);
 
   CMDFN(mini_servo_enable);
   CMDFN(mini_servo_read_pos);
@@ -155,16 +153,14 @@ class ZCanProtocolParser : public IZCanReceiverListener {
 
   CMDFN(a8000_idcard_reader_read_raw);
 
-
-CMDFN(plate_code_scaner_push_card_and_scan);
-CMDFN(plate_code_scaner_stop_scan);
-CMDFN(plate_code_scaner_read_result);
-CMDFN(plate_code_scaner_read_result_point_num);
-CMDFN(plate_code_scaner_read_code);
-CMDFN(plate_code_scaner_adc_readraw);
-CMDFN(plate_code_scaner_open_laser);
-CMDFN(plate_code_scaner_close_laser);
-
+  CMDFN(plate_code_scaner_push_card_and_scan);
+  CMDFN(plate_code_scaner_stop_scan);
+  CMDFN(plate_code_scaner_read_result);
+  CMDFN(plate_code_scaner_read_result_point_num);
+  CMDFN(plate_code_scaner_read_code);
+  CMDFN(plate_code_scaner_adc_readraw);
+  CMDFN(plate_code_scaner_open_laser);
+  CMDFN(plate_code_scaner_close_laser);
 };
 
 }  // namespace iflytop