添加TMC51X0_MOTOR

components/step_motor_ctrl_module/tmc51x0_motor.cpp

@@ -647,9 +647,71 @@ int32_t TMC51X0Motor::motor_easy_move_to_zero(int32_t direction) {
     return err::koperation_not_support;
   }
 }
-int32_t TMC51X0Motor::motor_read_enc_val(int32_t* enc_val) {}
-int32_t TMC51X0Motor::motor_set_enc_resolution(int32_t enc_resolution) {}
-int32_t TMC51X0Motor::motor_get_enc_resolution(int32_t* enc_resolution) {}
+int32_t TMC51X0Motor::motor_set_enc_resolution(int32_t enc_resolution) {
+  /**
+   * @brief
+   *
+   *  TODO:
+   *      1.假设电机是256细分
+   *      2.假设TMC5130_ENC_CONST是十进制模式
+   *      3.只支持指定分辨率的编码器
+   */
+
+  int32_t enc_resolution_tmp   = enc_resolution * 4;
+  int32_t enc_const_integral   = 0;
+  int32_t enc_const_fractional = 0;
+
+  switch (abs(enc_resolution_tmp)) {
+    case 1000:
+      enc_const_integral   = 51;
+      enc_const_fractional = 2;
+      break;
+    case 1024:
+      enc_const_integral   = 50;
+      enc_const_fractional = 0;
+      break;
+    case 4000:
+      enc_const_integral   = 12;
+      enc_const_fractional = 8;
+      break;
+    case 4096:
+      enc_const_integral   = 12;
+      enc_const_fractional = 5;
+      break;
+    case 16384:
+      enc_const_integral   = 3;
+      enc_const_fractional = 125;
+      break;
+    default:
+      return err::kparam_out_of_range;
+      break;
+  }
+
+  if (enc_resolution_tmp < 0) {
+    enc_const_integral = -enc_const_integral;
+  }
+  m_motor_enc_resolution = enc_resolution;
+  int32_t setval         = enc_const_integral * (2 ^ 16) + enc_const_fractional;
+  m_stepM1->writeInt(TMC5130_ENC_CONST, setval);
+  return 0;
+}
+int32_t TMC51X0Motor::motor_get_enc_resolution(int32_t* enc_resolution) {
+  *enc_resolution = m_motor_enc_resolution;
+  return 0;
+}
+int32_t TMC51X0Motor::motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) {
+  m_stepM1->writeInt((uint8_t)reg_addr, reg_val);
+  return 0;
+}
+int32_t TMC51X0Motor::motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) {
+  *reg_val = m_stepM1->readInt((uint8_t)reg_addr);
+  return 0;
+}
+
+int32_t TMC51X0Motor::motor_read_enc_val(int32_t* enc_val) {
+  inverse_kinematics(m_stepM1->getENCVAL(), *enc_val);
+  return 0;
+}
 
 int32_t TMC51X0Motor::motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return _exec_move_to_io_task(ioindex, direction); }
 

components/step_motor_ctrl_module/tmc51x0_motor.hpp

@@ -72,9 +72,8 @@ class TMC51X0Motor : public ZIModule, public ZIMotor {
 
   int32_t     m_calculate_curpos_result = 0;
   const char* m_flashmark               = nullptr;
-  // action_cb_status_t m_status_cb;
 
-  // int32_t m_enable = 0;
+  int32_t m_motor_enc_resolution = 0;
 
  public:
   void            initialize(int moduleid, TMC5130* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg);
@@ -180,6 +179,9 @@ class TMC51X0Motor : public ZIModule, public ZIMotor {
   virtual int32_t motor_set_enc_resolution(int32_t enc_resolution);
   virtual int32_t motor_get_enc_resolution(int32_t* enc_resolution);
 
+  virtual int32_t motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val);
+  virtual int32_t motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val);
+
  private:
   int32_t _read_io();
 

components/tmc/ic/ztmc5130.cpp

@@ -140,8 +140,11 @@ uint8_t TMC5130::reset() {
   return 0;
 }
 int32_t TMC5130::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); }
+
 void    TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, to_motor_pos(value)); }
 int32_t TMC5130::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); }
+int32_t TMC5130::getENCVAL() { return to_user_pos(readInt(TMC5130_XENC)); }
+
 void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); }  // 设置最大加速度
 void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(to_motor_acc(accelerationpps2))); }  // 设置最大减速度
 void TMC5130::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); }

components/tmc/ic/ztmc5130.hpp

@@ -114,6 +114,7 @@ class TMC5130 : public IStepperMotor {
   virtual void    setXACTUAL(int32_t value);  // 设置电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
 
   virtual int32_t getVACTUAL();   // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
+  virtual int32_t getENCVAL();    //
   uint32_t        readXTARGET();  // 读取驱动器目标位置
 
   virtual int32_t getENC_POS() { return 0; }    // TODO impl it

components/zprotocols/zcancmder_v2/api/zi_motor.hpp

@@ -44,7 +44,6 @@ class ZIMotor {
   virtual int32_t motor_read_enc_val(int32_t* enc_val) { return err::koperation_not_support; }
   virtual int32_t motor_set_enc_resolution(int32_t enc_resolution) { return err::koperation_not_support; }
   virtual int32_t motor_get_enc_resolution(int32_t* enc_resolution) { return err::koperation_not_support; }
-
   virtual int32_t motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) { return err::koperation_not_support; }
   virtual int32_t motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) { return err::koperation_not_support; }
 

components/zprotocols/zcancmder_v2/protocol_parser.cpp

@@ -235,6 +235,13 @@ void ZIProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, in
   PROCESS_PACKET_10(kmotor_easy_set_current_pos, ZIMotor, motor_easy_set_current_pos);
   PROCESS_PACKET_20(kmotor_easy_move_to_io, ZIMotor, motor_easy_move_to_io);
 
+  PROCESS_PACKET_01(kmotor_read_enc_val, ZIMotor, motor_read_enc_val);
+  PROCESS_PACKET_10(kmotor_set_enc_resolution, ZIMotor, motor_set_enc_resolution);
+  PROCESS_PACKET_01(kmotor_get_enc_resolution, ZIMotor, motor_get_enc_resolution);
+  PROCESS_PACKET_20(kmotor_set_subdevice_reg, ZIMotor, motor_set_subdevice_reg);
+  PROCESS_PACKET_11(kmotor_get_subdevice_reg, ZIMotor, motor_get_subdevice_reg);
+
+
   /*******************************************************************************
    *                                   xymotor                                  *
    *******************************************************************************/