diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index c405db4..df53d92 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -79,7 +79,7 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, functionregisterListener(this); +} + +void ZCanMiniServoCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { +#if 0 +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_enable, CMD(u8 id; u8 enable;), ACK(u8 id;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_stop, CMD(u8 id; u8 stop_type;), ACK(u8 id;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_position_calibrate, CMD(u8 id; s32 calibrate_pos;), ACK(u8 id;)); + +ZPACKET_CMD_ACK_AND_REPORT(kcmd_mini_servo_ctrl_rotate, CMD(s32 speed; s32 torque; s32 run_time;), ACK(u8 id;), REPORT(u8 id; I_MiniServoModule::rotate_cb_status_t report;)); +ZPACKET_CMD_ACK_AND_REPORT(kcmd_mini_servo_ctrl_move_to, CMD(s32 pos; s32 speed; s32 torque;), ACK(u8 id;), REPORT(u8 id; I_MiniServoModule::move_to_cb_status_t report;)); +ZPACKET_CMD_ACK_AND_REPORT(kcmd_mini_servo_ctrl_move_by, CMD(s32 pos; s32 speed; s32 torque;), ACK(u8 id;), REPORT(u8 id; I_MiniServoModule::move_by_cb_status_t report;)); +ZPACKET_CMD_ACK_AND_REPORT(kcmd_mini_servo_ctrl_run_with_torque, CMD(s32 torque; s32 run_time;), ACK(u8 id;), REPORT(u8 id; I_MiniServoModule::run_with_torque_cb_status_t report;)); +ZPACKET_CMD_ACK_AND_REPORT(kcmd_mini_servo_ctrl_move_by_nolimit, CMD(s32 pos; s32 speed; s32 torque;), ACK(u8 id;), REPORT(u8 id; I_MiniServoModule::move_by_nolimit_cb_status_t report;)); + +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_read_version, CMD(u8 id;), ACK(u8 id; I_MiniServoModule::version_t ack;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_read_status, CMD(u8 id;), ACK(u8 id; I_MiniServoModule::status_t ack;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_read_detailed_status, CMD(u8 id;), ACK(u8 id; I_MiniServoModule::detailed_status_t ack;)); + +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_set_run_param, CMD(u8 id; u8 opt_type; I_MiniServoModule::run_param_t param;), ACK(u8 id; I_MiniServoModule::run_param_t ack;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_set_basic_param, CMD(u8 id; u8 opt_type; I_MiniServoModule::basic_param_t param;), ACK(u8 id; I_MiniServoModule::basic_param_t ack;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_set_warning_limit_param, CMD(u8 id; u8 opt_type; I_MiniServoModule::warning_limit_param_t param;), ACK(u8 id; I_MiniServoModule::warning_limit_param_t ack;)); + +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_get_run_param, CMD(u8 id; u8 opt_type;), ACK(u8 id; I_MiniServoModule::run_param_t ack;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_get_basic_param, CMD(u8 id; u8 opt_type;), ACK(u8 id; I_MiniServoModule::basic_param_t ack;)); +ZPACKET_CMD_ACK(kcmd_mini_servo_ctrl_get_warning_limit_param, CMD(u8 id; u8 opt_type;), ACK(u8 id; I_MiniServoModule::warning_limit_param_t ack;)); +#endif +#if 0 + PROCESS_PACKET(kcmd_step_motor_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); } + END_PP(); +#endif + PROCESS_PACKET(kcmd_mini_servo_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_stop, m_id) { errorcode = m_module->stop(cmd->stop_type); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_position_calibrate, m_id) { errorcode = m_module->position_calibrate(cmd->calibrate_pos); } + END_PP(); +#if 0 + PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { // + errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](I_XYRobotCtrlModule::move_by_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_xy_robot_ctrl_move_by_report_t report = {0}; + ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d %d %d", status.exec_status, status.dx, status.dy); + + report.id = m_id; + report.status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); +#endif + PROCESS_PACKET(kcmd_mini_servo_ctrl_rotate, m_id) { // + errorcode = m_module->rotate(cmd->speed, cmd->torque, cmd->run_time, [this, cmdheader](I_MiniServoModule::rotate_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_mini_servo_ctrl_rotate_report_t report = {0}; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_rotate exec_status:%d %d", status.exec_status, status.has_run_time); + report.id = m_id; + report.report = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_move_to, m_id) { // + errorcode = m_module->move_to(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](I_MiniServoModule::move_to_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_mini_servo_ctrl_move_to_report_t report = {0}; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_to exec_status:%d %d", status.exec_status, status.pos); + report.id = m_id; + report.report = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by, m_id) { // + errorcode = m_module->move_by(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](I_MiniServoModule::move_by_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_mini_servo_ctrl_move_by_report_t report = {0}; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_by exec_status:%d %d", status.exec_status, status.dpos); + report.id = m_id; + report.report = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_run_with_torque, m_id) { // + errorcode = m_module->run_with_torque(cmd->torque, cmd->run_time, [this, cmdheader](I_MiniServoModule::run_with_torque_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_mini_servo_ctrl_run_with_torque_report_t report = {0}; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_run_with_torque exec_status:%d %d", status.exec_status, status.has_run_time); + report.id = m_id; + report.report = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by_nolimit, m_id) { // + errorcode = m_module->move_by_nolimit(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](I_MiniServoModule::move_by_nolimit_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_mini_servo_ctrl_move_by_nolimit_report_t report = {0}; + ZLOGI(TAG, "report kcmd_mini_servo_ctrl_move_by_nolimit exec_status:%d %d", status.exec_status, status.pos); + report.id = m_id; + report.report = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_read_version, m_id) { errorcode = m_module->read_version(ack->ack); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_read_status, m_id) { errorcode = m_module->read_status(ack->ack); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_read_detailed_status, m_id) { errorcode = m_module->read_detailed_status(ack->ack); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_set_run_param, m_id) { errorcode = m_module->set_run_param(cmd->param); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_set_basic_param, m_id) { errorcode = m_module->set_basic_param(cmd->param); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_set_warning_limit_param, m_id) { errorcode = m_module->set_warning_limit_param(cmd->param); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_get_run_param, m_id) { errorcode = m_module->get_run_param(ack->ack); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_get_basic_param, m_id) { errorcode = m_module->get_basic_param(ack->ack); } + END_PP(); + + PROCESS_PACKET(kcmd_mini_servo_ctrl_get_warning_limit_param, m_id) { errorcode = m_module->get_warning_limit_param(ack->ack); } + END_PP(); + +} diff --git a/components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp b/components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp new file mode 100644 index 0000000..b640068 --- /dev/null +++ b/components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp @@ -0,0 +1,16 @@ +#pragma once +#include "sdk/components/zprotocols/zcancmder/api/i_mini_servo_module.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" +namespace iflytop { +class ZCanMiniServoCtrlModule : public ZCanCmderListener { + ZCanCmder* m_cancmder = nullptr; + int m_id = 0; + I_MiniServoModule* m_module; + + uint8_t m_txbuf[128] = {0}; + + public: + void initialize(ZCanCmder* cancmder, int id, I_MiniServoModule* module); + virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); +}; +} // namespace iflytop diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index ca80d7f..804b964 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit ca80d7fd03f22edaac9927a64e31946323c87dd5 +Subproject commit 804b964be4e24a8fc929794768db6ffb5eeaa9a6