update

components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -474,6 +474,62 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to_end_point() {
   }
   return do_step_motor_easy_move_to_end_point();
 }
+int32_t StepMotorCtrlModule::step_motor_easy_reciprocating_motion(int32_t startpos, int32_t endpos, int32_t times) {
+  ZLOGI(TAG, "m%d motor_reciprocating_motion %d %d %d", m_id, startpos, endpos, times);
+
+  int32_t ecode = check_befor_run();
+  if (ecode != 0) return ecode;
+  
+  int32_t x_startpos;
+  int32_t x_endpos;
+  int32_t x_beginpos;
+  getnowpos(x_beginpos);
+
+  if (m_cfg.min_d != 0 && x_startpos < m_cfg.min_d) x_startpos = m_cfg.min_d;
+  if (m_cfg.max_d != 0 && x_startpos > m_cfg.max_d) x_startpos = m_cfg.max_d;
+
+  if (m_cfg.min_d != 0 && x_endpos < m_cfg.min_d) x_endpos = m_cfg.min_d;
+  if (m_cfg.max_d != 0 && x_endpos > m_cfg.max_d) x_endpos = m_cfg.max_d;
+  m_thread.stop();
+  m_thread.start(
+      [this, x_startpos, x_endpos, x_beginpos, times]() {
+        befor_motor_move();
+
+        for (size_t i = 0; i < times; i++) {
+          ZLOGI(TAG, "reciprocating_motion move pos:%d", x_startpos);
+          moveTo(x_startpos, m_cfg.motor_default_velocity);
+          while (!m_stepM1->isReachTarget()) {
+            if (m_thread.getExitFlag()) break;
+            if (!check_when_run()) break;
+
+            vTaskDelay(5);
+          }
+
+          ZLOGI(TAG, "reciprocating_motion move pos:%d", x_endpos);
+          moveTo(x_endpos, m_cfg.motor_default_velocity);
+          while (!m_stepM1->isReachTarget()) {
+            if (m_thread.getExitFlag()) break;
+            if (!check_when_run()) break;
+
+            vTaskDelay(5);
+          }
+        }
+
+        ZLOGI(TAG, "reciprocating_motion finish,move begin pos:%d", x_beginpos);
+        moveTo(x_beginpos, m_cfg.motor_default_velocity);
+        while (!m_stepM1->isReachTarget()) {
+          if (m_thread.getExitFlag()) break;
+          if (!check_when_run()) break;
+
+          vTaskDelay(5);
+        }
+
+        after_motor_move();
+      },
+      [this]() { m_stepM1->stop(); });
+
+  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

@@ -84,6 +84,8 @@ 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_easy_reciprocating_motion(int32_t startpos, int32_t endpos, int32_t times) override;
+
 
   virtual int32_t step_motor_easy_set_current_pos(int32_t pos) override;
   virtual int32_t step_motor_active_cfg() override;
@@ -97,6 +99,8 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
 
   virtual int32_t step_motor_read_io_index_in_stm32(int32_t ioindex, int32_t* index_in_stm32) override;
 
+
+
  public:
   TMC51X0*  getMotor() { return m_stepM1; }
   config_t* getCfg() { return &m_cfg; }

components/zcancmder/zcan_protocol_parser.cpp

@@ -35,6 +35,7 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
   REGFN(step_motor_stop);
   REGFN(step_motor_read_io_state);
   REGFN(step_motor_easy_move_to_end_point);
+  REGFN(step_motor_easy_reciprocating_motion);
 
   REGFN(step_motor_read_tmc5130_status);
   REGFN(step_motor_read_tmc5130_state);
@@ -322,6 +323,11 @@ int32_t ZCanProtocolParser::step_motor_easy_move_to_end_point(cmdcontxt_t* cxt)
   CHECK_AND_GET_MODULE(0);
   return module->step_motor_easy_move_to_end_point();
 }
+// step_motor_easy_reciprocating_motion
+int32_t ZCanProtocolParser::step_motor_easy_reciprocating_motion(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(3);
+  return module->step_motor_easy_reciprocating_motion(cxt->params[0], cxt->params[1], cxt->params[2]);
+}
 
 int32_t ZCanProtocolParser::step_motor_read_tmc5130_status(cmdcontxt_t* cxt) {
   CHECK_AND_GET_MODULE(0);
@@ -762,8 +768,6 @@ int32_t ZCanProtocolParser::a8000_optical_read_raw(cmdcontxt_t* cxt) {
   return suc;
 }
 
-
-
 int32_t ZCanProtocolParser::a8k_opt_v2_t_start_scan(cmdcontxt_t* cxt) {
   CHECK_AND_GET_MODULE(3);
   return module->a8k_opt_v2_t_start_scan(cxt->params[0], cxt->params[1], cxt->params[2]);

components/zcancmder/zcan_protocol_parser.hpp

@@ -77,6 +77,9 @@ 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_easy_reciprocating_motion);
+
+  
 
   CMDFN(mini_servo_enable);
   CMDFN(mini_servo_read_pos);