From 078b5d840c55e0ad5dcf64345d54411898983c63 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 27 Sep 2023 21:25:20 +0800 Subject: [PATCH] update --- .../step_motor_ctrl_module.cpp | 334 +++++++++++++++++++++ .../step_motor_ctrl_module.hpp | 95 ++++++ components/zcancmder/cmd/basic_bean.hpp | 85 ++++-- components/zcancmder/cmd/c1006_module_cmd.hpp | 102 ++++--- components/zcancmder/cmd/c1007_module_cmd.hpp | 80 +++++ .../zcancmder_module/zcan_xy_robot_module.cpp | 16 - 6 files changed, 625 insertions(+), 87 deletions(-) create mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_module.cpp create mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_module.hpp create mode 100644 components/zcancmder/cmd/c1007_module_cmd.hpp diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp new file mode 100644 index 0000000..d033449 --- /dev/null +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -0,0 +1,334 @@ +#include "step_motor_ctrl_module.hpp" + +#include "sdk/components/errorcode/errorcode.hpp" + +using namespace iflytop; +#define TAG "StepMotorCtrlModule" +void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_gpio, ZGPIO* end_gpio) { + m_id = id; + m_stepM1 = stepM; + m_Xgpio = zero_gpio; + m_end_gpio = end_gpio; + + m_lock.init(); + m_statu_lock.init(); + m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); +} + +int32_t StepMotorCtrlModule::set_acc(int32_t acc) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_acc %d", m_id, acc); + m_cfg_acc = acc; + return 0; +} +int32_t StepMotorCtrlModule::set_dec(int32_t dec) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_dec %d", m_id, dec); + m_cfg_dec = dec; + return 0; +} +int32_t StepMotorCtrlModule::set_break_dec(int32_t dec) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_break_dec %d", m_id, dec); + m_cfg_break_dec = dec; + return 0; +} +int32_t StepMotorCtrlModule::set_speed(int32_t speed) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_speed %d", m_id, speed); + m_cfg_velocity = speed; + return 0; +} +#if 0 +int32_t StepMotorCtrlModule::set_zero_robottype(RobotType_t robot_type) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_zero_robottype %d", m_id, robot_type); + m_robot_type = robot_type; + return 0; +} +#endif +int32_t StepMotorCtrlModule::set_shaft(bool shaft) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_shaft %d", m_id, shaft); + m_x_shaft = shaft; + m_stepM1->setMotorShaft(shaft); + return 0; +} +int32_t StepMotorCtrlModule::set_zero_shift(int32_t shift) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_zero_shift %d", m_id, shift); + m_zero_shift_x = shift; + return 0; +} +int32_t StepMotorCtrlModule::set_runtozero_max_distance(int32_t distance) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_runtozero_max_distance %d", m_id, distance); + m_cfg_runtozero_maxX = distance; + return 0; +} +int32_t StepMotorCtrlModule::set_runtozero_speed(int32_t speed) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_runtozero_speed %d", m_id, speed); + m_cfg_runtozero_speed = speed; + return 0; +} +int32_t StepMotorCtrlModule::set_runtozero_dec(int32_t dec) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_runtozero_dec %d", m_id, dec); + m_cfg_runtozero_dec = dec; + return 0; +} +int32_t StepMotorCtrlModule::set_runtozero_leave_away_zero_distance(int32_t distance) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_runtozero_leave_away_zero_distance %d", m_id, distance); + m_cfg_runtozero_leave_away_zero_max_distance = distance; + return 0; +} +int32_t StepMotorCtrlModule::set_distance_scale(float distance_scale) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_distance_scale %f", m_id, distance_scale); + m_cfg_distance_scale = distance_scale; + m_stepM1->setScale(distance_scale); + return 0; +} +int32_t StepMotorCtrlModule::set_current_pos(int32_t xpos) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_current_pos %d", m_id, xpos); + int32_t motor_pos = 0; + forward_kinematics(xpos, motor_pos); + m_stepM1->setXACTUAL(motor_pos); + return 0; +} +int32_t StepMotorCtrlModule::set_ihold_irun_iholddelay(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d set_ihold_irun_iholddelay %d %d %d", m_id, ihold, irun, iholddelay); + m_stepM1->setIHOLD_IRUN(ihold, irun, iholddelay); + return 0; +} + +void StepMotorCtrlModule::dumpcfg(const char* tag) {} + +int32_t StepMotorCtrlModule::enable(bool venable) { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d enable %d", m_id, venable); + m_stepM1->enable(venable); + return 0; +} +int32_t StepMotorCtrlModule::stop() { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d stop", m_id); + m_thread.stop(); + m_stepM1->stop(); + return 0; +} +int32_t StepMotorCtrlModule::brake() { + zlock_guard l(m_lock); + ZLOGI(TAG, "m%d brake", m_id); + m_stepM1->setDeceleration(m_cfg_break_dec); + m_stepM1->stop(); + m_thread.stop(); + m_stepM1->stop(); + return 0; +} + +/******************************************************************************* + * ACTION * + *******************************************************************************/ +int32_t StepMotorCtrlModule::move_to(int32_t xnow, function status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d move_to %d", m_id, xnow); + m_thread.stop(); + + updateStatus(1); + m_thread.start([this, xnow, status_cb]() { + _motor_move_to(xnow, m_cfg_velocity, m_cfg_acc, m_cfg_dec); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(10); + } + _motor_stop(); + + int32_t xnow = 0; + getnowpos(xnow); + if (status_cb) status_cb(0, xnow); + updateStatus(0); + }); + return 0; +} +int32_t StepMotorCtrlModule::move_by(int32_t dpos, function status_cb) { // + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d move_by %d", m_id, dpos); + m_thread.stop(); + + int32_t xnow = 0; + getnowpos(xnow); + + int32_t toxnow = xnow + dpos; + move_to(toxnow, status_cb); + return 0; +} +int32_t StepMotorCtrlModule::move_to_zero(function status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "m%d move_to_zero", m_id); + m_thread.stop(); + + return 0; +} +int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, function status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "move_to_zero_with_calibrate x:%d", nowx); + m_thread.stop(); + updateStatus(1); + + m_thread.start([this, nowx, status_cb]() { + int32_t dx; + int32_t erro = exec_move_to_zero_task(dx); + if (erro != 0) { + ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro); + _motor_stop(); + if (status_cb) status_cb(erro, 0); + updateStatus(0); + return; + } + + int32_t calibrate_x, calibrate_y; + calibrate_x = dx + nowx; + + m_zero_shift_x = calibrate_x; + + m_stepM1->setXACTUAL(0); + if (status_cb) status_cb(erro, m_zero_shift_x); + updateStatus(0); + return; + }); + + return 0; +} +int32_t StepMotorCtrlModule::rotate(uint8_t direction, int32_t velocity, int32_t acc) { return 0; } + +/******************************************************************************* + * READ * + *******************************************************************************/ + +int32_t StepMotorCtrlModule::read_status(uint8_t* module_statu, int32_t* pos, int32_t* velocity) { + zlock_guard lock(m_statu_lock); + *module_statu = m_status; + getnowpos(*pos); + *velocity = m_stepM1->getVACTUAL(); + return 0; +} +int32_t StepMotorCtrlModule::get_zero_shift(int32_t* pos) { + zlock_guard lock(m_lock); + *pos = m_zero_shift_x; + return 0; +} + +/******************************************************************************* + * PRIVATE * + *******************************************************************************/ +void StepMotorCtrlModule::updateStatus(uint8_t status) { + zlock_guard lock(m_statu_lock); + m_status = status; +} +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零点 * + *******************************************************************************/ + if (m_Xgpio->getState()) { + /** + * @brief 如果设备已经在零点，则反向移动一定距离远离零点 + */ + _motor_move_by(m_cfg_runtozero_leave_away_zero_max_distance, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + while (!_motor_is_reach_target()) { + if (m_thread.getExitFlag()) break; + vTaskDelay(1); + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return 0; + } + if (m_Xgpio->getState()) { + ZLOGE(TAG, "leave away zero failed"); + return err::kcommon_error_x_leave_away_zero_point_fail; + } + } + /******************************************************************************* + * 移动X轴到零点 * + *******************************************************************************/ + _motor_move_by(-m_cfg_runtozero_maxX, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + bool xreach_zero = false; + while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { + // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); + if (m_Xgpio->getState()) { + xreach_zero = true; + _motor_stop(-1); + break; + } + vTaskDelay(1); + } + + if (!xreach_zero) { + // 触发异常回调 + ZLOGE(TAG, "x reach zero failed"); + return err::kcommon_error_not_found_x_zero_point; + } + + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return 0; + } + + ZLOGI(TAG, "x reach zero"); + ZLOGI(TAG, "move_to_zero success"); + + return 0; +} + +void StepMotorCtrlModule::getnowpos(int32_t& pos) { + int32_t motor_pos = m_stepM1->getXACTUAL(); + forward_kinematics(motor_pos, pos); +} +void StepMotorCtrlModule::_motor_move_to(int32_t pos, int32_t maxv, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _motor_move_to %d maxv:%d acc:%d dec:%d", m_id, pos, maxv, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + m_stepM1->moveTo(pos, maxv); +} +void StepMotorCtrlModule::_motor_move_by(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec) { + ZLOGI(TAG, "m%d _motor_move_by %d maxv:%d acc:%d dec:%d", m_id, dpos, maxv, acc, dec); + m_stepM1->setAcceleration(acc); + m_stepM1->setDeceleration(dec); + m_stepM1->moveBy(dpos, maxv); +} +void StepMotorCtrlModule::_motor_stop(int32_t dec = -1) { + ZLOGI(TAG, "m%d _motor_stop %d", m_id, dec); + if (dec > 0) { + m_stepM1->setDeceleration(dec); + } + m_stepM1->stop(); +} +bool StepMotorCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget(); } +void StepMotorCtrlModule::inverse_kinematics(int32_t motor_pos, int32_t& x) { + // m_zero_shift_x + x = motor_pos; + if (m_x_shaft) x = -x; + x += m_zero_shift_x; +} +void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { + if (m_x_shaft) x = -x; + x -= m_zero_shift_x; + motor_pos = x; +} \ 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 new file mode 100644 index 0000000..31fe6dc --- /dev/null +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -0,0 +1,95 @@ +#pragma once +// +#include "sdk/os/zos.hpp" +#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" + +namespace iflytop { +class StepMotorCtrlModule { + public: + private: + IStepperMotor* m_stepM1; + int m_id = 0; + + ZGPIO* m_Xgpio; + ZGPIO* m_end_gpio; + + ZThread m_thread; + + uint8_t m_status = 0; + + int32_t m_zero_shift_x = 0; + + zmutex m_lock; + zmutex m_statu_lock; + + int32_t m_cfg_acc = 300000; + int32_t m_cfg_dec = 300000; + int32_t m_cfg_break_dec = 300000; + int32_t m_cfg_velocity = 300000; + + int32_t m_cfg_runtozero_maxX = 5120000; + int32_t m_cfg_runtozero_speed = 30000; + int32_t m_cfg_runtozero_dec = 600000; + int32_t m_cfg_runtozero_leave_away_zero_max_distance = 51200; + float m_cfg_distance_scale = 1.0f; + + bool m_x_shaft = false; + + public: + void initialize(int id, IStepperMotor* stepM, ZGPIO* zero_gpio, ZGPIO* end_gpio); + + int32_t set_current_pos(int32_t pos); + int32_t set_distance_scale(float distance_scale); + int32_t set_ihold_irun_iholddelay(uint8_t ihold, uint8_t irun, uint16_t iholddelay); + int32_t set_acc(int32_t acc); + int32_t set_dec(int32_t dec); + int32_t set_speed(int32_t speed); + int32_t set_shaft(bool shaft); + int32_t set_runtozero_max_distance(int32_t distance); + int32_t set_runtozero_speed(int32_t speed); + int32_t set_runtozero_dec(int32_t dec); + int32_t set_runtozero_leave_away_zero_distance(int32_t distance); + int32_t set_zero_shift(int32_t shift); + int32_t set_break_dec(int32_t dec); + + void dumpcfg(const char* tag); + + int32_t enable(bool venable); + int32_t stop(); + int32_t brake(); + + /******************************************************************************* + * ACTION * + *******************************************************************************/ + int32_t move_to(int32_t pos, function status_cb); + int32_t move_by(int32_t dpos, function status_cb); + int32_t move_to_zero(function status_cb); + int32_t move_to_zero_with_calibrate(int32_t nowpos, function status_cb); + int32_t rotate(uint8_t direction, int32_t velocity, int32_t acc); + + /******************************************************************************* + * READ * + *******************************************************************************/ + + int32_t read_status(uint8_t* module_statu, int32_t* pos, int32_t* velocity); + int32_t read_gpio_status(bool* zeroio, bool* endio); + int32_t get_zero_shift(int32_t* pos); + + private: + void updateStatus(uint8_t status); + + void getnowpos(int32_t& pos); + + int32_t exec_move_to_zero_task(int32_t& dx); + int32_t exec_move_to_zero_task(); + + void _motor_move_to(int32_t pos, int32_t maxv, int32_t acc, int32_t dec); + void _motor_move_by(int32_t dpos, int32_t maxv, int32_t acc, int32_t dec); + void _motor_stop(int32_t dec = -1); + bool _motor_is_reach_target(); + + void inverse_kinematics(int32_t motor_pos, int32_t& x); + void forward_kinematics(int32_t x, int32_t& motor_pos); +}; +} // namespace iflytop \ No newline at end of file diff --git a/components/zcancmder/cmd/basic_bean.hpp b/components/zcancmder/cmd/basic_bean.hpp index 95b7fd5..98890f7 100644 --- a/components/zcancmder/cmd/basic_bean.hpp +++ b/components/zcancmder/cmd/basic_bean.hpp @@ -27,6 +27,19 @@ return; \ } +typedef uint8_t u8; +typedef uint16_t u16; +typedef uint32_t u32; +typedef uint64_t u64; + +typedef int8_t s8; +typedef int16_t s16; +typedef int32_t s32; +typedef int64_t s64; + +typedef float f32; +typedef double f64; + /******************************************************************************* * STRUCT * *******************************************************************************/ @@ -67,13 +80,18 @@ typedef enum { /******************************************************************************* * 1006 * *******************************************************************************/ - kcmd_xy_robot_ctrl_enable = CMDID(1006, 0), // 机器人使能 - kcmd_xy_robot_ctrl_stop = CMDID(1006, 1), // 机器人停止 - kcmd_xy_robot_ctrl_move_to_zero = CMDID(1006, 2), // 机器人回零 - kcmd_xy_robot_ctrl_move_to_zero_with_calibrate = CMDID(1006, 3), // 机器人回零并标定 - kcmd_xy_robot_ctrl_move_to = CMDID(1006, 4), // 机器人移动到指定位置 - kcmd_xy_robot_ctrl_move_by = CMDID(1006, 5), // 机器人移动指定距离 - kcmd_xy_robot_ctrl_read_status = CMDID(1006, 50), // 读取机器人状态 + kcmd_xy_robot_ctrl_enable = CMDID(1006, 0), // 机器人使能 + kcmd_xy_robot_ctrl_stop = CMDID(1006, 1), // 机器人停止 + kcmd_xy_robot_ctrl_move_to_zero = CMDID(1006, 2), // 机器人回零 + kcmd_xy_robot_ctrl_move_to_zero_with_calibrate = CMDID(1006, 3), // 机器人回零并标定 + kcmd_xy_robot_ctrl_move_to = CMDID(1006, 4), // 机器人移动到指定位置 + kcmd_xy_robot_ctrl_move_by = CMDID(1006, 5), // 机器人移动指定距离 + kcmd_xy_robot_ctrl___p1 = CMDID(1006, 6), // 占位指令 + kcmd_xy_robot_ctrl_break = CMDID(1006, 7), // 机器人刹车 + + kcmd_xy_robot_ctrl_read_status = CMDID(1006, 50), // 读取机器人状态 + kcmd_xy_robot_ctrl_read_io = CMDID(1006, 51), // 读取机器人IO + kcmd_xy_robot_ctrl_set_robottype = CMDID(1006, 100), // 机器人类型 kcmd_xy_robot_ctrl_set_current_pos = CMDID(1006, 101), // 设置当前位置 kcmd_xy_robot_ctrl_set_distance_scale = CMDID(1006, 102), // 设置距离比例 @@ -86,31 +104,42 @@ typedef enum { kcmd_xy_robot_ctrl_set_runtozero_speed = CMDID(1006, 109), // 设置回零速度 kcmd_xy_robot_ctrl_set_runtozero_dec = CMDID(1006, 110), // 设置回零减速度 kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_distance = CMDID(1006, 111), // 设置离零点距离 + kcmd_xy_robot_ctrl_set_zero_shift = CMDID(1006, 112), // 设置零点偏移 + kcmd_xy_robot_ctrl_set_break_dec = CMDID(1006, 113), // 设置刹车减速度 -#if 1 - // /******************************************************************************* * 1007:单轴步进电机控制 * *******************************************************************************/ -// kcmd_step_motor_ctrl_enable -// kcmd_step_motor_ctrl_stop -// kcmd_step_motor_ctrl_move_to_zero -// kcmd_step_motor_ctrl_move_to_zero_with_calibrate -// kcmd_step_motor_ctrl_move_to -// kcmd_step_motor_ctrl_move_by -// kcmd_step_motor_ctrl_read_status -// kcmd_step_motor_ctrl_set_robottype -// kcmd_step_motor_ctrl_set_current_pos -// kcmd_step_motor_ctrl_set_distance_scale -// kcmd_step_motor_ctrl_set_ihold_irun_iholddelay -// kcmd_step_motor_ctrl_set_acc -// kcmd_step_motor_ctrl_set_dec -// kcmd_step_motor_ctrl_set_speed -// kcmd_step_motor_ctrl_set_shaft -// kcmd_step_motor_ctrl_set_runtozero_max_distance -// kcmd_step_motor_ctrl_set_runtozero_speed -// kcmd_step_motor_ctrl_set_runtozero_dec -// kcmd_step_motor_ctrl_set_runtozero_leave_away_zero_distance + + kcmd_step_motor_ctrl_enable = CMDID(1007, 0), // 步进电机使能 + kcmd_step_motor_ctrl_stop = CMDID(1007, 1), // 步进电机停止 + kcmd_step_motor_ctrl_move_to_zero = CMDID(1007, 2), // 步进电机回零 + kcmd_step_motor_ctrl_move_to_zero_with_calibrate = CMDID(1007, 3), // 步进电机回零并标定 + kcmd_step_motor_ctrl_move_to = CMDID(1007, 4), // 步进电机移动到指定位置 + kcmd_step_motor_ctrl_move_by = CMDID(1007, 5), // 步进电机移动指定距离 + kcmd_step_motor_ctrl_rotate = CMDID(1007, 6), // 步进电机rotate + kcmd_step_motor_ctrl_break = CMDID(1007, 7), // 步进电机刹车 + + kcmd_step_motor_ctrl_read_status = CMDID(1007, 50), // 读取机器人状态 + kcmd_step_motor_ctrl_read_io = CMDID(1007, 51), // 读取机器人IO + + kcmd_step_motor_ctrl___p1 = CMDID(1007, 100), // 占位指令 + kcmd_step_motor_ctrl_set_current_pos = CMDID(1007, 101), // 设置当前位置 + kcmd_step_motor_ctrl_set_distance_scale = CMDID(1007, 102), // 设置距离比例 + kcmd_step_motor_ctrl_set_ihold_irun_iholddelay = CMDID(1007, 103), // 设置电流 + kcmd_step_motor_ctrl_set_acc = CMDID(1007, 104), // 设置加速度 + kcmd_step_motor_ctrl_set_dec = CMDID(1007, 105), // 设置减速度 + kcmd_step_motor_ctrl_set_speed = CMDID(1007, 106), // 设置位置模式速度 + kcmd_step_motor_ctrl_set_shaft = CMDID(1007, 107), // 设置轴是否反向 + kcmd_step_motor_ctrl_set_runtozero_max_distance = CMDID(1007, 108), // 设置回零最大距离 + kcmd_step_motor_ctrl_set_runtozero_speed = CMDID(1007, 109), // 设置回零速度 + kcmd_step_motor_ctrl_set_runtozero_dec = CMDID(1007, 110), // 设置回零减速度 + kcmd_step_motor_ctrl_set_runtozero_leave_away_zero_distance = CMDID(1007, 111), // 设置离零点距离 + kcmd_step_motor_ctrl_set_zero_shift = CMDID(1007, 112), // 设置零点偏移 + kcmd_step_motor_ctrl_set_break_dec = CMDID(1007, 113), // 设置断电减速度 + +#if 1 + #endif } CmdID_t; diff --git a/components/zcancmder/cmd/c1006_module_cmd.hpp b/components/zcancmder/cmd/c1006_module_cmd.hpp index 50c75e3..b42d630 100644 --- a/components/zcancmder/cmd/c1006_module_cmd.hpp +++ b/components/zcancmder/cmd/c1006_module_cmd.hpp @@ -12,57 +12,73 @@ namespace zcr { /******************************************************************************* * CMD * *******************************************************************************/ -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, cmd, uint8_t id; uint8_t enable;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, cmd, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, cmd, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, cmd, uint8_t id; uint8_t _pad; int32_t dx; int32_t dy;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, cmd, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_robottype, cmd, uint8_t id; uint8_t _pad; uint8_t zero_robottype;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_current_pos, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, cmd, uint8_t id; uint8_t _pad; float distance_scale;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_ihold_irun_iholddelay, cmd, uint8_t id; uint8_t _pad; uint8_t ihold; uint8_t irun; uint16_t iholddelay;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, cmd, uint8_t id; uint8_t _pad; int32_t acc;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, cmd, uint8_t id; uint8_t _pad; int32_t dec;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, cmd, uint8_t id; uint8_t _pad; int32_t speed;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_shaft, cmd, uint8_t id; uint8_t _pad; uint8_t x_shaft; uint8_t y_shaft;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_max_distance, cmd, uint8_t id; uint8_t _pad; int32_t maxX; int32_t maxY;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, cmd, uint8_t id; uint8_t _pad; int32_t speed;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, cmd, uint8_t id; uint8_t _pad; int32_t dec;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_distance, cmd, uint8_t id; uint8_t _pad; int32_t distance;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, /* */ cmd, u8 id; u8 enable;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, /* */ cmd, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, /* */ cmd, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, /* */ cmd, u8 id; u8 _pad; s32 x; s32 y;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, /* */ cmd, u8 id; u8 _pad; s32 x; s32 y;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, /* */ cmd, u8 id; u8 _pad; s32 dx; s32 dy;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl___p1, /* */ cmd, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_break, /* */ cmd, u8 id; u8 _pad;); + +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, /* */ cmd, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_io, /* */ cmd, u8 id; u8 _pad;); + +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_robottype, /* */ cmd, u8 id; u8 _pad; u8 zero_robottype;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_current_pos, /* */ cmd, u8 id; u8 _pad; s32 x; s32 y;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, /* */ cmd, u8 id; u8 _pad; f32 distance_scale;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_ihold_irun_iholddelay, /* */ cmd, u8 id; u8 _pad; u8 ihold; u8 irun; u16 iholddelay;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, /* */ cmd, u8 id; u8 _pad; s32 acc;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, /* */ cmd, u8 id; u8 _pad; s32 dec;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, /* */ cmd, u8 id; u8 _pad; s32 speed;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_shaft, /* */ cmd, u8 id; u8 _pad; u8 x_shaft; u8 y_shaft;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_max_distance, /* */ cmd, u8 id; u8 _pad; s32 maxX; s32 maxY;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, /* */ cmd, u8 id; u8 _pad; s32 speed;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, /* */ cmd, u8 id; u8 _pad; s32 dec;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_distance, /* */ cmd, u8 id; u8 _pad; s32 distance;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_zero_shift, /* */ cmd, u8 id; u8 _pad; s32 x_shift; s32 y_shift;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_break_dec, /* */ cmd, u8 id; u8 _pad; s32 dec;); /******************************************************************************* * ACK * *******************************************************************************/ -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, ack, uint8_t id; uint8_t module_statu; int32_t x; int32_t y;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_robottype, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_current_pos, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_ihold_irun_iholddelay, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_shaft, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_max_distance, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, ack, uint8_t id; uint8_t _pad;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_distance, ack, uint8_t id; uint8_t _pad;); + +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl___p1, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_break, /* */ ack, u8 id; u8 _pad;); + +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, /* */ ack, u8 id; u8 _pad; u8 module_statu; s32 x; s32 y;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_io, /* */ ack, u8 id; u8 _pad; u8 xzero, yzero; u8 xend, yend;); + +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_robottype, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_current_pos, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_ihold_irun_iholddelay, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_shaft, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_max_distance, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_distance, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_zero_shift, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_break_dec, /* */ ack, u8 id; u8 _pad;); /******************************************************************************* * status_report * *******************************************************************************/ -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t dx; int32_t dy;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t zero_shift_x; - int32_t zero_shift_y;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t tox; int32_t toy;); -ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t tox; int32_t toy;); + +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 dx; s32 dy;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 zero_shift_x; + s32 zero_shift_y;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 tox; s32 toy;); +ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 tox; s32 toy;); #pragma pack(pop) diff --git a/components/zcancmder/cmd/c1007_module_cmd.hpp b/components/zcancmder/cmd/c1007_module_cmd.hpp new file mode 100644 index 0000000..fd7aa6f --- /dev/null +++ b/components/zcancmder/cmd/c1007_module_cmd.hpp @@ -0,0 +1,80 @@ +#pragma once +#include + +#include "basic_bean.hpp" +namespace iflytop { +namespace zcr { + +/******************************************************************************* + * 1007 * + *******************************************************************************/ +#pragma pack(push, 1) +ZPACKET_STRUCT(kcmd_step_motor_ctrl_enable, /* */ cmd, u8 id; u8 enable;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_stop, /* */ cmd, u8 id;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to_zero, /* */ cmd, u8 id;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, /* */ cmd, u8 id;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to, /* */ cmd, u8 id; s32 pos;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_by, /* */ cmd, u8 id; s32 distance;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_rotate, /* */ cmd, u8 id; s32 angle;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_break, /* */ cmd, u8 id;); + +ZPACKET_STRUCT(kcmd_step_motor_ctrl_read_status, /* */ cmd, u8 id;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_read_io, /* */ cmd, u8 id;); + +ZPACKET_STRUCT(kcmd_step_motor_ctrl___p1, /* */ cmd, u8 id;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_current_pos, /* */ cmd, u8 id; s32 pos;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_distance_scale, /* */ cmd, u8 id; s32 scale;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_ihold_irun_iholddelay, /* */ cmd, u8 id; u8 ihold; u8 irun; u8 iholddelay;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_acc, /* */ cmd, u8 id; s32 acc;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_dec, /* */ cmd, u8 id; s32 dec;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_speed, /* */ cmd, u8 id; s32 speed;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_shaft, /* */ cmd, u8 id; u8 shaft;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_max_distance, /* */ cmd, u8 id; s32 max_distance;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_speed, /* */ cmd, u8 id; s32 speed;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_dec, /* */ cmd, u8 id; s32 dec;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_leave_away_zero_distance, /* */ cmd, u8 id; s32 distance;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_zero_shift, /* */ cmd, u8 id; s32 x_shift; s32 y_shift;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_break_dec, /* */ cmd, u8 id; s32 dec;); + +/******************************************************************************* + * ACK * + *******************************************************************************/ +ZPACKET_STRUCT(kcmd_step_motor_ctrl_enable, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_stop, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to_zero, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_by, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_rotate, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_break, /* */ ack, u8 id; u8 _pad;); + +ZPACKET_STRUCT(kcmd_step_motor_ctrl_read_status, /* */ ack, u8 id; u8 _pad; u8 module_statu; s32 pos; s32 velocity;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_read_io, /* */ ack, u8 id; u8 _pad; u8 zeroio; u8 endio;); + +ZPACKET_STRUCT(kcmd_step_motor_ctrl___p1, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_current_pos, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_distance_scale, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_ihold_irun_iholddelay, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_acc, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_dec, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_speed, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_shaft, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_max_distance, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_speed, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_dec, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_runtozero_leave_away_zero_distance, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_zero_shift, /* */ ack, u8 id; u8 _pad;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_set_break_dec, /* */ ack, u8 id; u8 _pad;); + +/******************************************************************************* + * status_report * + *******************************************************************************/ +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to_zero, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 ddistance;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 zero_shift;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_to, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 npos;); +ZPACKET_STRUCT(kcmd_step_motor_ctrl_move_by, /* */ status_report, u8 id; u8 _pad; s32 exec_status; s32 ddistance;); + +#pragma pack(pop) + +} // namespace zcr +} // namespace iflytop diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp index 91d00bb..8b6bb1f 100644 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ b/components/zcancmder_module/zcan_xy_robot_module.cpp @@ -9,22 +9,6 @@ void ZCANXYRobotCtrlModule::initialize(ZCanCmder* cancmder, int id, XYRobotCtrlM m_xyRobotCtrlModule = xyRobotCtrlModule; cancmder->registerListener(this); } -#if 0 - kcmd_xy_robot_ctrl_read_status = CMDID(1006, 50), // 读取机器人状态 - kcmd_xy_robot_ctrl_set_robottype = CMDID(1006, 100), // 机器人类型 - kcmd_xy_robot_ctrl_set_current_pos = CMDID(1006, 101), // 设置当前位置 - kcmd_xy_robot_ctrl_set_distance_scale = CMDID(1006, 102), // 设置距离比例 - kcmd_xy_robot_ctrl_set_ihold_irun_iholddelay = CMDID(1006, 103), // 设置电流 - kcmd_xy_robot_ctrl_set_acc = CMDID(1006, 104), // 设置加速度 - kcmd_xy_robot_ctrl_set_dec = CMDID(1006, 105), // 设置减速度 - kcmd_xy_robot_ctrl_set_speed = CMDID(1006, 106), // 设置位置模式速度 - kcmd_xy_robot_ctrl_set_shaft = CMDID(1006, 107), // 设置轴是否反向 - kcmd_xy_robot_ctrl_set_runtozero_max_distance = CMDID(1006, 108), // 设置回零最大距离 - kcmd_xy_robot_ctrl_set_runtozero_speed = CMDID(1006, 109), // 设置回零速度 - kcmd_xy_robot_ctrl_set_runtozero_dec = CMDID(1006, 110), // 设置回零减速度 - kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_distance = CMDID(1006, 111), // 设置离零点距离 -#endif - void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { printf("ZCANXYRobotCtrlModule::onRceivePacket %d %d\n", rxcmd->get_cmdheader()->cmdid, rxcmd->get_cmdheader()->subcmdid); PROCESS_PACKET(kcmd_xy_robot_ctrl_enable, m_id) { //