diff --git a/components/cmdscheduler/cmd_scheduler.cpp b/components/cmdscheduler/cmd_scheduler.cpp index 3e68200..cb3c16a 100644 --- a/components/cmdscheduler/cmd_scheduler.cpp +++ b/components/cmdscheduler/cmd_scheduler.cpp @@ -68,7 +68,7 @@ void CmdScheduler::schedule() { if (!m_dataisready) { return; } - for (size_t i = 0; i < m_rxsize; i++) { + for (int i = 0; i < m_rxsize; i++) { if (rxbuf[i] == '\r' || rxbuf[i] == '\n') { rxbuf[i] = '\0'; } diff --git a/components/cmdscheduler/cmd_scheduler.hpp b/components/cmdscheduler/cmd_scheduler.hpp index 19a9304..06bba17 100644 --- a/components/cmdscheduler/cmd_scheduler.hpp +++ b/components/cmdscheduler/cmd_scheduler.hpp @@ -98,12 +98,12 @@ class CmdScheduler { cmd_dump_ack(ack); \ return (int32_t)0; -#define REG_CMD(prefix, cmd, para, npara, ...) /**/ \ +#define REG_CMD___NO_ACK(prefix, cmd, para, npara, ...) /**/ \ m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) { /**/ \ IMPL_CMD(cmd, __VA_ARGS__); /**/ \ }); -#define REG_DUMP_ACK_CMD(prefix, cmd, para, npara, acktype, ...) /**/ \ +#define REG_CMD_WITH_ACK(prefix, cmd, para, npara, acktype, ...) /**/ \ m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) { /**/ \ acktype ack; /**/ \ IMPL_READ_STATE(cmd, __VA_ARGS__); /**/ \ 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 1788e72..b33a7d8 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 @@ -58,16 +58,16 @@ void ScirptCmderMiniServoMotorCtrlModule::regmodule(int id, I_MiniServoModule* r } void ScirptCmderMiniServoMotorCtrlModule::regcmd() { - REG_CMD("mini_servo_", enable, "(id,en)", 2, con->getBool(2)); - REG_CMD("mini_servo_", stop, "(id,stop_type)", 2, con->getInt(2)); - REG_CMD("mini_servo_", position_calibrate, "(id,pos)", 2, con->getInt(2)); - REG_CMD("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("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("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("mini_servo_", move_forward, "(id,torque)", 2, con->getInt(2)); - REG_CMD("mini_servo_", move_backward, "(id,torque)", 2, con->getInt(2)); - REG_DUMP_ACK_CMD("mini_servo_", read_version, "(id)", 1, I_MiniServoModule::version_t, ack); - REG_DUMP_ACK_CMD("mini_servo_", read_status, "(id)", 1, I_MiniServoModule::status_t, ack); - REG_DUMP_ACK_CMD("mini_servo_", read_detailed_status, "(id)", 1, I_MiniServoModule::detailed_status_t, ack); - REG_DUMP_ACK_CMD("mini_servo_", get_basic_param, "(id)", 1, I_MiniServoModule::basic_param_t, ack); + 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); + REG_CMD_WITH_ACK("mini_servo_", get_basic_param, "(id)", 1, I_MiniServoModule::basic_param_t, ack); } \ No newline at end of file diff --git a/components/step_motor_45/script_cmder_step_motor_45.cpp b/components/step_motor_45/script_cmder_step_motor_45.cpp new file mode 100644 index 0000000..dad93a2 --- /dev/null +++ b/components/step_motor_45/script_cmder_step_motor_45.cpp @@ -0,0 +1,51 @@ + +#include "script_cmder_step_motor_45.hpp" + +#include + +// #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" +using namespace iflytop; +#define TAG "CMD" + +void ScriptCmderStepMotor45::initialize(CmdScheduler* cmdScheduler) { + m_cmdScheduler = cmdScheduler; + ZASSERT(m_cmdScheduler != nullptr); + regcmd(); +} + +int32_t ScriptCmderStepMotor45::findmodule(int id, StepMotor45** module) { + auto it = moduler.find(id); + if (it == moduler.end()) { + return err::kce_no_such_module; + } + *module = it->second; + return 0; +} + +// static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } + +void ScriptCmderStepMotor45::regmodule(int id, StepMotor45* robot) { + ZASSERT(moduler.find(id) == moduler.end()); + ZASSERT(robot != nullptr); + moduler[id] = robot; +} + +static void cmd_dump_ack(int32_t& ack) { ZLOGI(TAG, "ack %d", ack); } +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_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, 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_scheduler.hpp b/components/step_motor_45/script_cmder_step_motor_45.hpp similarity index 78% rename from components/step_motor_45/script_cmder_step_motor_45_scheduler.hpp rename to components/step_motor_45/script_cmder_step_motor_45.hpp index 23af3ba..4e084b5 100644 --- a/components/step_motor_45/script_cmder_step_motor_45_scheduler.hpp +++ b/components/step_motor_45/script_cmder_step_motor_45.hpp @@ -6,9 +6,10 @@ #include "step_motor_45.hpp" namespace iflytop { -class ScriptCmderStepMotor45Scheduler { +class ScriptCmderStepMotor45 { CmdScheduler* m_cmdScheduler; map moduler; + StepMotor45* module; public: void initialize(CmdScheduler* cmdScheduler); @@ -16,7 +17,7 @@ class ScriptCmderStepMotor45Scheduler { void regcmd(); private: - StepMotor45* findModule(int id); + int32_t findmodule(int id, StepMotor45** module); }; } // namespace iflytop diff --git a/components/step_motor_45/script_cmder_step_motor_45_scheduler.cpp b/components/step_motor_45/script_cmder_step_motor_45_scheduler.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/components/step_motor_45/step_motor_45.cpp b/components/step_motor_45/step_motor_45.cpp index 772014c..2a683c6 100644 --- a/components/step_motor_45/step_motor_45.cpp +++ b/components/step_motor_45/step_motor_45.cpp @@ -57,11 +57,11 @@ void StepMotor45::initialize(cfg_t cfg) { return; } -void StepMotor45::rotate(int direction, int speed) { +int32_t StepMotor45::rotate(int direction, int speed) { CriticalContext cc; if (direction == 0) { stop(); - return; + return (int32_t)0; } if (direction > 0) { @@ -73,20 +73,23 @@ void StepMotor45::rotate(int direction, int speed) { posmode = false; m_state = direction > 0 ? kforward_rotation : krollback; ZLOGI(TAG, "rotate %d", direction); + return (int32_t)0; } -void StepMotor45::stop() { +int32_t StepMotor45::stop() { CriticalContext cc; stop_motor(); + return (int32_t)0; } -void StepMotor45::zeroCalibration() { +int32_t StepMotor45::zero_calibration() { CriticalContext cc; if (getzeropinstate()) { m_calibration = true; m_pos = 0; - return; + return (int32_t)0; } m_exec_zero_calibration_task = true; m_state = krollback; + return (int32_t)0; } void StepMotor45::onTimIRQ1ms() { @@ -197,11 +200,11 @@ void StepMotor45::schedule() { m_pos += direction; } -void StepMotor45::moveBy(int bypos, int speed) { +int32_t StepMotor45::move_by(int bypos, int speed) { CriticalContext cc; if (bypos == 0) { m_targetPos = m_pos; - return; + return (int32_t)0; } if (bypos > 0) { m_state = kforward_rotation; @@ -211,13 +214,14 @@ void StepMotor45::moveBy(int bypos, int speed) { m_targetPos = bypos + m_pos; posmode = true; + return (int32_t)0; } -void StepMotor45::moveTo(int to, int speed) { +int32_t StepMotor45::move_to(int to, int speed) { CriticalContext cc; if (to == m_pos) { - return; + return (int32_t)0; } if (to <= m_pos) { @@ -228,15 +232,15 @@ void StepMotor45::moveTo(int to, int speed) { m_targetPos = to; posmode = true; + return (int32_t)0; } -void StepMotor45::moveTo(int to) { moveTo(to, defaultspeed); } - -bool StepMotor45::isReachTargetPos() { +int32_t StepMotor45::move_to(int to) { move_to(to, defaultspeed); } +int32_t StepMotor45::is_reach_target_pos(bool& reach) { CriticalContext cc; if (posmode) { - return m_pos == m_targetPos; + reach = m_pos == m_targetPos; } - return true; + return (int32_t)0; } bool StepMotor45::getzeropinstate() { diff --git a/components/step_motor_45/step_motor_45.hpp b/components/step_motor_45/step_motor_45.hpp index 36b46a5..c04f77d 100644 --- a/components/step_motor_45/step_motor_45.hpp +++ b/components/step_motor_45/step_motor_45.hpp @@ -61,20 +61,27 @@ class StepMotor45 { public: void initialize(cfg_t cfg); - void rotate(int direction) { rotate(direction, defaultspeed); } - void rotate(int direction, int speed); - void moveBy(int pos, int speed); - void moveBy(int pos) { moveBy(pos, defaultspeed); } - - void moveTo(int to, int speed); - void moveTo(int to); - void setDefaultSpeed(int speed) { defaultspeed = speed; } - bool isReachTargetPos(); - void stop(); - void zeroCalibration(); - void setPos(int pos) { m_pos = pos; } - - int getPos() { return m_pos; } + int32_t rotate(int direction) { rotate(direction, defaultspeed); } + int32_t rotate(int direction, int speed); + int32_t move_by(int pos, int speed); + int32_t move_by(int pos) { move_by(pos, defaultspeed); } + + int32_t move_to(int to, int speed); + int32_t move_to(int to); + int32_t set_default_speed(int speed) { defaultspeed = speed; } + int32_t get_default_speed(int32_t &speed) { + speed = defaultspeed; + return (int32_t)0; + } + int32_t is_reach_target_pos(bool &reach); + int32_t stop(); + int32_t zero_calibration(); + int32_t set_pos(int32_t pos) { m_pos = pos; } + + int32_t get_pos(int32_t &pos) { + pos = m_pos; + return (int32_t)0; + } private: bool getzeropinstate(); diff --git a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp index da31f3b..1ebef56 100644 --- a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp @@ -73,23 +73,23 @@ void XYRobotScriptCmderModule::regcmd() { // #define PREIX "xy_robot_ctrl_" - REG_CMD(PREIX, enable, "(id,en)", 2, con->getBool(2)); - REG_CMD(PREIX, stop, "(id,stop_type)", 2, con->getInt(2)); - REG_CMD(PREIX, move_to_zero, "(id)", 1, [this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); }); - REG_CMD(PREIX, move_to_zero_with_calibrate, "(id,nowx,nowy)", 3, con->getInt(2), con->getInt(3), [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); }); - REG_CMD(PREIX, move_to, "(id,x,y,speed)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); }); - REG_CMD(PREIX, move_by, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); - REG_CMD(PREIX, force_change_current_pos, "(id,x,y)", 3, con->getInt(2), con->getInt(3)); - REG_DUMP_ACK_CMD(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); - REG_DUMP_ACK_CMD(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); - REG_DUMP_ACK_CMD(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); - REG_DUMP_ACK_CMD(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack); - REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); - REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); - REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); - REG_CMD("xy_robot_ctrl_", flush, "(id)", 1); - REG_CMD("xy_robot_ctrl_", factory_reset, "(id)", 1); - REG_CMD("xy_robot_ctrl_", move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); }); + REG_CMD___NO_ACK(PREIX, enable, "(id,en)", 2, con->getBool(2)); + REG_CMD___NO_ACK(PREIX, stop, "(id,stop_type)", 2, con->getInt(2)); + REG_CMD___NO_ACK(PREIX, move_to_zero, "(id)", 1, [this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); }); + REG_CMD___NO_ACK(PREIX, move_to_zero_with_calibrate, "(id,nowx,nowy)", 3, con->getInt(2), con->getInt(3), [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); }); + REG_CMD___NO_ACK(PREIX, move_to, "(id,x,y,speed)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); }); + REG_CMD___NO_ACK(PREIX, move_by, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); + REG_CMD___NO_ACK(PREIX, force_change_current_pos, "(id,x,y)", 3, con->getInt(2), con->getInt(3)); + REG_CMD_WITH_ACK(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); + REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); + REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); + REG_CMD_WITH_ACK(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack); + REG_CMD_WITH_ACK("xy_robot_ctrl_", read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); + REG_CMD_WITH_ACK("xy_robot_ctrl_", read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); + REG_CMD_WITH_ACK("xy_robot_ctrl_", read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); + REG_CMD___NO_ACK("xy_robot_ctrl_", flush, "(id)", 1); + REG_CMD___NO_ACK("xy_robot_ctrl_", factory_reset, "(id)", 1); + REG_CMD___NO_ACK("xy_robot_ctrl_", move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); }); m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) { const char* paramName = con->getString(2);