diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 24d4620..e42ef3a 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -13,6 +13,18 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g m_lock.init(); m_statu_lock.init(); m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); + + run_param_t run_param; + run_to_zero_param_t run_to_zero_param; + warning_limit_param_t warning_limit_param; + + read_run_param(run_param); + read_run_to_zero_param(run_to_zero_param); + read_warning_limit_param(warning_limit_param); + + set_run_param(run_param); + set_run_to_zero_param(run_to_zero_param); + set_warning_limit_param(warning_limit_param); } int32_t StepMotorCtrlModule::move_to(int32_t tox, function status_cb) { // @@ -191,11 +203,15 @@ int32_t StepMotorCtrlModule::read_version(version_t& version) { return 0; } int32_t StepMotorCtrlModule::read_status(status_t& status) { zlock_guard l(m_statu_lock); status.status = m_status; + if (m_Xgpio) status.io_state |= m_Xgpio->getState() ? 0x01 : 0x00; + if (m_end_gpio) status.io_state |= m_end_gpio->getState() ? 0x02 : 0x00; getnowpos(status.x); return 0; } int32_t StepMotorCtrlModule::read_debug_info(debug_info_t& debug_info) { debug_info.status = m_status; + if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00; + if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00; getnowpos(debug_info.x); return 0; } @@ -277,7 +293,18 @@ int32_t StepMotorCtrlModule::set_warning_limit_param(uint8_t operation, const wa } return 0; } +int32_t StepMotorCtrlModule::exec_move_to_zero_task(int32_t& dx) { + int32_t xnow; + getnowpos(xnow); + int32_t ret = exec_move_to_zero_task(); + + int32_t xnow2; + getnowpos(xnow2); + + dx = xnow2 - xnow; + return ret; +} int32_t StepMotorCtrlModule::exec_move_to_zero_task() { /******************************************************************************* * 远离X零点 * @@ -377,3 +404,7 @@ void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { x -= m_zero_shift_x; motor_pos = x; } +void StepMotorCtrlModule::updateStatus(uint8_t status) { + zlock_guard lock(m_statu_lock); + m_status = status; +} \ No newline at end of file diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index d277dee..31fa999 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -64,6 +64,14 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override; virtual int32_t set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) override; + int32_t read_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_read, param, param); } + int32_t read_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_read, param, param); } + int32_t read_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_read, param, param); } + + int32_t set_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_set, param, param); } + int32_t set_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_set, param, param); } + int32_t set_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_set, param, param); } + private: void updateStatus(uint8_t status); diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 5e5d162..3cf77a4 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -194,12 +194,17 @@ int32_t XYRobotCtrlModule::read_status(status_t& status) { zlock_guard lock(m_statu_lock); status = {0}; status.status = m_status; + if (m_Xgpio) status.iostate |= m_Xgpio->getState() ? 0x01 : 0x00; + if (m_Ygpio) status.iostate |= m_Ygpio->getState() ? 0x02 : 0x00; getnowpos(status.x, status.y); return 0; } int32_t XYRobotCtrlModule::read_debug_info(debug_info_t& debug_info) { zlock_guard lock(m_lock); - debug_info = {0}; + debug_info = {0}; + if (m_Xgpio) debug_info.iostate |= m_Xgpio->getState() ? 0x01 : 0x00; + if (m_Ygpio) debug_info.iostate |= m_Ygpio->getState() ? 0x02 : 0x00; + debug_info.status = m_status; getnowpos(debug_info.x, debug_info.y); return 0; diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp new file mode 100644 index 0000000..23459ca --- /dev/null +++ b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp @@ -0,0 +1,118 @@ +#include "zcan_step_motor_ctrl_module.hpp" +using namespace iflytop; + +#define TAG "ZCanStepMotorCtrlModule" +#if 1 +void ZCanStepMotorCtrlModule::initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* module) { + m_cancmder = cancmder; + m_id = id; + m_module = module; + cancmder->registerListener(this); +} +void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { +// printf("ZCanStepMotorCtrlModule::onRceivePacket %d %d\n", rxcmd->get_cmdheader()->cmdid, rxcmd->get_cmdheader()->subcmdid); + + PROCESS_PACKET(kcmd_step_motor_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_stop, m_id) { errorcode = m_module->stop(cmd->stop_type); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero, m_id) { + errorcode = m_module->move_to_zero([this, cmdheader](I_StepMotorCtrlModule::move_to_zero_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_step_motor_ctrl_move_to_zero_report_t report = {0}; + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero exec_status:%d", status.exec_status); + + report.id = m_id; + report.status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, m_id) { + errorcode = m_module->move_to_zero_with_calibrate(cmd->nowx, [this, cmdheader](I_StepMotorCtrlModule::move_to_zero_with_calibrate_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_step_motor_ctrl_move_to_zero_with_calibrate_report_t report = {0}; + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero_with_calibrate exec_status:%d %d", status.exec_status, status.zero_shift_x); + + report.id = m_id; + report.status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_move_to, m_id) { + errorcode = m_module->move_to(cmd->x, [this, cmdheader](I_StepMotorCtrlModule::move_to_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_step_motor_ctrl_move_to_report_t report = {0}; + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to exec_status:%d %d", status.exec_status, status.tox); + + report.id = m_id; + report.status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_move_by, m_id) { + errorcode = m_module->move_by(cmd->dx, [this, cmdheader](I_StepMotorCtrlModule::move_by_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_step_motor_ctrl_move_by_report_t report = {0}; + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_by exec_status:%d %d", status.exec_status, status.dx); + + report.id = m_id; + report.status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_force_change_current_pos, m_id) { errorcode = m_module->force_change_current_pos(cmd->x); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_rotate, m_id) { + errorcode = m_module->rotate(cmd->speed, cmd->run_time, [this, cmdheader](I_StepMotorCtrlModule::rotate_cb_status_t& status) { + osDelay(5); // 用来保证回执消息在前面 + kcmd_step_motor_ctrl_rotate_report_t report = {0}; + ZLOGI(TAG, "kcmd_step_motor_ctrl_rotate exec_status:%d %d", status.exec_status, status.lastforms); + + report.id = m_id; + report.status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_read_version, m_id) { // + errorcode = m_module->read_version(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_read_status, m_id) { // + errorcode = m_module->read_status(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_read_debug_info, m_id) { // + errorcode = m_module->read_debug_info(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_param, m_id) { // + errorcode = m_module->set_run_param(cmd->opt_type, cmd->param, ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_set_warning_limit_param, m_id) { // + errorcode = m_module->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_set_run_to_zero_param, m_id) { // + errorcode = m_module->set_run_to_zero_param(cmd->opt_type, cmd->param, ack->ack); + } + END_PP(); +} +#endif \ No newline at end of file diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp new file mode 100644 index 0000000..a65ad4b --- /dev/null +++ b/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp @@ -0,0 +1,16 @@ +#pragma once +#include "sdk/components/zprotocols/zcancmder/api/i_step_motor_ctrl_module.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" +namespace iflytop { +class ZCanStepMotorCtrlModule : public ZCanCmderListener { + ZCanCmder* m_cancmder = nullptr; + int m_id = 0; + I_StepMotorCtrlModule* m_module; + + uint8_t m_txbuf[128] = {0}; + + public: + void initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* m_module); + virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); +}; +} // namespace iflytop diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index ba0b504..614d8b8 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit ba0b50468b776c0bf4182300d80d5d973ebea058 +Subproject commit 614d8b811b4737c5b2e08e9abc3e10a5fd6cb11c