From 78d0adecdca2d183c80348f9de3fa55e82bbcdb9 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 23 Oct 2023 12:51:27 +0800 Subject: [PATCH] update --- components/cmdscheduler/cmd_scheduler_v2.hpp | 61 ++++++++++++++++++++++ .../script_cmder_eq20_servomotor.cpp | 32 ++++++------ .../script_cmder_eq20_servomotor.hpp | 7 +-- .../scirpt_cmder_mini_servo_motor_ctrl_module.cpp | 18 +++---- .../scirpt_cmder_mini_servo_motor_ctrl_module.hpp | 6 +-- .../step_motor_45/script_cmder_step_motor_45.cpp | 14 ++--- .../step_motor_45/script_cmder_step_motor_45.hpp | 6 +-- 7 files changed, 103 insertions(+), 41 deletions(-) diff --git a/components/cmdscheduler/cmd_scheduler_v2.hpp b/components/cmdscheduler/cmd_scheduler_v2.hpp index 1075455..672bf47 100644 --- a/components/cmdscheduler/cmd_scheduler_v2.hpp +++ b/components/cmdscheduler/cmd_scheduler_v2.hpp @@ -46,4 +46,65 @@ class CmdSchedulerV2 : public ICmdParser { void dumpack(ICmdParserACK* ack); }; +class __Params { + int32_t m_paramN; + const char** m_paraV; + + public: + __Params(int32_t paramN, const char** paraV) : m_paramN(paramN), m_paraV(paraV) {} + + int32_t getInt(int32_t index) { + index -= 1; + if (index >= m_paramN) { + return 0; + } + if (index < 0) { + return 0; + } + return atoi(m_paraV[index]); + } + + bool getBool(int32_t index) { + index -= 1; + if (index >= m_paramN) { + return false; + } + if (index < 0) { + return false; + } + return atoi(m_paraV[index]) != 0; + } +}; + +#define DO_CMD(cond) \ + { \ + int32_t ret = cond; \ + if (ret != 0) { \ + return ret; \ + } \ + } +#define IMPL_CMD(cmd, ...) \ + __Params con(paramN, paraV); \ + DO_CMD(findmodule(con.getInt(1), &module)); \ + DO_CMD(module->cmd(__VA_ARGS__)); \ + return (int32_t)0; + +#define IMPL_READ_STATE(cmd, ...) \ + __Params con(paramN, paraV); \ + DO_CMD(findmodule(con.getInt(1), &module)); \ + DO_CMD(module->cmd(__VA_ARGS__)); \ + cmd_dump_ack(ack); \ + return (int32_t)0; + +#define REG_CMD___NO_ACK(prefix, cmd, para, npara, ...) /**/ \ + m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](int32_t paramN, const char** paraV, ICmdParserACK* __ack) { /**/ \ + IMPL_CMD(cmd, __VA_ARGS__); /**/ \ + }); + +#define REG_CMD_WITH_ACK(prefix, cmd, para, npara, acktype, ...) /**/ \ + m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](int32_t paramN, const char** paraV, ICmdParserACK* __ack) { /**/ \ + acktype ack; /**/ \ + IMPL_READ_STATE(cmd, __VA_ARGS__); /**/ \ + }); + } // namespace iflytop \ No newline at end of file diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp index debfa1a..51b21b9 100644 --- a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp @@ -7,7 +7,7 @@ using namespace iflytop; #define TAG "CMD" -void ScriptCmderEq20Servomotor::initialize(CmdScheduler* cmdScheduler) { +void ScriptCmderEq20Servomotor::initialize(CmdSchedulerV2* cmdScheduler) { m_cmdScheduler = cmdScheduler; ZASSERT(m_cmdScheduler != nullptr); regcmd(); @@ -48,7 +48,7 @@ static void cmd_dump_ack(Eq20ServoMotor::servo_internal_status_t ack) { // ZLOGI(TAG, "encoder_battery_warning:%d", ack.data.sbit.encoder_battery_warning); ZLOGI(TAG, "encoder_battery_report :%d", ack.data.sbit.encoder_battery_report); } -//static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } +// static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } void ScriptCmderEq20Servomotor::regmodule(int id, Eq20ServoMotor* robot) { ZASSERT(moduler.find(id) == moduler.end()); @@ -57,24 +57,24 @@ void ScriptCmderEq20Servomotor::regmodule(int id, Eq20ServoMotor* robot) { } void ScriptCmderEq20Servomotor::regcmd() { - REG_CMD___NO_ACK("eq20_", enable, "(id,en)", 2, con->getBool(2)); - REG_CMD___NO_ACK("eq20_", move_to, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); - REG_CMD___NO_ACK("eq20_", move_by, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); - REG_CMD___NO_ACK("eq20_", rotate, "(id,rpm,acctime)", 3, con->getInt(2), con->getInt(3)); - REG_CMD___NO_ACK("eq20_", move_to_zero_forward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); - REG_CMD___NO_ACK("eq20_", move_to_zero_backward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD___NO_ACK("eq20_", enable, "(id,en)", 2, con.getBool(2)); + REG_CMD___NO_ACK("eq20_", move_to, "(id,pos,rpm,acctime)", 4, con.getInt(2), con.getInt(3), con.getInt(4)); + REG_CMD___NO_ACK("eq20_", move_by, "(id,pos,rpm,acctime)", 4, con.getInt(2), con.getInt(3), con.getInt(4)); + REG_CMD___NO_ACK("eq20_", rotate, "(id,rpm,acctime)", 3, con.getInt(2), con.getInt(3)); + REG_CMD___NO_ACK("eq20_", move_to_zero_forward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con.getInt(2), con.getInt(3), con.getInt(4)); + REG_CMD___NO_ACK("eq20_", move_to_zero_backward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con.getInt(2), con.getInt(3), con.getInt(4)); REG_CMD___NO_ACK("eq20_", stop, "(id)", 1); REG_CMD_WITH_ACK("eq20_", get_pos, "(id)", 1, int32_t, ack); REG_CMD_WITH_ACK("eq20_", get_servo_internal_state, "(id)", 1, Eq20ServoMotor::servo_internal_status_t, ack); REG_CMD_WITH_ACK("eq20_", get_io_state, "(id)", 1, int32_t, ack); - REG_CMD_WITH_ACK("eq20_", read_pn, "(id,pnadd)", 2, int32_t, con->getInt(2), ack); - REG_CMD___NO_ACK("eq20_", write_pn, "(id,pnadd,value)", 3, con->getInt(2), con->getInt(3)); - REG_CMD___NO_ACK("eq20_", write_pn_bit, "(id,pnadd,off,value)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); - REG_CMD_WITH_ACK("eq20_", read_pn_bit, "(id,pnadd,off)", 3, int32_t, con->getInt(2), con->getInt(3), ack); + REG_CMD_WITH_ACK("eq20_", read_pn, "(id,pnadd)", 2, int32_t, con.getInt(2), ack); + REG_CMD___NO_ACK("eq20_", write_pn, "(id,pnadd,value)", 3, con.getInt(2), con.getInt(3)); + REG_CMD___NO_ACK("eq20_", write_pn_bit, "(id,pnadd,off,value)", 4, con.getInt(2), con.getInt(3), con.getInt(4)); + REG_CMD_WITH_ACK("eq20_", read_pn_bit, "(id,pnadd,off)", 3, int32_t, con.getInt(2), con.getInt(3), ack); - REG_CMD___NO_ACK("eq20_", write_reg, "(id,regadd,value)", 3, con->getInt(2), con->getInt(3)); - REG_CMD_WITH_ACK("eq20_", read_reg, "(id,regadd)", 2, int32_t, con->getInt(2), ack); - REG_CMD___NO_ACK("eq20_", write_reg_bit, "(id,regadd,off,value)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); - REG_CMD_WITH_ACK("eq20_", read_reg_bit, "(id,regadd,off)", 3, int32_t, con->getInt(2), con->getInt(3), ack); + REG_CMD___NO_ACK("eq20_", write_reg, "(id,regadd,value)", 3, con.getInt(2), con.getInt(3)); + REG_CMD_WITH_ACK("eq20_", read_reg, "(id,regadd)", 2, int32_t, con.getInt(2), ack); + REG_CMD___NO_ACK("eq20_", write_reg_bit, "(id,regadd,off,value)", 4, con.getInt(2), con.getInt(3), con.getInt(4)); + REG_CMD_WITH_ACK("eq20_", read_reg_bit, "(id,regadd,off)", 3, int32_t, con.getInt(2), con.getInt(3), ack); } diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp index 1781462..2060b8f 100644 --- a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp @@ -3,17 +3,18 @@ #include "eq20_servomotor.hpp" #include "sdk/os/zos.hpp" -#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +// #include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +#include "sdk/components/cmdscheduler/cmd_scheduler_v2.hpp" namespace iflytop { class ScriptCmderEq20Servomotor { - CmdScheduler* m_cmdScheduler; + CmdSchedulerV2* m_cmdScheduler; map moduler; Eq20ServoMotor* module; public: - void initialize(CmdScheduler* cmdScheduler); + void initialize(CmdSchedulerV2* cmdScheduler); void regmodule(int id, Eq20ServoMotor* robot); void regcmd(); diff --git a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp index a53be79..856324f 100644 --- a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp @@ -34,7 +34,7 @@ static void cmd_dump_ack(I_MiniServoModule::basic_param_t& ack) { // ZLOGI(TAG, "protectTorque: %d", ack.protectTorque); } -void ScirptCmderMiniServoMotorCtrlModule::initialize(CmdScheduler* cmdScheduler) { +void ScirptCmderMiniServoMotorCtrlModule::initialize(CmdSchedulerV2* cmdScheduler) { m_cmdScheduler = cmdScheduler; ZASSERT(m_cmdScheduler != nullptr); regcmd(); @@ -58,14 +58,14 @@ void ScirptCmderMiniServoMotorCtrlModule::regmodule(int id, I_MiniServoModule* r } void ScirptCmderMiniServoMotorCtrlModule::regcmd() { - REG_CMD___NO_ACK("mini_servo_", enable, "(id,en)", 2, con->getBool(2)); - REG_CMD___NO_ACK("mini_servo_", stop, "(id,stop_type)", 2, con->getInt(2)); - REG_CMD___NO_ACK("mini_servo_", position_calibrate, "(id,pos)", 2, con->getInt(2)); - REG_CMD___NO_ACK("mini_servo_", rotate, "(id,speed,torque,time)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_rotate done %d", ecode); }); - REG_CMD___NO_ACK("mini_servo_", move_to, "(id,pos,speed,torque)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_to done %d", ecode); }); - REG_CMD___NO_ACK("mini_servo_", move_by, "(id,pos,speed,torque)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_by done %d", ecode); }); - REG_CMD___NO_ACK("mini_servo_", move_forward, "(id,torque)", 2, con->getInt(2)); - REG_CMD___NO_ACK("mini_servo_", move_backward, "(id,torque)", 2, con->getInt(2)); + REG_CMD___NO_ACK("mini_servo_", enable, "(id,en)", 2, con.getBool(2)); + REG_CMD___NO_ACK("mini_servo_", stop, "(id,stop_type)", 2, con.getInt(2)); + REG_CMD___NO_ACK("mini_servo_", position_calibrate, "(id,pos)", 2, con.getInt(2)); + REG_CMD___NO_ACK("mini_servo_", rotate, "(id,speed,torque,time)", 4, con.getInt(2), con.getInt(3), con.getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_rotate done %d", ecode); }); + REG_CMD___NO_ACK("mini_servo_", move_to, "(id,pos,speed,torque)", 4, con.getInt(2), con.getInt(3), con.getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_to done %d", ecode); }); + REG_CMD___NO_ACK("mini_servo_", move_by, "(id,pos,speed,torque)", 4, con.getInt(2), con.getInt(3), con.getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_by done %d", ecode); }); + REG_CMD___NO_ACK("mini_servo_", move_forward, "(id,torque)", 2, con.getInt(2)); + REG_CMD___NO_ACK("mini_servo_", move_backward, "(id,torque)", 2, con.getInt(2)); REG_CMD_WITH_ACK("mini_servo_", read_version, "(id)", 1, I_MiniServoModule::version_t, ack); REG_CMD_WITH_ACK("mini_servo_", read_status, "(id)", 1, I_MiniServoModule::status_t, ack); REG_CMD_WITH_ACK("mini_servo_", read_detailed_status, "(id)", 1, I_MiniServoModule::detailed_status_t, ack); diff --git a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp index 3e28326..75b259a 100644 --- a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp @@ -2,18 +2,18 @@ #include #include "sdk/os/zos.hpp" -#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp" #include "sdk\components\zprotocols\zcancmder\api\i_mini_servo_module.hpp" namespace iflytop { class ScirptCmderMiniServoMotorCtrlModule { - CmdScheduler* m_cmdScheduler; + CmdSchedulerV2* m_cmdScheduler; map moduler; I_MiniServoModule* module; public: - void initialize(CmdScheduler* cmdScheduler); + void initialize(CmdSchedulerV2* cmdScheduler); void regmodule(int id, I_MiniServoModule* robot); void regcmd(); diff --git a/components/step_motor_45/script_cmder_step_motor_45.cpp b/components/step_motor_45/script_cmder_step_motor_45.cpp index a7fc53e..9938ec8 100644 --- a/components/step_motor_45/script_cmder_step_motor_45.cpp +++ b/components/step_motor_45/script_cmder_step_motor_45.cpp @@ -7,7 +7,7 @@ using namespace iflytop; #define TAG "CMD" -void ScriptCmderStepMotor45::initialize(CmdScheduler* cmdScheduler) { +void ScriptCmderStepMotor45::initialize(CmdSchedulerV2* cmdScheduler) { m_cmdScheduler = cmdScheduler; ZASSERT(m_cmdScheduler != nullptr); regcmd(); @@ -36,17 +36,17 @@ static void cmd_dump_ack(bool& ack) { ZLOGI(TAG, "ack %d", ack); } void ScriptCmderStepMotor45::regcmd() { #define PREFIX "step_motor_45_" - REG_CMD___NO_ACK(PREFIX, rotate, "(id,direction)", 2, con->getInt(2)); - REG_CMD___NO_ACK(PREFIX, move_by, "(id,pos)", 2, con->getInt(2)); - REG_CMD___NO_ACK(PREFIX, move_to, "(id,pos)", 2, con->getInt(2)); - REG_CMD___NO_ACK(PREFIX, set_default_speed, "(id,speed)", 2, con->getInt(2)); + REG_CMD___NO_ACK(PREFIX, rotate, "(id,direction)", 2, con.getInt(2)); + REG_CMD___NO_ACK(PREFIX, move_by, "(id,pos)", 2, con.getInt(2)); + REG_CMD___NO_ACK(PREFIX, move_to, "(id,pos)", 2, con.getInt(2)); + REG_CMD___NO_ACK(PREFIX, set_default_speed, "(id,speed)", 2, con.getInt(2)); REG_CMD_WITH_ACK(PREFIX, get_default_speed, "(id)", 1, int32_t, ack); REG_CMD_WITH_ACK(PREFIX, is_reach_target_pos, "(id)", 1, bool, ack); REG_CMD___NO_ACK(PREFIX, stop, "(id)", 1); REG_CMD___NO_ACK(PREFIX, zero_calibration, "(id)", 1); REG_CMD_WITH_ACK(PREFIX, get_pos, "(id)", 1, int32_t, ack); - REG_CMD___NO_ACK(PREFIX, set_pos, "(id,pos)", 2, con->getInt(2)); + REG_CMD___NO_ACK(PREFIX, set_pos, "(id,pos)", 2, con.getInt(2)); REG_CMD_WITH_ACK(PREFIX, get_zero_pin_state, "(id,pos)", 1, int32_t, ack); - // REG_CMD___NO_ACK(PREFIX, initialize, "(id,driverPin1,driverPin2,driverPin3,driverPin4,zeroPin)", 6, con->getInt(2), con->getInt(3), con->getInt(4), con->getInt(5), con->getInt(6), con->getInt(7)); + // REG_CMD___NO_ACK(PREFIX, initialize, "(id,driverPin1,driverPin2,driverPin3,driverPin4,zeroPin)", 6, con.getInt(2), con.getInt(3), con.getInt(4), con.getInt(5), con.getInt(6), con.getInt(7)); } \ No newline at end of file diff --git a/components/step_motor_45/script_cmder_step_motor_45.hpp b/components/step_motor_45/script_cmder_step_motor_45.hpp index 4e084b5..3a2f80c 100644 --- a/components/step_motor_45/script_cmder_step_motor_45.hpp +++ b/components/step_motor_45/script_cmder_step_motor_45.hpp @@ -2,17 +2,17 @@ #include #include "sdk/os/zos.hpp" -#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp" #include "step_motor_45.hpp" namespace iflytop { class ScriptCmderStepMotor45 { - CmdScheduler* m_cmdScheduler; + CmdSchedulerV2* m_cmdScheduler; map moduler; StepMotor45* module; public: - void initialize(CmdScheduler* cmdScheduler); + void initialize(CmdSchedulerV2* cmdScheduler); void regmodule(int id, StepMotor45* robot); void regcmd();