update

components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -513,6 +513,7 @@ void XYRobotCtrlModule::moveTo(int32_t t_x, int32_t t_y, int32_t v) {
   trysyncpos();
   int32_t target_m1pos, target_m2pos;
   forward_kinematics(t_x, t_y, target_m1pos, target_m2pos);
+  ZLOGI(TAG, "moveTo x:%d y:%d m1:%d m2:%d v:%d", t_x, t_y, target_m1pos, target_m2pos, v);
   m_stepM1->moveTo(target_m1pos, v);
   m_stepM2->moveTo(target_m2pos, v);
 }
@@ -532,3 +533,4 @@ int32_t XYRobotCtrlModule::xymotor_read_inio_index_in_stm32(int32_t ioindex, int
   *val = m_gpiotable[ioindex].getPin();
   return 0;
 }
+int32_t XYRobotCtrlModule::xymotor_set_pos(int32_t x, int32_t y) { setnowpos(x, y); }

components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp

@@ -99,6 +99,7 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule {
 
   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;
+  virtual int32_t xymotor_set_pos(int32_t x, int32_t y) override;
 
  private:
   int32_t do_public_check();

components/zcancmder/zcan_protocol_parser.cpp

@@ -102,6 +102,8 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) {
   REGFN(xymotor_read_pos);
   REGFN(xymotor_read_inio);
   REGFN(xymotor_read_inio_index_in_stm32);
+  REGFN(xymotor_set_pos);
+
 #if 1
   REGFN(a8000_optical_module_power_ctrl);
   REGFN(a8000_optical_open_laser);
@@ -666,6 +668,11 @@ int32_t ZCanProtocolParser::xymotor_read_inio_index_in_stm32(cmdcontxt_t* cxt) {
   return module->xymotor_read_inio_index_in_stm32(cxt->params[0], ack);
 }
 
+int32_t ZCanProtocolParser::xymotor_set_pos(cmdcontxt_t* cxt) {
+  CHECK_AND_GET_MODULE(2);
+  return module->xymotor_set_pos(cxt->params[0], cxt->params[1]);
+}
+
 #undef MODULE_CLASS
 
 #if 1

components/zcancmder/zcan_protocol_parser.hpp

@@ -141,6 +141,7 @@ class ZCanProtocolParser : public IZCanReceiverListener {
   CMDFN(xymotor_read_pos);
   CMDFN(xymotor_read_inio);
   CMDFN(xymotor_read_inio_index_in_stm32);
+  CMDFN(xymotor_set_pos);
 
 #if 1
   CMDFN(a8000_optical_module_power_ctrl);