update

components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -13,6 +13,18 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g
   m_lock.init();
   m_statu_lock.init();
   m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal);
+
+  run_param_t           run_param;
+  run_to_zero_param_t   run_to_zero_param;
+  warning_limit_param_t warning_limit_param;
+
+  read_run_param(run_param);
+  read_run_to_zero_param(run_to_zero_param);
+  read_warning_limit_param(warning_limit_param);
+
+  set_run_param(run_param);
+  set_run_to_zero_param(run_to_zero_param);
+  set_warning_limit_param(warning_limit_param);
 }
 int32_t StepMotorCtrlModule::move_to(int32_t tox, function<void(move_to_cb_status_t& status)> status_cb) {  //
 
@@ -191,11 +203,15 @@ int32_t StepMotorCtrlModule::read_version(version_t& version) { return 0; }
 int32_t StepMotorCtrlModule::read_status(status_t& status) {
   zlock_guard l(m_statu_lock);
   status.status = m_status;
+  if (m_Xgpio) status.io_state |= m_Xgpio->getState() ? 0x01 : 0x00;
+  if (m_end_gpio) status.io_state |= m_end_gpio->getState() ? 0x02 : 0x00;
   getnowpos(status.x);
   return 0;
 }
 int32_t StepMotorCtrlModule::read_debug_info(debug_info_t& debug_info) {
   debug_info.status = m_status;
+  if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00;
+  if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00;
   getnowpos(debug_info.x);
   return 0;
 }
@@ -277,7 +293,18 @@ int32_t StepMotorCtrlModule::set_warning_limit_param(uint8_t operation, const wa
   }
   return 0;
 }
+int32_t StepMotorCtrlModule::exec_move_to_zero_task(int32_t& dx) {
+  int32_t xnow;
+  getnowpos(xnow);
 
+  int32_t ret = exec_move_to_zero_task();
+
+  int32_t xnow2;
+  getnowpos(xnow2);
+
+  dx = xnow2 - xnow;
+  return ret;
+}
 int32_t StepMotorCtrlModule::exec_move_to_zero_task() {
   /*******************************************************************************
    *                                  Ô¶ÀëXÁãµã                                   *
@@ -377,3 +404,7 @@ void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) {
   x -= m_zero_shift_x;
   motor_pos = x;
 }
+void StepMotorCtrlModule::updateStatus(uint8_t status) {
+  zlock_guard lock(m_statu_lock);
+  m_status = status;
+}

components/step_motor_ctrl_module/step_motor_ctrl_module.hpp

@@ -64,6 +64,14 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
   virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override;
   virtual int32_t set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) override;
 
+  int32_t read_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_read, param, param); }
+  int32_t read_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_read, param, param); }
+  int32_t read_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_read, param, param); }
+
+  int32_t set_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_set, param, param); }
+  int32_t set_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_set, param, param); }
+  int32_t set_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_set, param, param); }
+
  private:
   void updateStatus(uint8_t status);
 

components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -194,12 +194,17 @@ int32_t XYRobotCtrlModule::read_status(status_t& status) {
   zlock_guard lock(m_statu_lock);
   status        = {0};
   status.status = m_status;
+  if (m_Xgpio) status.iostate |= m_Xgpio->getState() ? 0x01 : 0x00;
+  if (m_Ygpio) status.iostate |= m_Ygpio->getState() ? 0x02 : 0x00;
   getnowpos(status.x, status.y);
   return 0;
 }
 int32_t XYRobotCtrlModule::read_debug_info(debug_info_t& debug_info) {
   zlock_guard lock(m_lock);
-  debug_info        = {0};
+  debug_info = {0};
+  if (m_Xgpio) debug_info.iostate |= m_Xgpio->getState() ? 0x01 : 0x00;
+  if (m_Ygpio) debug_info.iostate |= m_Ygpio->getState() ? 0x02 : 0x00;
+
   debug_info.status = m_status;
   getnowpos(debug_info.x, debug_info.y);
   return 0;

components/zcancmder_module/zcan_step_motor_ctrl_module.cpp

@@ -0,0 +1,118 @@
+#include "zcan_step_motor_ctrl_module.hpp"
+using namespace iflytop;
+
+#define TAG "ZCanStepMotorCtrlModule"
+#if 1
+void ZCanStepMotorCtrlModule::initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* module) {
+  m_cancmder = cancmder;
+  m_id       = id;
+  m_module   = module;
+  cancmder->registerListener(this);
+}
+void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
+// printf("ZCanStepMotorCtrlModule::onRceivePacket %d %d\n", rxcmd->get_cmdheader()->cmdid, rxcmd->get_cmdheader()->subcmdid);
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_stop, m_id) { errorcode = m_module->stop(cmd->stop_type); }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero, m_id) {
+    errorcode = m_module->move_to_zero([this, cmdheader](I_StepMotorCtrlModule::move_to_zero_cb_status_t& status) {
+      osDelay(5);  // 用来保证回执消息在前面
+      kcmd_step_motor_ctrl_move_to_zero_report_t report = {0};
+      ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero exec_status:%d", status.exec_status);
+
+      report.id     = m_id;
+      report.status = status;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
+  }
+  END_PP();
+  PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, m_id) {
+    errorcode = m_module->move_to_zero_with_calibrate(cmd->nowx, [this, cmdheader](I_StepMotorCtrlModule::move_to_zero_with_calibrate_cb_status_t& status) {
+      osDelay(5);  // 用来保证回执消息在前面
+      kcmd_step_motor_ctrl_move_to_zero_with_calibrate_report_t report = {0};
+      ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero_with_calibrate exec_status:%d %d", status.exec_status, status.zero_shift_x);
+
+      report.id     = m_id;
+      report.status = status;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_move_to, m_id) {
+    errorcode = m_module->move_to(cmd->x, [this, cmdheader](I_StepMotorCtrlModule::move_to_cb_status_t& status) {
+      osDelay(5);  // 用来保证回执消息在前面
+      kcmd_step_motor_ctrl_move_to_report_t report = {0};
+      ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to exec_status:%d %d", status.exec_status, status.tox);
+
+      report.id     = m_id;
+      report.status = status;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_move_by, m_id) {
+    errorcode = m_module->move_by(cmd->dx, [this, cmdheader](I_StepMotorCtrlModule::move_by_cb_status_t& status) {
+      osDelay(5);  // 用来保证回执消息在前面
+      kcmd_step_motor_ctrl_move_by_report_t report = {0};
+      ZLOGI(TAG, "kcmd_step_motor_ctrl_move_by exec_status:%d %d", status.exec_status, status.dx);
+
+      report.id     = m_id;
+      report.status = status;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_force_change_current_pos, m_id) { errorcode = m_module->force_change_current_pos(cmd->x); }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_rotate, m_id) {
+    errorcode = m_module->rotate(cmd->speed, cmd->run_time, [this, cmdheader](I_StepMotorCtrlModule::rotate_cb_status_t& status) {
+      osDelay(5);  // 用来保证回执消息在前面
+      kcmd_step_motor_ctrl_rotate_report_t report = {0};
+      ZLOGI(TAG, "kcmd_step_motor_ctrl_rotate exec_status:%d %d", status.exec_status, status.lastforms);
+
+      report.id     = m_id;
+      report.status = status;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_read_version, m_id) {  //
+    errorcode = m_module->read_version(ack->ack);
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_read_status, m_id) {  //
+    errorcode = m_module->read_status(ack->ack);
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_read_debug_info, m_id) {  //
+    errorcode = m_module->read_debug_info(ack->ack);
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_param, m_id) {  //
+    errorcode = m_module->set_run_param(cmd->opt_type, cmd->param, ack->ack);
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_set_warning_limit_param, m_id) {  //
+    errorcode = m_module->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack);
+  }
+  END_PP();
+
+  PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_to_zero_param, m_id) {  //
+    errorcode = m_module->set_run_to_zero_param(cmd->opt_type, cmd->param, ack->ack);
+  }
+  END_PP();
+}
+#endif

components/zcancmder_module/zcan_step_motor_ctrl_module.hpp

@@ -0,0 +1,16 @@
+#pragma once
+#include "sdk/components/zprotocols/zcancmder/api/i_step_motor_ctrl_module.hpp"
+#include "sdk\components\zcancmder\zcanreceiver.hpp"
+namespace iflytop {
+class ZCanStepMotorCtrlModule : public ZCanCmderListener {
+  ZCanCmder*             m_cancmder = nullptr;
+  int                    m_id       = 0;
+  I_StepMotorCtrlModule* m_module;
+
+  uint8_t m_txbuf[128] = {0};
+
+ public:
+  void         initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* m_module);
+  virtual void onRceivePacket(CanPacketRxBuffer* rxcmd);
+};
+}  // namespace iflytop

components/zprotocols/zcancmder

@@ -1 +1 @@
-Subproject commit ba0b50468b776c0bf4182300d80d5d973ebea058
+Subproject commit 614d8b811b4737c5b2e08e9abc3e10a5fd6cb11c