#include "X_V2.h" #include "step_motor.h" /********************************************************** *** X_V2步进闭环控制例程 *** 编写作者：ZHANGDATOU *** 技术支持：张大头闭环伺服 *** 淘宝店铺：https://zhangdatou.taobao.com *** CSDN博客：http s://blog.csdn.net/zhangdatou666 *** qq交流群：262438510 **********************************************************/ uint16_t MMCL_count = 0, MMCL_cmd[MMCL_LEN] = {0}; /********************************************************** *** 触发动作命令 **********************************************************/ /** * @brief 触发编码器校准 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Trig_Encoder_Cal(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x06; // 功能码 cmd[2] = 0x45; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr, cmd, 4); } /** * @brief 重启电机（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Reset_Motor(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x08; // 功能码 cmd[2] = 0x97; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr, cmd, 4); } /** * @brief 将当前位置清零 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Reset_CurPos_To_Zero(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0A; // 功能码 cmd[2] = 0x6D; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr, cmd, 4); } /** * @brief 解除堵转保护 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Reset_Clog_Pro(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0E; // 功能码 cmd[2] = 0x52; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr, cmd, 4); } /** * @brief 恢复出厂设置 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Restore_Motor(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0F; // 功能码 cmd[2] = 0x5F; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr, cmd, 4); } /********************************************************** *** 运动控制命令 **********************************************************/ /** * @brief 多电机命令（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Multi_Motor_Cmd(uint8_t addr) { uint16_t i = 0, j = 0, len = 0; static uint8_t cmd[MMCL_LEN] = {0}; // 多电机命令长度大于0 if(MMCL_count > 0) { // 多电机命令的总字节数 len = MMCL_count + 5; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xAA; // 功能码 cmd[2] = (uint8_t)(len >> 8); // 总字节数高8位 cmd[3] = (uint8_t)(len); // 总字节数低8位 for(i=0,j=4; i < MMCL_count; i++,j++) { cmd[j] = MMCL_cmd[i]; } cmd[j] = 0x6B; ++j; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, j); MMCL_count = 0; } else { MMCL_count = 0; } } /** * @brief 使能信号控制 * @param addr ：电机地址 * @param state ：使能状态 ，true为使能电机，false为关闭电机 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_En_Control(uint8_t addr, bool state, bool snF) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xF3; // 功能码 cmd[2] = 0xAB; // 辅助码 cmd[3] = (uint8_t)state; // 使能状态 cmd[4] = snF; // 多机同步运动标志 cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 力矩模式 * @param addr ：电机地址 * @param sign ：符号（方向） ，0为正，1为负 * @param t_ramp ：电流斜率(Ma/s) ，范围0 - 65535Ma/s * @param torque ：力矩电流(Ma) ，范围0 - 6000Ma * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Torque_Control(uint8_t addr, uint8_t sign, uint16_t t_ramp, uint16_t torque, bool snF) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xF5; // 功能码 cmd[2] = sign; // 符号（方向） cmd[3] = (uint8_t)(t_ramp >> 8); // 电流斜率(Ma/s) cmd[4] = (uint8_t)(t_ramp >> 0); cmd[5] = (uint8_t)(torque >> 8); // 力矩电流(Ma) cmd[6] = (uint8_t)(torque >> 0); cmd[7] = snF; // 多机同步标志 cmd[8] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 9); } /** * @brief 力矩模式限速控制（Y42） * @param addr ：电机地址 * @param sign ：符号（方向） ，0为正，1为负 * @param t_ramp ：电流斜率(Ma/s) ，范围0 - 65535Ma/s * @param torque ：力矩电流(Ma) ，范围0 - 6000Ma * @param snF ：多机同步标志 ，false为不启用，true为启用 * @param maxVel ：最大速度(RPM) ，范围0.0 - 3000.0RPM * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Torque_LV_Control(uint8_t addr, uint8_t sign, uint16_t t_ramp, uint16_t torque, bool snF, float maxVel) { static uint8_t cmd[16] = {0}; uint16_t v = 0; // 将速度放大10倍发送过去 v = (uint16_t)ABS(maxVel * 10.0f); // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xC5; // 功能码 cmd[2] = sign; // 符号（方向） cmd[3] = (uint8_t)(t_ramp >> 8); // 电流斜率(Ma/s) cmd[4] = (uint8_t)(t_ramp >> 0); cmd[5] = (uint8_t)(torque >> 8); // 力矩电流(Ma) cmd[6] = (uint8_t)(torque >> 0); cmd[7] = snF; // 多机同步标志 cmd[8] = (uint8_t)(v >> 8); // 最大速度(RPM) cmd[9] = (uint8_t)(v >> 0); cmd[10] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 11); } /** * @brief 速度模式 * @param addr ：电机地址 * @param dir ：方向 ，0为CW，1为CCW * @param acc ：加速度(RPM/s) ，范围0 - 65535RPM/s * @param vel ：速度(RPM) ，范围0.0 - 3000.0RPM * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Vel_Control(uint8_t addr, uint8_t dir, uint16_t acc, float vel, bool snF) { static uint8_t cmd[16] = {0}; uint16_t v = 0; // 将速度放大10倍发送过去 v = (uint16_t)ABS(vel * 10.0f); // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xF6; // 功能码 cmd[2] = dir; // 符号（方向） cmd[3] = (uint8_t)(acc >> 8); // 加速度(RPM/s) cmd[4] = (uint8_t)(acc >> 0); cmd[5] = (uint8_t)(v >> 8); // 速度(RPM) cmd[6] = (uint8_t)(v >> 0); cmd[7] = snF; // 多机同步运动标志 cmd[8] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 9); } /** * @brief 速度模式限电流控制（Y42） * @param addr ：电机地址 * @param dir ：方向 ，0为CW，1为CCW * @param acc ：加速度(RPM/s) ，范围0 - 65535RPM/s * @param vel ：速度(RPM) ，范围0.0 - 3000.0RPM * @param snF ：多机同步标志 ，false为不启用，true为启用 * @param maxCur：最大电流(mA) ，范围0 - 6000mA * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Vel_LC_Control(uint8_t addr, uint8_t dir, uint16_t acc, float vel, bool snF, uint16_t maxCur) { static uint8_t cmd[16] = {0}; uint16_t v = 0; // 将速度放大10倍发送过去 v = (uint16_t)ABS(vel * 10.0f); // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xC6; // 功能码 cmd[2] = dir; // 符号（方向） cmd[3] = (uint8_t)(acc >> 8); // 加速度(RPM/s) cmd[4] = (uint8_t)(acc >> 0); cmd[5] = (uint8_t)(v >> 8); // 速度(RPM) cmd[6] = (uint8_t)(v >> 0); cmd[7] = snF; // 多机同步运动标志 cmd[8] = (uint8_t)(maxCur >> 8); // 最大电流(mA) cmd[9] = (uint8_t)(maxCur >> 0); cmd[10] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 11); } /** * @brief 直通限速位置模式 * @param addr ：电机地址 * @param dir ：方向 ，0为CW，1为CCW * @param vel ：运动速度(RPM) ，范围0.0 - 3000.0RPM * @param pos ：位置角度(°) ，范围0.0°- (2^32 - 1) / 10° * @param raf ：相位/绝对运动标志 ，0为相对上一次输入目标位置，1为绝对位置，2为相对当前实时位置 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Bypass_Pos_LV_Control(uint8_t addr, uint8_t dir, float vel, float pos, uint8_t raf, bool snF) { uint8_t cmd[16] = {0}; uint16_t v = 0; uint32_t p = 0; // 将速度和位置放大10倍发送过去 v = (uint16_t)ABS(vel * 10.0f); p = (uint32_t)ABS(pos * 10.0f); // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFB; // 功能码 cmd[2] = dir; // 符号（方向） cmd[3] = (uint8_t)(v >> 8); // 运动速度(RPM) cmd[4] = (uint8_t)(v >> 0); cmd[5] = (uint8_t)(p >> 24); // 位置角度 cmd[6] = (uint8_t)(p >> 16); cmd[7] = (uint8_t)(p >> 8); cmd[8] = (uint8_t)(p >> 0); cmd[9] = raf; // 相位/绝对运动标志 cmd[10] = snF; // 多机同步运动标志 cmd[11] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 12); } /** * @brief 直通限速位置模式限电流控制 * @param addr ：电机地址 * @param dir ：方向 ，0为CW，1为CCW * @param vel ：运动速度(RPM) ，范围0.0 - 3000.0RPM * @param pos ：位置角度(°) ，范围0.0°- (2^32 - 1) / 10° * @param raf ：相位/绝对运动标志 ，0为相对上一次输入目标位置，1为绝对位置，2为相对当前实时位置 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @param maxCur：最大电流(mA) ，范围0 - 6000mA * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Bypass_Pos_LV_LC_Control(uint8_t addr, uint8_t dir, float vel, float pos, uint8_t raf, bool snF, uint16_t maxCur) { uint8_t cmd[16] = {0}; uint16_t v = 0; uint32_t p = 0; // 将速度和位置放大10倍发送过去 v = (uint16_t)ABS(vel * 10.0f); p = (uint32_t)ABS(pos * 10.0f); // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xCB; // 功能码 cmd[2] = dir; // 符号（方向） cmd[3] = (uint8_t)(v >> 8); // 运动速度(RPM) cmd[4] = (uint8_t)(v >> 0); cmd[5] = (uint8_t)(p >> 24); // 位置角度 cmd[6] = (uint8_t)(p >> 16); cmd[7] = (uint8_t)(p >> 8); cmd[8] = (uint8_t)(p >> 0); cmd[9] = raf; // 相位/绝对运动标志 cmd[10] = snF; // 多机同步运动标志 cmd[11] = (uint8_t)(maxCur >> 8); // 最大电流(mA) cmd[12] = (uint8_t)(maxCur >> 0); cmd[13] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 14); } /** * @brief 梯形曲线加减速位置模式控制 * @param addr ：电机地址 * @param dir ：方向 ，0为CW，其余值为CCW * @param acc ：加速加速度(RPM/s) ，0为CW，其余值为CCW * @param dec ：减速加速度(RPM/s) ，0为CW，其余值为CCW * @param vel ：最大速度(RPM) ，范围0.0 - 3000.0RPM * @param pos ：位置(°) ，范围0.0°- (2^32 - 1)° * @param raf ：相位/绝对运动标志 ，0为相对上一次输入目标位置，1为绝对位置，2为相对当前实时位置 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Traj_Pos_Control(uint8_t addr, uint8_t dir, uint16_t acc, uint16_t dec, float vel, float pos, uint8_t raf, bool snF) { uint8_t cmd[32] = {0}; uint16_t v = 0; uint32_t p = 0; // 将速度和位置放大10倍发送过去 v = (uint16_t)ABS(vel * 10.0f); p = (uint32_t)ABS(pos * 10.0f); // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFD; // 功能码 cmd[2] = dir; // 符号（方向） cmd[3] = (uint8_t)(acc >> 8); // 加速加速度(RPM/s) cmd[4] = (uint8_t)(acc >> 0); cmd[5] = (uint8_t)(dec >> 8); // 减速加速度(RPM/s) cmd[6] = (uint8_t)(dec >> 0); cmd[7] = (uint8_t)(v >> 8); // 最大速度(RPM) cmd[8] = (uint8_t)(v >> 0); cmd[9] = (uint8_t)(p >> 24); // 位置角度 cmd[10] = (uint8_t)(p >> 16); cmd[11] = (uint8_t)(p >> 8); cmd[12] = (uint8_t)(p >> 0); cmd[13] = raf; // 相位/绝对运动标志 cmd[14] = snF; // 多机同步运动标志 cmd[15] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 16); } /** * @brief 梯形曲线加减速位置模式限电流控制（Y42） * @param addr ：电机地址 * @param dir ：方向 ，0为CW，其余值为CCW * @param acc ：加速加速度(RPM/s) ，0为CW，其余值为CCW * @param dec ：减速加速度(RPM/s) ，0为CW，其余值为CCW * @param vel ：最大速度(RPM) ，范围0.0 - 3000.0RPM * @param pos ：位置(°) ，范围0.0°- (2^32 - 1)° * @param raf ：相位/绝对运动标志 ，0为相对上一次输入目标位置，1为绝对位置，2为相对当前实时位置 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @param maxCur：最大电流(mA) ，范围0 - 6000mA * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Traj_Pos_LC_Control(uint8_t addr, uint8_t dir, uint16_t acc, uint16_t dec, float vel, float pos, uint8_t raf, bool snF, uint16_t maxCur) { uint8_t cmd[32] = {0}; uint16_t v = 0; uint32_t p = 0; // 将速度和位置放大10倍发送过去 v = (uint16_t)ABS(vel * 10.0f); p = (uint32_t)ABS(pos * 10.0f); // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xCD; // 功能码 cmd[2] = dir; // 符号（方向） cmd[3] = (uint8_t)(acc >> 8); // 加速加速度(RPM/s) cmd[4] = (uint8_t)(acc >> 0); cmd[5] = (uint8_t)(dec >> 8); // 减速加速度(RPM/s) cmd[6] = (uint8_t)(dec >> 0); cmd[7] = (uint8_t)(v >> 8); // 最大速度(RPM) cmd[8] = (uint8_t)(v >> 0); cmd[9] = (uint8_t)(p >> 24); // 位置角度 cmd[10] = (uint8_t)(p >> 16); cmd[11] = (uint8_t)(p >> 8); cmd[12] = (uint8_t)(p >> 0); cmd[13] = raf; // 相位/绝对运动标志 cmd[14] = snF; // 多机同步运动标志 cmd[15] = (uint8_t)(maxCur >> 8); // 最大电流(mA) cmd[16] = (uint8_t)(maxCur >> 0); cmd[17] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 18); } /** * @brief 立即停止 * @param addr ：电机地址 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Stop_Now(uint8_t addr, bool snF) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFE; // 功能码 cmd[2] = 0x98; // 辅助码 cmd[3] = snF; // 多机同步运动标志 cmd[4] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 5); } /** * @brief 多机同步运动 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Synchronous_motion(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFF; // 功能码 cmd[2] = 0x66; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 4); } /********************************************************** *** 原点回零命令 **********************************************************/ /** * @brief 设置单圈回零的零点位置 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Origin_Set_O(uint8_t addr, bool svF) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x93; // 功能码 cmd[2] = 0x88; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 5); } /** * @brief 触发回零 * @param addr ：电机地址 * @param o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x9A; // 功能码 cmd[2] = o_mode; // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 cmd[3] = snF; // 多机同步运动标志，false为不启用，true为启用 cmd[4] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 5); } /** * @brief 强制中断并退出回零 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Origin_Interrupt(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x9C; // 功能码 cmd[2] = 0x48; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 4); } /** * @brief 读取回零参数 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Origin_Read_Params(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x22; // 功能码 cmd[2] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 3); } /** * @brief 修改回零参数 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 * @param o_dir ：回零方向，0为CW，其余值为CCW * @param o_vel ：回零速度，单位：RPM（转/分钟） * @param o_tm ：回零超时时间，单位：毫秒 * @param sl_vel ：无限位碰撞回零检测转速，单位：RPM（转/分钟） * @param sl_ma ：无限位碰撞回零检测电流，单位：Ma（毫安） * @param sl_ms ：无限位碰撞回零检测时间，单位：Ms（毫秒） * @param potF ：上电自动触发回零，false为不使能，true为使能 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm, uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF) { static uint8_t cmd[32] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x4C; // 功能码 cmd[2] = 0xAE; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = o_mode; // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 cmd[5] = o_dir; // 回零方向 cmd[6] = (uint8_t)(o_vel >> 8); // 回零速度(RPM)高8位字节 cmd[7] = (uint8_t)(o_vel >> 0); // 回零速度(RPM)低8位字节 cmd[8] = (uint8_t)(o_tm >> 24); // 回零超时时间(bit24 - bit31) cmd[9] = (uint8_t)(o_tm >> 16); // 回零超时时间(bit16 - bit23) cmd[10] = (uint8_t)(o_tm >> 8); // 回零超时时间(bit8 - bit15) cmd[11] = (uint8_t)(o_tm >> 0); // 回零超时时间(bit0 - bit7 ) cmd[12] = (uint8_t)(sl_vel >> 8); // 无限位碰撞回零检测转速(RPM)高8位字节 cmd[13] = (uint8_t)(sl_vel >> 0); // 无限位碰撞回零检测转速(RPM)低8位字节 cmd[14] = (uint8_t)(sl_ma >> 8); // 无限位碰撞回零检测电流(Ma)高8位字节 cmd[15] = (uint8_t)(sl_ma >> 0); // 无限位碰撞回零检测电流(Ma)低8位字节 cmd[16] = (uint8_t)(sl_ms >> 8); // 无限位碰撞回零检测时间(Ms)高8位字节 cmd[17] = (uint8_t)(sl_ms >> 0); // 无限位碰撞回零检测时间(Ms)低8位字节 cmd[18] = potF; // 上电自动触发回零，false为不使能，true为使能 cmd[19] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 20); } /********************************************************** *** 读取系统参数命令 **********************************************************/ /** * @brief 定时返回信息命令（Y42） * @param addr ：电机地址 * @param s ：系统参数类型 * @param time_ms ：定时时间 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Auto_Return_Sys_Params_Timed(uint8_t addr, SysParams_t s, uint16_t time_ms) { uint8_t i = 0; static uint8_t cmd[16] = {0}; // 装载命令 cmd[i] = addr; ++i; // 地址 cmd[i] = 0x11; ++i; // 功能码 cmd[i] = 0x18; ++i; // 辅助码 switch(s) // 信息功能码 { case S_VBUS : cmd[i] = 0x24; ++i; break; // 读取总线电压 case S_CBUS : cmd[i] = 0x26; ++i; break; // 读取总线电流 case S_CPHA : cmd[i] = 0x27; ++i; break; // 读取相电流 case S_ENCO : cmd[i] = 0x29; ++i; break; // 读取编码器原始值 case S_CLKC : cmd[i] = 0x30; ++i; break; // 读取实时脉冲数 case S_ENCL : cmd[i] = 0x31; ++i; break; // 读取经过线性化校准后的编码器值 case S_CLKI : cmd[i] = 0x32; ++i; break; // 读取输入脉冲数 case S_TPOS : cmd[i] = 0x33; ++i; break; // 读取电机目标位置 case S_SPOS : cmd[i] = 0x34; ++i; break; // 读取电机实时设定的目标位置 case S_VEL : cmd[i] = 0x35; ++i; break; // 读取电机实时转速 case S_CPOS : cmd[i] = 0x36; ++i; break; // 读取电机实时位置 case S_PERR : cmd[i] = 0x37; ++i; break; // 读取电机位置误差 case S_VBAT : cmd[i] = 0x38; ++i; break; // 读取多圈编码器电池电压（Y42） case S_TEMP : cmd[i] = 0x39; ++i; break; // 读取电机实时温度（Y42） case S_FLAG : cmd[i] = 0x3A; ++i; break; // 读取电机状态标志位 case S_OFLAG: cmd[i] = 0x3B; ++i; break; // 读取回零状态标志位 case S_OAF : cmd[i] = 0x3C; ++i; break; // 读取电机状态标志位 + 回零状态标志位（Y42） case S_PIN : cmd[i] = 0x3D; ++i; break; // 读取引脚状态（Y42） default: break; } cmd[i] = (uint8_t)(time_ms >> 8); ++i; // 定时时间 cmd[i] = (uint8_t)(time_ms >> 0); ++i; cmd[i] = 0x6B; ++i; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, i); } /** * @brief 读取系统参数 * @param addr ：电机地址 * @param s ：系统参数类型 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_Sys_Params(uint8_t addr, SysParams_t s) { uint8_t i = 0; static uint8_t cmd[16] = {0}; // 装载命令 cmd[i] = addr; ++i; // 地址 switch(s) // 功能码 { case S_VBUS : cmd[i] = 0x24; ++i; break; // 读取总线电压 case S_CBUS : cmd[i] = 0x26; ++i; break; // 读取总线电流 case S_CPHA : cmd[i] = 0x27; ++i; break; // 读取相电流 case S_ENCO : cmd[i] = 0x29; ++i; break; // 读取编码器原始值 case S_CLKC : cmd[i] = 0x30; ++i; break; // 读取实时脉冲数 case S_ENCL : cmd[i] = 0x31; ++i; break; // 读取经过线性化校准后的编码器值 case S_CLKI : cmd[i] = 0x32; ++i; break; // 读取输入脉冲数 case S_TPOS : cmd[i] = 0x33; ++i; break; // 读取电机目标位置 case S_SPOS : cmd[i] = 0x34; ++i; break; // 读取电机实时设定的目标位置 case S_VEL : cmd[i] = 0x35; ++i; break; // 读取电机实时转速 case S_CPOS : cmd[i] = 0x36; ++i; break; // 读取电机实时位置 case S_PERR : cmd[i] = 0x37; ++i; break; // 读取电机位置误差 case S_VBAT : cmd[i] = 0x38; ++i; break; // 读取多圈编码器电池电压（Y42） case S_TEMP : cmd[i] = 0x39; ++i; break; // 读取电机实时温度（Y42） case S_FLAG : cmd[i] = 0x3A; ++i; break; // 读取电机状态标志位 case S_OFLAG: cmd[i] = 0x3B; ++i; break; // 读取回零状态标志位 case S_OAF : cmd[i] = 0x3C; ++i; break; // 读取电机状态标志位 + 回零状态标志位（Y42） case S_PIN : cmd[i] = 0x3D; ++i; break; // 读取引脚状态（Y42） default: break; } cmd[i] = 0x6B; ++i; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, i); } /********************************************************** *** 读写驱动参数命令 **********************************************************/ /** * @brief 修改电机ID地址 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param id ：默认电机ID为1，可修改为1-255，0为广播地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Motor_ID(uint8_t addr, bool svF, uint8_t id) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xAE; // 功能码 cmd[2] = 0x4B; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = id; // 默认电机ID为1，可修改为1-255，0为广播地址 cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改细分值 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param mstep ：默认细分为16，可修改为1-2556，0为256细分 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_MicroStep(uint8_t addr, bool svF, uint8_t mstep) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x84; // 功能码 cmd[2] = 0x8A; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = mstep; // 默认细分为16，可修改为1-2556，0为256细分 cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改掉电标志 * @param addr ：电机地址 * @param pdf ：掉电标志 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_PDFlag(uint8_t addr, bool pdf) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x50; // 功能码 cmd[2] = pdf; // 掉电标志 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 4); } /** * @brief 读取选项参数状态（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_Opt_Param_Sta(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x1A; // 功能码 cmd[2] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 3); } /** * @brief 修改电机类型（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param mottype ：电机类型，默认为0，0表示1.8°步进电机，1表示0.9°步进电机 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Motor_Type(uint8_t addr, bool svF, bool mottype) { static uint8_t cmd[16] = {0}; uint8_t MotType = 0; if(mottype) { MotType = 25; } else { MotType = 50; } // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xD7; // 功能码 cmd[2] = 0x35; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = MotType; // 电机类型，0表示0.9°步进电机，1表示1.8°步进电机 cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改固件类型（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param fwtype ：固件类型，默认为0，0为X固件，1为Emm固件 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Firmware_Type(uint8_t addr, bool svF, bool fwtype) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xD5; // 功能码 cmd[2] = 0x69; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = fwtype; // 电机类型，25表示0.9°步进电机，50表示1.8°步进电机 cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改开环/闭环控制模式（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param ctrl_mode：控制模式，默认为1,0为开环模式，1为闭环FOC模式 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Ctrl_Mode(uint8_t addr, bool svF, bool ctrl_mode) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x46; // 功能码 cmd[2] = 0x69; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = ctrl_mode; // 控制模式，默认为1,0为开环模式，1为闭环FOC模式 cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改电机运动正方向（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param dir ：电机运动正方向，默认为CW，0为CW（顺时针方向），1为CCW * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Motor_Dir(uint8_t addr, bool svF, bool dir) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xD4; // 功能码 cmd[2] = 0x60; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = dir; // 电机运动正方向，默认为CW，0为CW（顺时针方向），1为CCW cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改锁定按键功能（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param lock ：锁定按键功能，默认为Disable，0为Disable，1为Enable * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Lock_Btn(uint8_t addr, bool svF, bool lock) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xD0; // 功能码 cmd[2] = 0xB3; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = lock; // 锁定按键功能，默认为Disable，0为Disable），1为Enable cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改命令位置角度是否继续缩小10倍输入（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param s_vel ：命令位置角度是否继续缩小10倍输入，默认为Disable，0为Disable，1为Enable * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_S_Vel(uint8_t addr, bool svF, bool s_vel) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x4F; // 功能码 cmd[2] = 0x71; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = s_vel; // 命令位置角度是否继续缩小10倍输入，默认为Disable，0为Disable，1为Enable cmd[5] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 6); } /** * @brief 修改开环模式工作电流 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param om_ma ：开环模式工作电流，单位mA * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_OM_mA(uint8_t addr, bool svF, uint16_t om_ma) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x44; // 功能码 cmd[2] = 0x33; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(om_ma >> 8); // 开环模式工作电流，单位mA cmd[5] = (uint8_t)(om_ma >> 0); cmd[6] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 7); } /** * @brief 修改闭环模式最大电流 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param foc_mA ：闭环模式最大电流，单位mA * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_FOC_mA(uint8_t addr, bool svF, uint16_t foc_mA) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x45; // 功能码 cmd[2] = 0x66; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(foc_mA >> 8); // 闭环模式最大电流，单位mA cmd[5] = (uint8_t)(foc_mA >> 0); cmd[6] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 7); } /** * @brief 读取PID参数 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_PID_Params(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x21; // 功能码 cmd[2] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 3); } /** * @brief 修改PID参数 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param kp ：比例系数，默认为Y42/18000 * @param ki ：积分系数，默认为Y42/10 * @param kd ：微分系数，默认为Y42/18000 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_PID_Params(uint8_t addr, bool svF, uint32_t kp, uint32_t ki, uint32_t kd) { static uint8_t cmd[20] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x4A; // 功能码 cmd[2] = 0xC3; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(kp >> 24); // kp cmd[5] = (uint8_t)(kp >> 16); cmd[6] = (uint8_t)(kp >> 8); cmd[7] = (uint8_t)(kp >> 0); cmd[8] = (uint8_t)(ki >> 24); // ki cmd[9] = (uint8_t)(ki >> 16); cmd[10] = (uint8_t)(ki >> 8); cmd[11] = (uint8_t)(ki >> 0); cmd[12] = (uint8_t)(kd >> 24); // kd cmd[13] = (uint8_t)(kd >> 16); cmd[14] = (uint8_t)(kd >> 8); cmd[15] = (uint8_t)(kd >> 0); cmd[16] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 17); } /** * @brief 读取DMX512协议参数（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_DMX512_Params(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x49; // 功能码 cmd[2] = 0x78; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 4); } /** * @brief 读取DMX512协议参数（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param tch ：总通道数，默认为192，该值要与自身 DMX512 控制器的总通道数一样 * @param nch ：每个电机占用的通道数，默认为1，1为单通道模式,2为双通道模式 * @param mode ：运动模式，默认为1，0表示相对位置模式运动，1表示绝对坐标式位置运动 * @param vel ：单通道模式的运动速度，默认值为1000， 单位RPM， 即1000RPM； * @param acc ：加速度，acc=加速数值/8=125，加速时间见说明书“5.3.12 位置模式控制（Emm）” * @param vel_step ：双通道模式速度步长，默认值为 10， 即电机运动速度为(通道值 * 10)RPM * @param pos_step ：双通道模式运动步长，默认值为 100， 即电机转动角度为(通道值 * 10.0)° * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_DMX512_Params(uint8_t addr, bool svF, uint16_t tch, uint8_t nch, uint8_t mode, uint16_t vel, uint16_t acc, uint16_t vel_step, uint32_t pos_step) { static uint8_t cmd[32] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xD9; // 功能码 cmd[2] = 0x90; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(tch >> 8); // 总通道数 cmd[5] = (uint8_t)(tch >> 0); cmd[6] = nch; // 每个电机占用的通道数 cmd[7] = mode; // 运动模式 cmd[8] = (uint8_t)(vel >> 8); // 单通道模式的运动速度 cmd[9] = (uint8_t)(vel >> 0); cmd[10] = (uint8_t)(acc >> 8); // 双通道模式速度步长 cmd[11] = (uint8_t)(acc >> 0); cmd[12] = (uint8_t)(vel_step >> 8); // 双通道模式速度步长 cmd[13] = (uint8_t)(vel_step >> 0); cmd[14] = (uint8_t)(pos_step >> 24); // 双通道模式运动步长 cmd[15] = (uint8_t)(pos_step >> 16); cmd[16] = (uint8_t)(pos_step >> 8); cmd[17] = (uint8_t)(pos_step >> 0); cmd[18] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 19); } /** * @brief 读取位置到达窗口（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_Pos_Window(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x41; // 功能码 cmd[2] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 3); } /** * @brief 修改位置到达窗口（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param prw ：位置到达窗口，默认值为8，即0.8° * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Pos_Window(uint8_t addr, bool svF, uint16_t prw) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xD1; // 功能码 cmd[2] = 0x07; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(prw >> 8); // 位置到达窗口，默认值为8，即0.8° cmd[5] = (uint8_t)(prw >> 0); cmd[6] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 7); } /** * @brief 读取过热过流保护检测阈值（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_Otocp(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x13; // 功能码 cmd[2] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 3); } /** * @brief 修改过热过流保护检测阈值（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param otp ：过热保护检测阈值，默认100℃ * @param ocp ：过流保护检测阈值，默认6600mA * @param time_ms ：过热过流检测时间，默认1000ms * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Otocp(uint8_t addr, bool svF, uint16_t otp, uint16_t ocp, uint16_t time_ms) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xD3; // 功能码 cmd[2] = 0x56; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(otp >> 8); // 过热保护检测阈值 cmd[5] = (uint8_t)(otp >> 0); cmd[6] = (uint8_t)(ocp >> 8); // 过流保护检测阈值 cmd[7] = (uint8_t)(ocp >> 0); cmd[8] = (uint8_t)(time_ms >> 8); // 过热过流检测时间 cmd[9] = (uint8_t)(time_ms >> 0); cmd[10] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 11); } /** * @brief 读取心跳保护功能时间（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_Heart_Protect(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x16; // 功能码 cmd[2] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 3); } /** * @brief 修改心跳保护功能时间（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param hp ：心跳保护时间，单位：ms * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Heart_Protect(uint8_t addr, bool svF, uint32_t hp) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x68; // 功能码 cmd[2] = 0x38; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(hp >> 24); // 心跳保护时间，单位：ms cmd[5] = (uint8_t)(hp >> 16); cmd[6] = (uint8_t)(hp >> 8); cmd[7] = (uint8_t)(hp >> 0); cmd[8] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 9); } /** * @brief 读取积分限幅/刚性系数（Y42） * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_Integral_Limit(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x23; // 功能码 cmd[2] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 3); } /** * @brief 修改积分限幅/刚性系数（Y42） * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param il ：刚性系数，X 固件默认为Y42/388、Y57/512 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Modify_Integral_Limit(uint8_t addr, bool svF, uint32_t il) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x4B; // 功能码 cmd[2] = 0x57; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = (uint8_t)(il >> 24); // 刚性系数，X 固件默认为Y42/388、Y57/512 cmd[5] = (uint8_t)(il >> 16); cmd[6] = (uint8_t)(il >> 8); cmd[7] = (uint8_t)(il >> 0); cmd[8] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 9); } /********************************************************** *** 读取所有驱动参数命令 **********************************************************/ /** * @brief 读取系统状态参数 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_System_State_Params(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x43; // 功能码 cmd[2] = 0x7A; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 4); } /** * @brief 读取驱动配置参数 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_Read_Motor_Conf_Params(uint8_t addr) { static uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x42; // 功能码 cmd[2] = 0x6C; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 step_sendcmd(addr,cmd, 4); } /** *********************************************************** *********************************************************** *** *** *** @brief 以下是把相应命令加载到Y42多电机命令上的函数（Y42） *** *** *********************************************************** *********************************************************** ***/ /********************************************************** *** 触发动作命令 **********************************************************/ /** * @brief 触发编码器校准 - 加载到多电机指令上 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Trig_Encoder_Cal(uint8_t addr) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x06; // 功能码 cmd[2] = 0x45; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 4; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 重启电机（Y42） - 加载到多电机指令上 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Reset_Motor(uint8_t addr) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x08; // 功能码 cmd[2] = 0x97; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 4; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 将当前位置清零 - 加载到多电机指令上 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Reset_CurPos_To_Zero(uint8_t addr) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0A; // 功能码 cmd[2] = 0x6D; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 4; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 解除堵转保护 - 加载到多电机指令上 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Reset_Clog_Pro(uint8_t addr) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0E; // 功能码 cmd[2] = 0x52; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 4; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 恢复出厂设置 - 加载到多电机指令上 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Restore_Motor(uint8_t addr) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0F; // 功能码 cmd[2] = 0x5F; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 4; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /********************************************************** *** 运动控制命令 **********************************************************/ /** * @brief 使能信号控制 - 加载到多电机指令上 * @param addr ：电机地址 * @param state ：使能状态 ，true为使能电机，false为关闭电机 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_En_Control(uint8_t addr, bool state, bool snF) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xF3; // 功能码 cmd[2] = 0xAB; // 辅助码 cmd[3] = (uint8_t)state; // 使能状态 cmd[4] = snF; // 多机同步运动标志 cmd[5] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 6; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 速度模式 - 加载到多电机指令上 * @param addr：电机地址 * @param dir ：方向 ，0为CW，其余值为CCW * @param vel ：速度 ，范围0 - 5000RPM * @param acc ：加速度 ，范围0 - 255，注意：0是直接启动 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xF6; // 功能码 cmd[2] = dir; // 方向 cmd[3] = (uint8_t)(vel >> 8); // 速度(RPM)高8位字节 cmd[4] = (uint8_t)(vel >> 0); // 速度(RPM)低8位字节 cmd[5] = acc; // 加速度，注意：0是直接启动 cmd[6] = snF; // 多机同步运动标志 cmd[7] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 8; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 位置模式 - 加载到多电机指令上 * @param addr：电机地址 * @param dir ：方向 ，0为CW，其余值为CCW * @param vel ：速度(RPM) ，范围0 - 5000RPM * @param acc ：加速度 ，范围0 - 255，注意：0是直接启动 * @param clk ：脉冲数 ，范围0- (2^32 - 1)个 * @param raF ：相位/绝对标志，false为相对运动，true为绝对值运动 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFD; // 功能码 cmd[2] = dir; // 方向 cmd[3] = (uint8_t)(vel >> 8); // 速度(RPM)高8位字节 cmd[4] = (uint8_t)(vel >> 0); // 速度(RPM)低8位字节 cmd[5] = acc; // 加速度，注意：0是直接启动 cmd[6] = (uint8_t)(clk >> 24); // 脉冲数(bit24 - bit31) cmd[7] = (uint8_t)(clk >> 16); // 脉冲数(bit16 - bit23) cmd[8] = (uint8_t)(clk >> 8); // 脉冲数(bit8 - bit15) cmd[9] = (uint8_t)(clk >> 0); // 脉冲数(bit0 - bit7 ) cmd[10] = raF; // 相位/绝对标志，false为相对运动，true为绝对值运动 cmd[11] = snF; // 多机同步运动标志，false为不启用，true为启用 cmd[12] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 13; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 立即停止 - 加载到多电机指令上 * @param addr ：电机地址 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Stop_Now(uint8_t addr, bool snF) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFE; // 功能码 cmd[2] = 0x98; // 辅助码 cmd[3] = snF; // 多机同步运动标志 cmd[4] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 5; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 多机同步运动 - 加载到多电机指令上 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Synchronous_motion(uint8_t addr) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFF; // 功能码 cmd[2] = 0x66; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 4; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /********************************************************** *** 原点回零命令 **********************************************************/ /** * @brief 设置单圈回零的零点位置 - 加载到多电机指令上 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Origin_Set_O(uint8_t addr, bool svF) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x93; // 功能码 cmd[2] = 0x88; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 5; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 触发回零 - 加载到多电机指令上 * @param addr ：电机地址 * @param o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x9A; // 功能码 cmd[2] = o_mode; // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 cmd[3] = snF; // 多机同步运动标志，false为不启用，true为启用 cmd[4] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 5; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 强制中断并退出回零 - 加载到多电机指令上 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Origin_Interrupt(uint8_t addr) { uint8_t j = 0, cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x9C; // 功能码 cmd[2] = 0x48; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 4; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 修改回零参数 - 加载到多电机指令上 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 * @param o_dir ：回零方向，0为CW，其余值为CCW * @param o_vel ：回零速度，单位：RPM（转/分钟） * @param o_tm ：回零超时时间，单位：毫秒 * @param sl_vel ：无限位碰撞回零检测转速，单位：RPM（转/分钟） * @param sl_ma ：无限位碰撞回零检测电流，单位：Ma（毫安） * @param sl_ms ：无限位碰撞回零检测时间，单位：Ms（毫秒） * @param potF ：上电自动触发回零，false为不使能，true为使能 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm, uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF) { uint8_t j = 0, cmd[32] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x4C; // 功能码 cmd[2] = 0xAE; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = o_mode; // 回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 cmd[5] = o_dir; // 回零方向 cmd[6] = (uint8_t)(o_vel >> 8); // 回零速度(RPM)高8位字节 cmd[7] = (uint8_t)(o_vel >> 0); // 回零速度(RPM)低8位字节 cmd[8] = (uint8_t)(o_tm >> 24); // 回零超时时间(bit24 - bit31) cmd[9] = (uint8_t)(o_tm >> 16); // 回零超时时间(bit16 - bit23) cmd[10] = (uint8_t)(o_tm >> 8); // 回零超时时间(bit8 - bit15) cmd[11] = (uint8_t)(o_tm >> 0); // 回零超时时间(bit0 - bit7 ) cmd[12] = (uint8_t)(sl_vel >> 8); // 无限位碰撞回零检测转速(RPM)高8位字节 cmd[13] = (uint8_t)(sl_vel >> 0); // 无限位碰撞回零检测转速(RPM)低8位字节 cmd[14] = (uint8_t)(sl_ma >> 8); // 无限位碰撞回零检测电流(Ma)高8位字节 cmd[15] = (uint8_t)(sl_ma >> 0); // 无限位碰撞回零检测电流(Ma)低8位字节 cmd[16] = (uint8_t)(sl_ms >> 8); // 无限位碰撞回零检测时间(Ms)高8位字节 cmd[17] = (uint8_t)(sl_ms >> 0); // 无限位碰撞回零检测时间(Ms)低8位字节 cmd[18] = potF; // 上电自动触发回零，false为不使能，true为使能 cmd[19] = 0x6B; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < 20; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /********************************************************** *** 读取系统参数命令 **********************************************************/ /********************************************************** *** 读取系统参数命令 **********************************************************/ /** * @brief 定时返回信息命令（Y42） * @param addr ：电机地址 * @param s ：系统参数类型 * @param time_ms ：定时时间 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Auto_Return_Sys_Params_Timed(uint8_t addr, SysParams_t s, uint16_t time_ms) { uint8_t i = 0, j = 0; uint8_t cmd[16] = {0}; // 装载命令 cmd[i] = addr; ++i; // 地址 cmd[i] = 0x11; ++i; // 功能码 cmd[i] = 0x18; ++i; // 辅助码 switch(s) // 信息功能码 { case S_VBUS : cmd[i] = 0x24; ++i; break; // 读取总线电压 case S_CBUS : cmd[i] = 0x26; ++i; break; // 读取总线电流 case S_CPHA : cmd[i] = 0x27; ++i; break; // 读取相电流 case S_ENCO : cmd[i] = 0x29; ++i; break; // 读取编码器原始值 case S_CLKC : cmd[i] = 0x30; ++i; break; // 读取实时脉冲数 case S_ENCL : cmd[i] = 0x31; ++i; break; // 读取经过线性化校准后的编码器值 case S_CLKI : cmd[i] = 0x32; ++i; break; // 读取输入脉冲数 case S_TPOS : cmd[i] = 0x33; ++i; break; // 读取电机目标位置 case S_SPOS : cmd[i] = 0x34; ++i; break; // 读取电机实时设定的目标位置 case S_VEL : cmd[i] = 0x35; ++i; break; // 读取电机实时转速 case S_CPOS : cmd[i] = 0x36; ++i; break; // 读取电机实时位置 case S_PERR : cmd[i] = 0x37; ++i; break; // 读取电机位置误差 case S_VBAT : cmd[i] = 0x38; ++i; break; // 读取多圈编码器电池电压（Y42） case S_TEMP : cmd[i] = 0x39; ++i; break; // 读取电机实时温度（Y42） case S_FLAG : cmd[i] = 0x3A; ++i; break; // 读取电机状态标志位 case S_OFLAG: cmd[i] = 0x3B; ++i; break; // 读取回零状态标志位 case S_OAF : cmd[i] = 0x3C; ++i; break; // 读取电机状态标志位 + 回零状态标志位（Y42） case S_PIN : cmd[i] = 0x3D; ++i; break; // 读取引脚状态（Y42） default: break; } cmd[i] = (uint8_t)(time_ms >> 8); ++i; // 定时时间 cmd[i] = (uint8_t)(time_ms >> 0); ++i; cmd[i] = 0x6B; ++i; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < i; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /** * @brief 读取系统参数 - 加载到多电机指令上 * @param addr ：电机地址 * @param s ：系统参数类型 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void X_V2_MMCL_Read_Sys_Params(uint8_t addr, SysParams_t s) { uint8_t i = 0, j = 0; uint8_t cmd[16] = {0}; // 装载命令 cmd[i] = addr; ++i; // 地址 switch(s) // 功能码 { case S_VBUS : cmd[i] = 0x24; ++i; break; // 读取总线电压 case S_CBUS : cmd[i] = 0x26; ++i; break; // 读取总线电流 case S_CPHA : cmd[i] = 0x27; ++i; break; // 读取相电流 case S_ENCO : cmd[i] = 0x29; ++i; break; // 读取编码器原始值 case S_CLKC : cmd[i] = 0x30; ++i; break; // 读取实时脉冲数 case S_ENCL : cmd[i] = 0x31; ++i; break; // 读取经过线性化校准后的编码器值 case S_CLKI : cmd[i] = 0x32; ++i; break; // 读取输入脉冲数 case S_TPOS : cmd[i] = 0x33; ++i; break; // 读取电机目标位置 case S_SPOS : cmd[i] = 0x34; ++i; break; // 读取电机实时设定的目标位置 case S_VEL : cmd[i] = 0x35; ++i; break; // 读取电机实时转速 case S_CPOS : cmd[i] = 0x36; ++i; break; // 读取电机实时位置 case S_PERR : cmd[i] = 0x37; ++i; break; // 读取电机位置误差 case S_VBAT : cmd[i] = 0x38; ++i; break; // 读取多圈编码器电池电压（Y42） case S_TEMP : cmd[i] = 0x39; ++i; break; // 读取电机实时温度（Y42） case S_FLAG : cmd[i] = 0x3A; ++i; break; // 读取电机状态标志位 case S_OFLAG: cmd[i] = 0x3B; ++i; break; // 读取回零状态标志位 case S_OAF : cmd[i] = 0x3C; ++i; break; // 读取电机状态标志位 + 回零状态标志位（Y42） case S_PIN : cmd[i] = 0x3D; ++i; break; // 读取引脚状态（Y42） default: break; } cmd[i] = 0x6B; ++i; // 校验字节 // 加载当前命令到多电机命令中 for(j=0; j < i; j++) { MMCL_cmd[MMCL_count] = cmd[j]; ++MMCL_count; } } /********************************************************** *** 读写驱动参数命令 **********************************************************/