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 74f88a2..3ec2f9f 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -537,3 +537,14 @@ int32_t XYRobotCtrlModule::xymotor_set_pos(int32_t x, int32_t y) { setnowpos(x, y); return 0; } + +int32_t XYRobotCtrlModule::xymotor_motor_move_by_direct(int32_t motor1_dpos, int32_t motor2_dpos) { + m_stepM1->moveBy(motor1_dpos, m_cfg.default_velocity); + m_stepM2->moveBy(motor2_dpos, m_cfg.default_velocity); + return 0; +} +int32_t XYRobotCtrlModule::xymotor_read_enc_direct(int32_t* enc1, int32_t* enc2) { + *enc1 = m_stepM1->read_enc_val(); + *enc2 = m_stepM2->read_enc_val(); + return 0; +} \ No newline at end of file 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 b344c8d..3a4a3b4 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -101,6 +101,9 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule { 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; + virtual int32_t xymotor_motor_move_by_direct(int32_t motor1_dpos, int32_t motor2_dpos) override; + virtual int32_t xymotor_read_enc_direct(int32_t* enc1, int32_t* enc2) 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 4ec3352..2a84ad3 100644 --- a/components/zcancmder/zcan_protocol_parser.cpp +++ b/components/zcancmder/zcan_protocol_parser.cpp @@ -102,6 +102,8 @@ void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { REGFN(xymotor_read_inio); REGFN(xymotor_read_inio_index_in_stm32); REGFN(xymotor_set_pos); + REGFN(xymotor_motor_move_by_direct); + REGFN(xymotor_read_enc_direct); #if 1 REGFN(a8000_optical_module_power_ctrl); diff --git a/components/zcancmder/zcan_protocol_parser.hpp b/components/zcancmder/zcan_protocol_parser.hpp index 791ff7d..d99d4b3 100644 --- a/components/zcancmder/zcan_protocol_parser.hpp +++ b/components/zcancmder/zcan_protocol_parser.hpp @@ -141,6 +141,8 @@ class ZCanProtocolParser : public IZCanReceiverListener { CMDFN(xymotor_read_inio); CMDFN(xymotor_read_inio_index_in_stm32); CMDFN(xymotor_set_pos); + CMDFN(xymotor_motor_move_by_direct); + CMDFN(xymotor_read_enc_direct); #if 1 CMDFN(a8000_optical_module_power_ctrl);