update

components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -531,6 +531,15 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_end_point() {
   }
   return do_step_motor_easy_move_to_end_point();
 }
+int32_t StepMotorCtrlModule::step_motor_read_pos_and_enc_pos(int32_t* pos, int32_t* encpos) {
+  *pos = m_stepM1->getXACTUAL();
+  if (m_cfg.motor_enable_enc_resolution != 0) {
+    *encpos = m_stepM1->read_enc_val();
+  } else {
+    *encpos = *pos;
+  }
+  return 0;
+}
 
 int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) {
   m_thread.stop();

components/step_motor_ctrl_module/step_motor_ctrl_module.hpp

@@ -87,6 +87,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
   virtual int32_t step_motor_easy_move_to_zero() override;
   virtual int32_t step_motor_easy_move_to_io(int32_t ioindex, int32_t direction) override;
   virtual int32_t step_motor_easy_move_to_end_point() override;
+  virtual int32_t step_motor_read_pos_and_enc_pos(int32_t* pos, int32_t* encpos) override;
 
   virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override;
   virtual int32_t step_motor_active_cfg() override;

components/zcancmder/zcan_protocol_parser.cpp

@@ -41,6 +41,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
   REGFN(step_motor_read_io_index_in_stm32);
   REGFN(step_motor_set_subdevice_reg);
   REGFN(step_motor_get_subdevice_reg);
+  REGFN(step_motor_read_pos_and_enc_pos);
 
   REGFN(mini_servo_enable);
   REGFN(mini_servo_read_pos);
@@ -334,6 +335,12 @@ int32_t ZCanProtocolParser::step_motor_get_subdevice_reg(cmdcontxt_t* cxt) {
   cxt->acklen  = 4;
   return module->step_motor_get_subdevice_reg(cxt->params[0], ack);
 }
+int32_t ZCanProtocolParser::step_motor_read_pos_and_enc_pos(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(0);
+  int32_t* ack = (int32_t*)cxt->ackbuf;
+  cxt->acklen  = 8;
+  return module->step_motor_read_pos_and_enc_pos(&ack[0], &ack[1]);
+}
 
 #undef MODULE_CLASS
 #define MODULE_CLASS ZIMiniServo

components/zcancmder/zcan_protocol_parser.hpp

@@ -77,6 +77,7 @@ class ZCanProtocolParser : public IZCanReceiverListener {
   CMDFN(step_motor_read_io_index_in_stm32);
   CMDFN(step_motor_set_subdevice_reg);
   CMDFN(step_motor_get_subdevice_reg);
+  CMDFN(step_motor_read_pos_and_enc_pos);
 
   CMDFN(mini_servo_enable);
   CMDFN(mini_servo_read_pos);