From 0f185e7634bc9edb53206e3219a7021bca3abd9f Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Wed, 26 Jun 2024 15:34:07 +0800
Subject: [PATCH] update

---
 components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 11 ++++++++++-
 components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp |  3 +++
 components/zcancmder/zcan_protocol_parser.cpp            | 16 ++++++++++++++++
 components/zcancmder/zcan_protocol_parser.hpp            |  3 +++
 4 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
index c859e8b..c19af66 100644
--- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
+++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
@@ -520,4 +520,13 @@ void XYRobotCtrlModule::moveBy(int32_t dx, int32_t dy, int32_t v) {
   int32_t x, y;
   getnowpos(x, y);
   moveTo(x + dx, y + dy, v);
-}
\ No newline at end of file
+}
+
+int32_t XYRobotCtrlModule::xymotor_read_inio(int32_t ioindex, int32_t* val) {
+  if (ioindex < 0 || ioindex >= m_ngpio) return err::kparam_out_of_range;
+  *val = m_gpiotable[ioindex].getState();
+}
+int32_t XYRobotCtrlModule::xymotor_read_inio_index_in_stm32(int32_t ioindex, int32_t* val) {
+  if (ioindex < 0 || ioindex >= m_ngpio) return err::kparam_out_of_range;
+  *val = m_gpiotable[ioindex].getPin();
+}
diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
index 901af12..9afa982 100644
--- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
+++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
@@ -97,6 +97,9 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule {
   virtual int32_t xymotor_move_to_zero() override;
   virtual int32_t xymotor_read_pos(int32_t* x, int32_t* y) override;
 
+  virtual int32_t xymotor_read_inio(int32_t ioindex, int32_t* val) override;
+  virtual int32_t xymotor_read_inio_index_in_stm32(int32_t ioindex, int32_t* val) override;
+
  private:
   int32_t do_public_check();
   int32_t do_xymotor_move_to(int32_t x, int32_t y);
diff --git a/components/zcancmder/zcan_protocol_parser.cpp b/components/zcancmder/zcan_protocol_parser.cpp
index 7a90d2a..fbf07af 100644
--- a/components/zcancmder/zcan_protocol_parser.cpp
+++ b/components/zcancmder/zcan_protocol_parser.cpp
@@ -100,6 +100,8 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
   REGFN(xymotor_move_to);
   REGFN(xymotor_move_to_zero);
   REGFN(xymotor_read_pos);
+  REGFN(xymotor_read_inio);
+  REGFN(xymotor_read_inio_index_in_stm32);
 #if 1
   REGFN(a8000_optical_module_power_ctrl);
   REGFN(a8000_optical_open_laser);
@@ -650,6 +652,20 @@ int32_t ZCanProtocolParser::xymotor_read_pos(cmdcontxt_t* cxt) {
   return module->xymotor_read_pos(&ack[0], &ack[1]);
 }
 
+int32_t ZCanProtocolParser::xymotor_read_inio(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(1);
+  int32_t* ack = (int32_t*)cxt->ackbuf;
+  cxt->acklen  = 4;
+  return module->xymotor_read_inio(cxt->params[0], ack);
+}
+
+int32_t ZCanProtocolParser::xymotor_read_inio_index_in_stm32(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(1);
+  int32_t* ack = (int32_t*)cxt->ackbuf;
+  cxt->acklen  = 4;
+  return module->xymotor_read_inio_index_in_stm32(cxt->params[0], ack);
+}
+
 #undef MODULE_CLASS
 
 #if 1
diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp
index 04e6bd5..fb1ff63 100644
--- a/components/zcancmder/zcan_protocol_parser.hpp
+++ b/components/zcancmder/zcan_protocol_parser.hpp
@@ -139,6 +139,9 @@ class ZCanProtocolParser : public IZCanReceiverListener {
   CMDFN(xymotor_move_to);
   CMDFN(xymotor_move_to_zero);
   CMDFN(xymotor_read_pos);
+  CMDFN(xymotor_read_inio);
+  CMDFN(xymotor_read_inio_index_in_stm32);
+
 #if 1
   CMDFN(a8000_optical_module_power_ctrl);
   CMDFN(a8000_optical_open_laser);