From 115dbcfc41877dd4ceb3412df2838322ce7f09e2 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 21 Oct 2023 11:05:50 +0800 Subject: [PATCH] update --- api/config_index.hpp | 12 ++++++ api/zi_module.hpp | 11 ++++++ api/zi_motor.hpp | 5 +++ cmdid.hpp | 24 +++++++----- protocol_parser.cpp | 20 +++++----- protocol_proxy.cpp | 5 +++ protocol_proxy.hpp | 6 +++ zmodule_device_manager.cpp | 4 ++ zmodule_device_manager.hpp | 5 +++ zmodule_device_script_cmder_paser.cpp | 74 ++++++++++++++++++++++++++++++----- 10 files changed, 137 insertions(+), 29 deletions(-) diff --git a/api/config_index.hpp b/api/config_index.hpp index fbe396f..6c0dc98 100644 --- a/api/config_index.hpp +++ b/api/config_index.hpp @@ -7,6 +7,18 @@ using namespace std; #define CONFIG_CODE(type, subconfigindex) (type + subconfigindex) typedef enum { + kmotorcfg_x_shift = CONFIG_CODE(100, 0), // x偏移 + kmotorcfg_y_shift = CONFIG_CODE(100, 1), // y偏移 + kmotorcfg_z_shift = CONFIG_CODE(100, 2), // z偏移 + + kmotorcfg_x_shaft = CONFIG_CODE(100, 3), // x轴是否反转 + kmotorcfg_y_shaft = CONFIG_CODE(100, 4), // y轴是否反转 + kmotorcfg_z_shaft = CONFIG_CODE(100, 5), // z轴是否反转 + + kmotorcfg_x_one_circle_pulse = CONFIG_CODE(100, 6), // x轴一圈脉冲数 + kmotorcfg_y_one_circle_pulse = CONFIG_CODE(100, 7), // y轴一圈脉冲数 + kmotorcfg_z_one_circle_pulse = CONFIG_CODE(100, 8), // z轴一圈脉冲数 + } config_index_t; } // namespace iflytop \ No newline at end of file diff --git a/api/zi_module.hpp b/api/zi_module.hpp index e0d5259..afe4927 100644 --- a/api/zi_module.hpp +++ b/api/zi_module.hpp @@ -8,6 +8,8 @@ namespace iflytop { using namespace std; class ZIModule { + int32_t m_inited_flag = 0; + public: virtual ~ZIModule() {} @@ -27,5 +29,14 @@ class ZIModule { virtual int32_t module_writeio(int32_t io) { return err::koperation_not_support; } virtual int32_t module_read_adc(int32_t adcindex, int32_t *adc) { return err::koperation_not_support; } + + virtual int32_t module_set_inited_flag(int32_t flag) { + m_inited_flag = flag; + return 0; + } + virtual int32_t module_get_inited_flag(int32_t *flag) { + *flag = m_inited_flag; + return 0; + } }; } // namespace iflytop \ No newline at end of file diff --git a/api/zi_motor.hpp b/api/zi_motor.hpp index 2ee4bb4..b3178cc 100644 --- a/api/zi_motor.hpp +++ b/api/zi_motor.hpp @@ -24,6 +24,11 @@ class ZIMotor { 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; } + virtual int32_t motor_read_pos(int32_t* pos) { return err::koperation_not_support; } + virtual int32_t motor_set_current_pos_by_change_shift(int32_t pos) { return err::koperation_not_support; } // 一般用于舵机 + + // virtual int32_t motor_set_shaft(); + // s32 pos, s32 speed, s32 torque, }; } // namespace iflytop \ No newline at end of file diff --git a/cmdid.hpp b/cmdid.hpp index 84c90f8..efeeb01 100644 --- a/cmdid.hpp +++ b/cmdid.hpp @@ -15,17 +15,21 @@ typedef enum { kmodule_read_adc = CMDID(1, 9), // para:{4}, ack:{4} kmodule_get_error = CMDID(1, 10), // para:{}, ack:{1} kmodule_clear_error = CMDID(1, 11), // para:{}, ack:{} + kmodule_set_inited_flag = CMDID(1, 12), // para:{4}, ack:{} + kmodule_get_inited_flag = CMDID(1, 13), // para:{}, ack:{4} - 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 + 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 + kmotor_read_pos = CMDID(2, 11), // para:{}, ack:{4} + kmotor_set_current_pos_by_change_shift = CMDID(2, 12), // para:{4}, ack:{} 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 42ab72c..1cb3e30 100644 --- a/protocol_parser.cpp +++ b/protocol_parser.cpp @@ -4,14 +4,14 @@ using namespace iflytop; using namespace std; #define TAG "ZIProtocolParser" -#define PROCESS_PACKET_BEGIN(var_cmdid, var_moduleType) \ - int32_t* param __attribute__((__unused__)) = (int32_t*)data; \ +#define PROCESS_PACKET_BEGIN(var_cmdid, var_moduleType) \ + int32_t* param __attribute__((__unused__)) = (int32_t*)data; \ 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); \ - return; \ + if (cmdid == var_cmdid) { \ + auto* mod = dynamic_cast(module); \ + if (mod == nullptr) { \ + m_cancmder->sendErrorAck(rxcmd, err::koperation_not_support); \ + return; \ } #define CHECK_PARAM_NUM(num) \ @@ -63,7 +63,6 @@ using namespace std; #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); @@ -93,7 +92,8 @@ void ZIProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, in PROCESS_PACKET_11(kmodule_read_adc, ZIModule, module_read_adc); PROCESS_PACKET_01(kmodule_get_error, ZIModule, module_get_error); PROCESS_PACKET_00(kmodule_clear_error, ZIModule, module_clear_error); - + PROCESS_PACKET_10(kmodule_set_inited_flag, ZIModule, module_set_inited_flag); + PROCESS_PACKET_01(kmodule_get_inited_flag, ZIModule, module_get_inited_flag); /******************************************************************************* * motor * *******************************************************************************/ @@ -107,6 +107,8 @@ void ZIProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, in 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); + PROCESS_PACKET_01(kmotor_read_pos, ZIMotor, motor_read_pos); + PROCESS_PACKET_10(kmotor_set_current_pos_by_change_shift, ZIMotor, motor_set_current_pos_by_change_shift); /******************************************************************************* * xymotor * diff --git a/protocol_proxy.cpp b/protocol_proxy.cpp index 7edb0a4..f5cf287 100644 --- a/protocol_proxy.cpp +++ b/protocol_proxy.cpp @@ -88,6 +88,9 @@ int32_t ZIProtocolProxy::module_read_adc(int32_t para0, int32_t *ack0) { PROXY_I int32_t ZIProtocolProxy::module_get_error(int32_t *ack0) { PROXY_IMPL_01(kmodule_get_error); } int32_t ZIProtocolProxy::module_clear_error() { PROXY_IMPL_00(kmodule_clear_error); } +int32_t ZIProtocolProxy::module_set_inited_flag(int32_t para0) { PROXY_IMPL_10(kmodule_set_inited_flag); }; +int32_t ZIProtocolProxy::module_get_inited_flag(int32_t *ack0) { PROXY_IMPL_01(kmodule_get_inited_flag); }; + /******************************************************************************* * ZIMotor * *******************************************************************************/ @@ -104,6 +107,8 @@ int32_t ZIProtocolProxy::motor_move_to_acctime(int32_t para0, int32_t para1, int 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); } +int32_t ZIProtocolProxy::motor_read_pos(int32_t *ack0) { PROXY_IMPL_01(kmotor_read_pos); } +int32_t ZIProtocolProxy::motor_set_current_pos_by_change_shift(int32_t para0) { PROXY_IMPL_10(kmotor_set_current_pos_by_change_shift); } /******************************************************************************* * ZIXYMotor * *******************************************************************************/ diff --git a/protocol_proxy.hpp b/protocol_proxy.hpp index 69af298..28bd038 100644 --- a/protocol_proxy.hpp +++ b/protocol_proxy.hpp @@ -40,6 +40,9 @@ class ZIProtocolProxy : public ZIMotor, // virtual int32_t module_get_error(int32_t *iserror) override; virtual int32_t module_clear_error() override; + virtual int32_t module_set_inited_flag(int32_t flag) override; + virtual int32_t module_get_inited_flag(int32_t *flag) override; + /******************************************************************************* * ZIMotor * *******************************************************************************/ @@ -57,6 +60,9 @@ class ZIProtocolProxy : public ZIMotor, // 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; + virtual int32_t motor_read_pos(int32_t *pos) override; + virtual int32_t motor_set_current_pos_by_change_shift(int32_t pos) override; + /******************************************************************************* * ZIXYMotor * *******************************************************************************/ diff --git a/zmodule_device_manager.cpp b/zmodule_device_manager.cpp index c608aaf..cfbd0f2 100644 --- a/zmodule_device_manager.cpp +++ b/zmodule_device_manager.cpp @@ -23,6 +23,8 @@ int32_t ZModuleDeviceManager::module_writeio(uint16_t id, int32_t io) { PROXY_IM int32_t ZModuleDeviceManager::module_read_adc(uint16_t id, int32_t adcindex, int32_t *adc) { PROXY_IMPL(ZIModule, module_read_adc, adcindex, adc); } int32_t ZModuleDeviceManager::module_get_error(uint16_t id, int32_t *iserror) { PROXY_IMPL(ZIModule, module_get_error, iserror); } int32_t ZModuleDeviceManager::module_clear_error(uint16_t id) { PROXY_IMPL(ZIModule, module_clear_error); } +int32_t ZModuleDeviceManager::module_set_inited_flag(uint16_t id, int32_t flag) { PROXY_IMPL(ZIModule, module_set_inited_flag, flag); } +int32_t ZModuleDeviceManager::module_get_inited_flag(uint16_t id, int32_t *flag) { PROXY_IMPL(ZIModule, module_get_inited_flag, flag); } /******************************************************************************* * ZIMotor * *******************************************************************************/ @@ -36,6 +38,8 @@ int32_t ZModuleDeviceManager::motor_move_by_acctime(uint16_t id, int32_t distanc 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); } +int32_t ZModuleDeviceManager::motor_read_pos(uint16_t id, int32_t *pos) { PROXY_IMPL(ZIMotor, motor_read_pos, pos); } +int32_t ZModuleDeviceManager::motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos) { PROXY_IMPL(ZIMotor, motor_set_current_pos_by_change_shift, pos); } /******************************************************************************* * ZIXYMotor * *******************************************************************************/ diff --git a/zmodule_device_manager.hpp b/zmodule_device_manager.hpp index 4a78f7f..ef2a565 100644 --- a/zmodule_device_manager.hpp +++ b/zmodule_device_manager.hpp @@ -34,6 +34,9 @@ class ZModuleDeviceManager { int32_t module_get_error(uint16_t id, int32_t *iserror); int32_t module_clear_error(uint16_t id); + int32_t module_set_inited_flag(uint16_t id, int32_t flag); + int32_t module_get_inited_flag(uint16_t id, int32_t *flag); + /******************************************************************************* * ZIMotor * *******************************************************************************/ @@ -48,6 +51,8 @@ class ZModuleDeviceManager { 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); + int32_t motor_read_pos(uint16_t id, int32_t *pos); + int32_t motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos); /******************************************************************************* * ZIXYMotor * diff --git a/zmodule_device_script_cmder_paser.cpp b/zmodule_device_script_cmder_paser.cpp index 4d19b07..5e86d08 100644 --- a/zmodule_device_script_cmder_paser.cpp +++ b/zmodule_device_script_cmder_paser.cpp @@ -44,17 +44,68 @@ using namespace std; #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)"); +#if 0 + /******************************************************************************* + * ZIModule * + *******************************************************************************/ + int32_t module_stop(uint16_t id); + int32_t module_break(uint16_t id); + + int32_t module_get_last_exec_status(uint16_t id, int32_t *status); + int32_t module_get_status(uint16_t id, int32_t *status); + + int32_t module_set_param(uint16_t id, int32_t param_id, int32_t param_value); + int32_t module_get_param(uint16_t id, int32_t param_id, int32_t *param_value); + + 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, 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); + + int32_t module_set_inited_flag(uint16_t id, int32_t flag); + int32_t module_get_inited_flag(uint16_t id, int32_t *flag); + + /******************************************************************************* + * ZIMotor * + *******************************************************************************/ + + int32_t motor_enable(uint16_t id, int32_t enable); + 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); + int32_t motor_read_pos(uint16_t id, int32_t *pos); + int32_t motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos); + + /******************************************************************************* + * ZIXYMotor * + *******************************************************************************/ + int32_t xymotor_enable(uint16_t id, int32_t enable); + int32_t xymotor_move_by(uint16_t id, int32_t dx, int32_t dy, int32_t motor_velocity); + int32_t xymotor_move_to(uint16_t id, int32_t x, int32_t y, int32_t motor_velocity); +#endif + + PROCESS_PACKET_10(module_stop, "module_stop (mid)"); PROCESS_PACKET_10(module_break, "module_break (mid)"); - PROCESS_PACKET_11(module_get_last_exec_status, "get_last_exec_status (mid)"); - PROCESS_PACKET_11(module_get_status, "get_status (mid)"); - PROCESS_PACKET_30(module_set_param, "set_param (mid, param_id, param_value)"); - PROCESS_PACKET_21(module_get_param, "get_param (mid, param_id)"); - PROCESS_PACKET_11(module_readio, "readio (mid)"); - PROCESS_PACKET_20(module_writeio, "writeio (mid, io)"); - PROCESS_PACKET_21(module_read_adc, "read_adc (mid,adc_id, adcindex)"); - PROCESS_PACKET_11(module_get_error, "get_error (mid)"); - PROCESS_PACKET_10(module_clear_error, "clear_error (mid)"); + PROCESS_PACKET_11(module_get_last_exec_status, "module_get_last_exec_status (mid)"); + PROCESS_PACKET_11(module_get_status, "module_get_status (mid)"); + PROCESS_PACKET_30(module_set_param, "module_set_param (mid, param_id, param_value)"); + PROCESS_PACKET_21(module_get_param, "module_get_param (mid, param_id)"); + PROCESS_PACKET_11(module_readio, "module_readio (mid)"); + PROCESS_PACKET_20(module_writeio, "module_writeio (mid, io)"); + PROCESS_PACKET_21(module_read_adc, "module_read_adc (mid,adc_id, adcindex)"); + PROCESS_PACKET_11(module_get_error, "module_get_error (mid)"); + PROCESS_PACKET_10(module_clear_error, "module_clear_error (mid)"); + PROCESS_PACKET_20(module_set_inited_flag, "module_set_inited_flag (mid, flag)"); + PROCESS_PACKET_11(module_get_inited_flag, "module_get_inited_flag (mid)"); PROCESS_PACKET_20(motor_enable, "motor_enable (mid, enable)"); PROCESS_PACKET_40(motor_rotate, "motor_rotate (mid, direction, motor_velocity, acc)"); @@ -69,6 +120,9 @@ void ZModuleDeviceScriptCmderPaser::initialize(ICmdParser* cancmder, ZModuleDevi 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_11(motor_read_pos, "motor_read_pos (mid)"); + PROCESS_PACKET_20(motor_set_current_pos_by_change_shift, "motor_set_current_pos_by_change_shift (mid, pos)"); + PROCESS_PACKET_20(xymotor_enable, "xymotor_enable (mid, enable)"); PROCESS_PACKET_40(xymotor_move_by, "xymotor_move_by (mid, dx, dy, motor_velocity)"); PROCESS_PACKET_40(xymotor_move_to, "xymotor_move_to (mid, x, y, motor_velocity)");