From 46580c915dfcd4c9e07675d04751c2df0c4f7380 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 21 Oct 2023 10:35:17 +0800 Subject: [PATCH] update --- api/errorcode.hpp | 1 + api/i_zcan_cmder.hpp | 8 ++++---- api/packet_interface.hpp | 2 +- api/zi_motor.hpp | 8 ++++++++ cmdid.hpp | 15 ++++++++++----- protocol_parser.cpp | 24 ++++++++++++++++++------ protocol_parser.hpp | 2 +- protocol_proxy.cpp | 12 ++++++++++++ protocol_proxy.hpp | 7 +++++++ zmodule_device_manager.cpp | 5 +++++ zmodule_device_manager.hpp | 7 ++++++- zmodule_device_script_cmder_paser.cpp | 12 ++++++++++++ 12 files changed, 85 insertions(+), 18 deletions(-) diff --git a/api/errorcode.hpp b/api/errorcode.hpp index 4676126..8331c53 100644 --- a/api/errorcode.hpp +++ b/api/errorcode.hpp @@ -47,6 +47,7 @@ typedef enum { kmodule_not_inited = ERROR_CODE(200, 0), kmodule_not_found = ERROR_CODE(200, 1), kmodule_opeation_break_by_user = ERROR_CODE(200, 2), // Óû§ÖÐ¶Ï + kmodule_error = ERROR_CODE(200, 3), // δ֪´íÎó /** * @brief motor error diff --git a/api/i_zcan_cmder.hpp b/api/i_zcan_cmder.hpp index 0a3d326..609e072 100644 --- a/api/i_zcan_cmder.hpp +++ b/api/i_zcan_cmder.hpp @@ -11,16 +11,16 @@ using namespace zcr; class IZcanCmderListener { public: - virtual void onRceivePacket(cmd_header_t *rxcmd, uint8_t *data, int32_t len) = 0; + virtual void onRceivePacket(zcr_cmd_header_t *rxcmd, uint8_t *data, int32_t len) = 0; }; class IZCanCmder { public: public: virtual void registerListener(IZcanCmderListener *listener) = 0; - virtual void sendAck(cmd_header_t *rx_cmd_header, uint8_t *data, int32_t len) = 0; - virtual void sendAck(cmd_header_t *rx_cmd_header, int32_t *ackvar, uint8_t nack) = 0; - virtual void sendErrorAck(cmd_header_t *rx_cmd_header, int32_t errorcode) = 0; + virtual void sendAck(zcr_cmd_header_t *rx_cmd_header, uint8_t *data, int32_t len) = 0; + virtual void sendAck(zcr_cmd_header_t *rx_cmd_header, int32_t *ackvar, uint8_t nack) = 0; + virtual void sendErrorAck(zcr_cmd_header_t *rx_cmd_header, int32_t errorcode) = 0; }; } // namespace iflytop \ No newline at end of file diff --git a/api/packet_interface.hpp b/api/packet_interface.hpp index 368cefc..a1f92e3 100644 --- a/api/packet_interface.hpp +++ b/api/packet_interface.hpp @@ -11,7 +11,7 @@ typedef struct { uint8_t packetType; uint16_t submoduleid; uint8_t data[]; -} cmd_header_t; +} zcr_cmd_header_t; #pragma pack(pop) typedef enum { diff --git a/api/zi_motor.hpp b/api/zi_motor.hpp index 61fb57c..2ee4bb4 100644 --- a/api/zi_motor.hpp +++ b/api/zi_motor.hpp @@ -14,8 +14,16 @@ class ZIMotor { virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } + + virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } + virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } + virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } + virtual int32_t motor_move_to_with_torque(int32_t pos, int32_t torque) { return err::koperation_not_support; } + virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; } + virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; } + // s32 pos, s32 speed, s32 torque, }; } // namespace iflytop \ No newline at end of file diff --git a/cmdid.hpp b/cmdid.hpp index 10e2355..84c90f8 100644 --- a/cmdid.hpp +++ b/cmdid.hpp @@ -16,11 +16,16 @@ typedef enum { kmodule_get_error = CMDID(1, 10), // para:{}, ack:{1} kmodule_clear_error = CMDID(1, 11), // para:{}, ack:{} - kmotor_enable = CMDID(2, 1), // para:{1}, ack:{} - kmotor_rotate = CMDID(2, 2), // para:{1,4}, ack:{} - kmotor_move_by = CMDID(2, 3), // para:{4,4}, ack:{} - kmotor_move_to = CMDID(2, 4), // para:{4,4}, ack:{} - kmotor_move_to_with_torque = CMDID(2, 5), // para:{4,4}, ack:{} + kmotor_enable = CMDID(2, 1), // para:{1}, ack:{} + kmotor_rotate = CMDID(2, 2), // para:{1,4}, ack:{} + kmotor_move_by = CMDID(2, 3), // para:{4,4}, ack:{} + kmotor_move_to = CMDID(2, 4), // para:{4,4}, ack:{} + kmotor_rotate_acctime = CMDID(2, 5), // para:{4,4}, ack:{} + kmotor_move_by_acctime = CMDID(2, 6), // para:{4,4}, ack:{} + kmotor_move_to_acctime = CMDID(2, 7), // para:{4,4}, ack:{} + kmotor_move_to_with_torque = CMDID(2, 8), // para:{4,4}, ack:{} + kmotor_move_to_zero_forward = CMDID(2, 9), // para:{4,4,4,4}, ack:{} //int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime + kmotor_move_to_zero_backward = CMDID(2, 10), // para:{4,4,4,4}, ack:{} //int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime kxymotor_enable = CMDID(3, 1), // para:{1}, ack:{} kxymotor_move_by = CMDID(3, 2), // para:{4,4,4}, ack:{} diff --git a/protocol_parser.cpp b/protocol_parser.cpp index e74fee5..42ab72c 100644 --- a/protocol_parser.cpp +++ b/protocol_parser.cpp @@ -6,18 +6,18 @@ using namespace std; #define PROCESS_PACKET_BEGIN(var_cmdid, var_moduleType) \ int32_t* param __attribute__((__unused__)) = (int32_t*)data; \ - int paramNum __attribute__((__unused__)) = (len - sizeof(cmd_header_t)) / sizeof(int32_t); \ + int paramNum __attribute__((__unused__)) = (len - sizeof(zcr_cmd_header_t)) / sizeof(int32_t); \ if (cmdid == var_cmdid) { \ auto* mod = dynamic_cast(module); \ if (mod == nullptr) { \ - m_cancmder->sendErrorAck(rxcmd, err::koperation_not_support); \ + m_cancmder->sendErrorAck(rxcmd, err::koperation_not_support); \ return; \ } -#define CHECK_PARAM_NUM(num) \ - if (paramNum != num) { \ +#define CHECK_PARAM_NUM(num) \ + if (paramNum != num) { \ m_cancmder->sendErrorAck(rxcmd, err::kcmd_param_num_error); \ - return; \ + return; \ } #define PROCESS_PACKET_XX_END(ackNum) \ @@ -58,12 +58,18 @@ using namespace std; #define PROCESS_PACKET_32(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 3, 2, param[0], param[1], param[2], &ack[1], &ack[2]) #define PROCESS_PACKET_33(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 3, 3, param[0], param[1], param[2], &ack[1], &ack[2], &ack[3]) +#define PROCESS_PACKET_40(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 0, param[0], param[1], param[2], param[3]) +#define PROCESS_PACKET_41(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 1, param[0], param[1], param[2], param[3], &ack[1]) +#define PROCESS_PACKET_42(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 2, param[0], param[1], param[2], param[3], &ack[1], &ack[2]) +#define PROCESS_PACKET_43(var_cmdid, var_moduleType, var_funcname) PROCESS_PACKET_XX(var_cmdid, var_moduleType, var_funcname, 4, 3, param[0], param[1], param[2], param[3], &ack[1], &ack[2], &ack[3]) + + void ZIProtocolParser::initialize(IZCanCmder* cancmder) { m_cancmder = cancmder; m_cancmder->registerListener(this); } void ZIProtocolParser::registerModule(uint16_t id, ZIModule* module) { m_modulers[id] = module; } -void ZIProtocolParser::onRceivePacket(cmd_header_t* rxcmd, uint8_t* data, int32_t len) { +void ZIProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, int32_t len) { uint16_t submoduleid = rxcmd->submoduleid; auto it = m_modulers.find(submoduleid); if (it == m_modulers.end()) { @@ -96,6 +102,12 @@ void ZIProtocolParser::onRceivePacket(cmd_header_t* rxcmd, uint8_t* data, int32_ PROCESS_PACKET_30(kmotor_move_by, ZIMotor, motor_move_by); PROCESS_PACKET_30(kmotor_move_to, ZIMotor, motor_move_to); PROCESS_PACKET_20(kmotor_move_to_with_torque, ZIMotor, motor_move_to_with_torque); + PROCESS_PACKET_30(kmotor_rotate_acctime, ZIMotor, motor_rotate_acctime); + PROCESS_PACKET_30(kmotor_move_by_acctime, ZIMotor, motor_move_by_acctime); + PROCESS_PACKET_30(kmotor_move_to_acctime, ZIMotor, motor_move_to_acctime); + PROCESS_PACKET_40(kmotor_move_to_zero_forward, ZIMotor, motor_move_to_zero_forward); + PROCESS_PACKET_40(kmotor_move_to_zero_backward, ZIMotor, motor_move_to_zero_backward); + /******************************************************************************* * xymotor * *******************************************************************************/ diff --git a/protocol_parser.hpp b/protocol_parser.hpp index 4077ff9..e3c6eed 100644 --- a/protocol_parser.hpp +++ b/protocol_parser.hpp @@ -12,7 +12,7 @@ class ZIProtocolParser : public IZcanCmderListener { public: void initialize(IZCanCmder* cancmder); - virtual void onRceivePacket(cmd_header_t* rxcmd, uint8_t* data, int32_t len); + virtual void onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, int32_t len); void registerModule(uint16_t id, ZIModule* module); }; diff --git a/protocol_proxy.cpp b/protocol_proxy.cpp index e80cb8e..7edb0a4 100644 --- a/protocol_proxy.cpp +++ b/protocol_proxy.cpp @@ -68,6 +68,11 @@ using namespace iflytop; #define PROXY_IMPL_32(cmdindex) PROXY_IMPL_XX(cmdindex, 3, 2, PROXY_IMPL_3X_PARA(), PROXY_IMPL_X2_ACK()); #define PROXY_IMPL_33(cmdindex) PROXY_IMPL_XX(cmdindex, 3, 3, PROXY_IMPL_3X_PARA(), PROXY_IMPL_X3_ACK()); +#define PROXY_IMPL_40(cmdindex) PROXY_IMPL_XX(cmdindex, 4, 0, PROXY_IMPL_4X_PARA(), PROXY_IMPL_X0_ACK()); +#define PROXY_IMPL_41(cmdindex) PROXY_IMPL_XX(cmdindex, 4, 1, PROXY_IMPL_4X_PARA(), PROXY_IMPL_X1_ACK()); +#define PROXY_IMPL_42(cmdindex) PROXY_IMPL_XX(cmdindex, 4, 2, PROXY_IMPL_4X_PARA(), PROXY_IMPL_X2_ACK()); +#define PROXY_IMPL_43(cmdindex) PROXY_IMPL_XX(cmdindex, 4, 3, PROXY_IMPL_4X_PARA(), PROXY_IMPL_X3_ACK()); + int32_t ZIProtocolProxy::module_stop() { PROXY_IMPL_00(kmodule_stop); } int32_t ZIProtocolProxy::module_break() { PROXY_IMPL_00(kmodule_break); } int32_t ZIProtocolProxy::module_get_last_exec_status(int32_t *ack0) { PROXY_IMPL_01(kmodule_get_last_exec_status); } @@ -92,6 +97,13 @@ int32_t ZIProtocolProxy::motor_move_by(int32_t para0, int32_t para1, int32_t par int32_t ZIProtocolProxy::motor_move_to(int32_t para0, int32_t para1, int32_t para2) { PROXY_IMPL_30(kmotor_move_to); } int32_t ZIProtocolProxy::motor_move_to_with_torque(int32_t para0, int32_t para1) { PROXY_IMPL_20(kmotor_move_to_with_torque); } +int32_t ZIProtocolProxy::motor_rotate_acctime(int32_t para0, int32_t para1, int32_t para2) { PROXY_IMPL_30(kmotor_rotate_acctime); } +int32_t ZIProtocolProxy::motor_move_by_acctime(int32_t para0, int32_t para1, int32_t para2) { PROXY_IMPL_30(kmotor_move_by_acctime); } +int32_t ZIProtocolProxy::motor_move_to_acctime(int32_t para0, int32_t para1, int32_t para2) { PROXY_IMPL_30(kmotor_move_to_acctime); } + +int32_t ZIProtocolProxy::motor_move_to_zero_forward(int32_t para0, int32_t para1, int32_t para2, int32_t para3) { PROXY_IMPL_40(kmotor_move_to_zero_forward); } +int32_t ZIProtocolProxy::motor_move_to_zero_backward(int32_t para0, int32_t para1, int32_t para2, int32_t para3) { PROXY_IMPL_40(kmotor_move_to_zero_backward); } + /******************************************************************************* * ZIXYMotor * *******************************************************************************/ diff --git a/protocol_proxy.hpp b/protocol_proxy.hpp index 7bdba74..69af298 100644 --- a/protocol_proxy.hpp +++ b/protocol_proxy.hpp @@ -50,6 +50,13 @@ class ZIProtocolProxy : public ZIMotor, // virtual int32_t motor_move_to(int32_t direction, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_to_with_torque(int32_t pos, int32_t torque) override; + virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override; + + virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + /******************************************************************************* * ZIXYMotor * *******************************************************************************/ diff --git a/zmodule_device_manager.cpp b/zmodule_device_manager.cpp index d073e3a..c608aaf 100644 --- a/zmodule_device_manager.cpp +++ b/zmodule_device_manager.cpp @@ -31,6 +31,11 @@ int32_t ZModuleDeviceManager::motor_rotate(uint16_t id, int32_t direction, int32 int32_t ZModuleDeviceManager::motor_move_by(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acc) { PROXY_IMPL(ZIMotor, motor_move_by, distance, motor_velocity, acc); } int32_t ZModuleDeviceManager::motor_move_to(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acc) { PROXY_IMPL(ZIMotor, motor_move_to, position, motor_velocity, acc); } int32_t ZModuleDeviceManager::motor_move_to_with_torque(uint16_t id, int32_t pos, int32_t torque) { PROXY_IMPL(ZIMotor, motor_move_to_with_torque, pos, torque); } +int32_t ZModuleDeviceManager::motor_rotate_acctime(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acctime) { PROXY_IMPL(ZIMotor, motor_rotate_acctime, direction, motor_velocity, acctime); } +int32_t ZModuleDeviceManager::motor_move_by_acctime(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acctime) { PROXY_IMPL(ZIMotor, motor_move_by_acctime, distance, motor_velocity, acctime); } +int32_t ZModuleDeviceManager::motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime) { PROXY_IMPL(ZIMotor, motor_move_to_acctime, position, motor_velocity, acctime); } +int32_t ZModuleDeviceManager::motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { PROXY_IMPL(ZIMotor, motor_move_to_zero_forward, findzerospeed, findzeroedge_speed, acc, overtime); } +int32_t ZModuleDeviceManager::motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { PROXY_IMPL(ZIMotor, motor_move_to_zero_backward, findzerospeed, findzeroedge_speed, acc, overtime); } /******************************************************************************* * ZIXYMotor * *******************************************************************************/ diff --git a/zmodule_device_manager.hpp b/zmodule_device_manager.hpp index cd62d49..4a78f7f 100644 --- a/zmodule_device_manager.hpp +++ b/zmodule_device_manager.hpp @@ -29,7 +29,7 @@ class ZModuleDeviceManager { int32_t module_readio(uint16_t id, int32_t *io); int32_t module_writeio(uint16_t id, int32_t io); - int32_t module_read_adc(uint16_t id, int adcindex, int32_t *adc); + int32_t module_read_adc(uint16_t id, int32_t adcindex, int32_t *adc); int32_t module_get_error(uint16_t id, int32_t *iserror); int32_t module_clear_error(uint16_t id); @@ -42,6 +42,11 @@ class ZModuleDeviceManager { int32_t motor_rotate(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acc); int32_t motor_move_by(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acc); int32_t motor_move_to(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acc); + int32_t motor_rotate_acctime(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acctime); + int32_t motor_move_by_acctime(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acctime); + int32_t motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime); + int32_t motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime); + int32_t motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime); int32_t motor_move_to_with_torque(uint16_t id, int32_t pos, int32_t torque); /******************************************************************************* diff --git a/zmodule_device_script_cmder_paser.cpp b/zmodule_device_script_cmder_paser.cpp index 515640c..4d19b07 100644 --- a/zmodule_device_script_cmder_paser.cpp +++ b/zmodule_device_script_cmder_paser.cpp @@ -38,6 +38,11 @@ using namespace std; #define PROCESS_PACKET_42(var_funcname, cmdhelp) PROCESS_PACKET_XX(var_funcname, cmdhelp, 4, 2, atoi(paraV[0]), atoi(paraV[1]), atoi(paraV[2]), atoi(paraV[3]), ack->getAck(0), ack->getAck(1)) #define PROCESS_PACKET_43(var_funcname, cmdhelp) PROCESS_PACKET_XX(var_funcname, cmdhelp, 4, 3, atoi(paraV[0]), atoi(paraV[1]), atoi(paraV[2]), atoi(paraV[3]), ack->getAck(0), ack->getAck(1), ack->getAck(2)) +#define PROCESS_PACKET_50(var_funcname, cmdhelp) PROCESS_PACKET_XX(var_funcname, cmdhelp, 5, 0, atoi(paraV[0]), atoi(paraV[1]), atoi(paraV[2]), atoi(paraV[3]), atoi(paraV[4])) +#define PROCESS_PACKET_51(var_funcname, cmdhelp) PROCESS_PACKET_XX(var_funcname, cmdhelp, 5, 1, atoi(paraV[0]), atoi(paraV[1]), atoi(paraV[2]), atoi(paraV[3]), atoi(paraV[4]), ack->getAck(0)) +#define PROCESS_PACKET_52(var_funcname, cmdhelp) PROCESS_PACKET_XX(var_funcname, cmdhelp, 5, 2, atoi(paraV[0]), atoi(paraV[1]), atoi(paraV[2]), atoi(paraV[3]), atoi(paraV[4]), ack->getAck(0), ack->getAck(1)) +#define PROCESS_PACKET_53(var_funcname, cmdhelp) PROCESS_PACKET_XX(var_funcname, cmdhelp, 5, 3, atoi(paraV[0]), atoi(paraV[1]), atoi(paraV[2]), atoi(paraV[3]), atoi(paraV[4]), ack->getAck(0), ack->getAck(1), ack->getAck(2)) + void ZModuleDeviceScriptCmderPaser::initialize(ICmdParser* cancmder, ZModuleDeviceManager* deviceManager) { PROCESS_PACKET_10(module_stop, "stop (mid)"); PROCESS_PACKET_10(module_break, "module_break (mid)"); @@ -55,7 +60,14 @@ void ZModuleDeviceScriptCmderPaser::initialize(ICmdParser* cancmder, ZModuleDevi PROCESS_PACKET_40(motor_rotate, "motor_rotate (mid, direction, motor_velocity, acc)"); PROCESS_PACKET_40(motor_move_by, "motor_move_by (mid, distance, motor_velocity, acc)"); PROCESS_PACKET_40(motor_move_to, "motor_move_to (mid, position, motor_velocity, acc)"); + + PROCESS_PACKET_40(motor_rotate_acctime, "motor_rotate_acctime (mid, direction, motor_velocity, acctime)"); + PROCESS_PACKET_40(motor_move_by_acctime, "motor_move_by_acctime (mid, distance, motor_velocity, acctime)"); + PROCESS_PACKET_40(motor_move_to_acctime, "motor_move_to_acctime (mid, position, motor_velocity, acctime)"); + PROCESS_PACKET_30(motor_move_to_with_torque, "motor_move_to_with_torque (mid, pos, torque)"); + PROCESS_PACKET_50(motor_move_to_zero_forward, "motor_move_to_zero_forward (mid, findzerospeed, findzeroedge_speed, acc, overtime)"); + PROCESS_PACKET_50(motor_move_to_zero_backward, "motor_move_to_zero_backward (mid, findzerospeed, findzeroedge_speed, acc, overtime)"); PROCESS_PACKET_20(xymotor_enable, "xymotor_enable (mid, enable)"); PROCESS_PACKET_40(xymotor_move_by, "xymotor_move_by (mid, dx, dy, motor_velocity)");