Arduino mega2560硬件串口2控制4台张大头闭环步进电机运动

//程序为mega2560通过Serial2控制4台编号为1-4的张大头闭环步进电机，电机供电为4S电池，开发板为5V供电，开发板与4台步进电机共地，步进与Arduino接线如下： // Arduino 42bujin1 42bujin2 42bujin3 42bujin4 // Rx2(pin17)------Tx-----------Tx----------Tx----------Tx // Tx2(pin16)------Rx-----------Rx----------Rx----------Rx // GND-------GND--------GND--------GND-------GND //程序基于张大头示例程序编写，并发现规律如下： //0.禁止带电插拨步进电机供电线路！ //1.速度不必太高，设定100左右即可。也可能与细分有关 //2.若为多电机同步控制模式，最后写一次0即可；若为非同步控制模式，每个电机的使动指令之间需要间隔400us以上，实取500us。目测为避免串口干扰 //3.每次写指令后，都需要进行串口收发指令 //4.在微小距离运动上，是否需要加速度功能，还需要进一步尝试 //5.闭环步进电机的主控串口通讯频率为115200，从Arduino侧仅能被动遵循。软串口设定如此高频率可能不稳定，导致功能异常、无法使用 //6.停止后可能存在个别电机的振动，目测为小误差需要修正，导致的闭环系统振荡。用手微转电机，待其自修正后一般不再振 //7.使用中注意电机供电线的接线，接反将引发短路事故。 //2024-11-27 by avery423 #define ABS(x) ((x) > 0 ? (x) : -(x)) typedef enum { S_VER = 0, /* 读取固件版本和对应的硬件版本 */ S_RL = 1, /* 读取读取相电阻和相电感 */ S_PID = 2, /* 读取PID参数 */ S_VBUS = 3, /* 读取总线电压 */ S_CPHA = 5, /* 读取相电流 */ S_ENCL = 7, /* 读取经过线性化校准后的编码器值 */ S_TPOS = 8, /* 读取电机目标位置角度 */ S_VEL = 9, /* 读取电机实时转速 */ S_CPOS = 10, /* 读取电机实时位置角度 */ S_PERR = 11, /* 读取电机位置误差角度 */ S_FLAG = 13, /* 读取使能/到位/堵转状态标志位 */ S_Conf = 14, /* 读取驱动参数 */ S_State = 15, /* 读取系统状态参数 */ S_ORG = 16, /* 读取正在回零/回零失败状态标志位 */ }SysParams_t; /********************************************************** *** 注意：每个函数的参数的具体说明，请查阅下方的函数的注释说明 **********************************************************/ void Emm_V5_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF); // 位置模式控制 void Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t *rxCount); // 返回数据接收函数 void Emm_V5_Synchronous_motion(uint8_t addr); // 触发多机同步开始运动 void Emm_V5_En_Control(uint8_t addr, bool state, bool snF); // 电机使能控制 void Emm_V5_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF); // 速度模式控制 void Emm_V5_Stop_Now(uint8_t addr, bool snF); // 让电机立即停止运动 void Emm_V5_Reset_CurPos_To_Zero(uint8_t addr); // 将当前位置清零 void Emm_V5_Reset_Clog_Pro(uint8_t addr); // 解除堵转保护 void Emm_V5_Read_Sys_Params(uint8_t addr, SysParams_t s); // 读取参数 void Emm_V5_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode); // 发送命令修改开环/闭环控制模式 void Emm_V5_Origin_Set_O(uint8_t addr, bool svF); // 设置单圈回零的零点位置 void Emm_V5_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); // 修改回零参数 void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF); // 发送命令触发回零 void Emm_V5_Origin_Interrupt(uint8_t addr); // 强制中断并退出回零 #define beep 22 #define key1 44 #define key2 43 #define key3 42 //步进电机控制指令的数据 uint8_t rxCmd[128] = {0}; uint8_t rxCount = 0; // 定义接收数据数组、接收数据长度 void setup() { pinMode(beep,OUTPUT);digitalWrite(beep,0); pinMode(key1,INPUT_PULLUP); pinMode(key2,INPUT_PULLUP); pinMode(key3,INPUT_PULLUP); Serial2.begin(115200); // 初始化硬件串口2，步进电机主控已设置串口频率为115200，故这里不能随意改为其它值 while (!Serial2) ; // // 等待串口初始化完成 wait for serial port to connect. Needed for native USB port only delay(2000); // 上电延时2秒等待Emm_V5.0闭环初始化完毕 di(1000); //Emm_V5_En_Control(2, 1, 0); //(电机地址：0-255，使能控制位：0关闭电机1启用电机，多机同步：0不启用同步1启用同步) 电机默认为启用状态 } void di(int shijian){digitalWrite(beep,1);delay(shijian);digitalWrite(beep,0);} void loop() { pos_zuo(7000); delay(4000);di(10); //注意，串口发送命令后，要留出足够时间给电机执行。后期可结合速度控制与openMV和陀螺仪实现闭环控制（结合电机停止命令） pos_qian(7000); delay(4000);di(10); pos_you(7000); delay(4000);di(10); pos_hou(7000); delay(4000);di(10); di(1000); while(digitalRead(key1)); //按下key1重复执行上述走“口”字的动作 } //-----------------------------以下，自定义函数，包含特定方向的特定距离 //13控制向前，实际可省略24的命令行 void pos_qian(long distance) //若多机同步模式下，需要指定个别几个电机运动，则电机使动的指令Emm_V5_Synchronous_motion之间需要加500us以上延迟。若为全部开始运动，则一条指令即可：Emm_V5_Synchronous_motion(0); { // (电机地址：0-255，方向：0顺时针/1逆时针，速度：0-5000，加速度：0-255、0为直接启动，脉冲数：0-2^32-1，运动模式：0相对运动1绝对值运动，多机同步功能：0不启用1启用) Emm_V5_Pos_Control(1, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(2, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); // //（电机2，方向，速度，直接启动无加速度设置，脉冲数，相对位置模式，启用多机协同）。 Emm_V5_Pos_Control(3, 1, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //注意旋转与平移时的车轮转向不同 Emm_V5_Pos_Control(4, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //接收串口缓存区数据，每写一个电机命令都需要缓存一次 //Emm_V5_Synchronous_motion(0); //多机运动（电机地址：0表示所有电机全部开始运动，其余为指定特定编号的单个单机运动） Emm_V5_Synchronous_motion(1); //指定1号电机运动 delayMicroseconds(500); //至少间隔500us Emm_V5_Synchronous_motion(3); //指定3号电机运动 //delay(5000); di(10); } //13控制向后，实际可省略24的命令行 void pos_hou(long distance) //若多机同步模式下，需要指定个别几个电机运动，则电机使动的指令Emm_V5_Synchronous_motion之间需要加500us以上延迟。若为全部开始运动，则一条指令即可：Emm_V5_Synchronous_motion(0); { // (电机地址：0-255，方向：0顺时针/1逆时针，速度：0-5000，加速度：0-255、0为直接启动，脉冲数：0-2^32-1，运动模式：0相对运动1绝对值运动，多机同步功能：0不启用1启用) Emm_V5_Pos_Control(1, 1, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(2, 0, 100, 0, distance, 0, 1); //（电机2，方向，速度，直接启动无加速度设置，脉冲数，相对位置模式，启用多机协同） Emm_V5_Receive_Data(rxCmd, &rxCount); //接收串口缓存区数据，每写一个电机命令都需要缓存一次 Emm_V5_Pos_Control(3, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //注意旋转与平移时的车轮转向不同 Emm_V5_Pos_Control(4, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //接收串口缓存区数据 //Emm_V5_Synchronous_motion(0); //多机运动（电机地址：0表示所有电机全部开始运动，其余为指定特定编号的单个单机运动） Emm_V5_Synchronous_motion(1); //指定1号电机运动 delayMicroseconds(500); //至少间隔500us Emm_V5_Synchronous_motion(3); //指定3号电机运动 //delay(5000); di(10); } //24控制向左，实际可省略13的命令行 void pos_zuo(long distance) { // (电机地址：0-255，方向：0顺时针/1逆时针，速度：0-5000，加速度：0-255、0为直接启动，脉冲数：0-2^32-1，运动模式：0相对运动1绝对值运动，多机同步功能：0不启用1启用) Emm_V5_Pos_Control(1, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //（电机2，方向，速度，直接启动无加速度设置，脉冲数，相对位置模式，启用多机协同） //接收串口缓存区数据，每写一个电机命令都需要缓存一次 Emm_V5_Pos_Control(2, 1, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(3, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(4, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //Emm_V5_Synchronous_motion(0); //多机运动（电机地址：0表示所有电机全部开始运动，其余为指定特定编号的单个单机运动） Emm_V5_Synchronous_motion(2); //指定2号电机运动 delay(1); //实测至少间隔400us，取500us以上，此处为1ms Emm_V5_Synchronous_motion(4); //指定4号电机运动 //delay(5000); di(10); } //24控制向右，实际可省略13的命令行 void pos_you(long distance) { // (电机地址：0-255，方向：0顺时针/1逆时针，速度：0-5000，加速度：0-255、0为直接启动，脉冲数：0-2^32-1，运动模式：0相对运动1绝对值运动，多机同步功能：0不启用1启用) Emm_V5_Pos_Control(1, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //（电机2，方向，速度，直接启动无加速度设置，脉冲数，相对位置模式，启用多机协同） //接收串口缓存区数据，每写一个电机命令都需要缓存一次 Emm_V5_Pos_Control(2, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(3, 0, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(4, 1, 100, 0, distance, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //Emm_V5_Synchronous_motion(0); //多机运动（电机地址：0表示所有电机全部开始运动，其余为指定特定编号的单个单机运动） Emm_V5_Synchronous_motion(2); //指定2号电机运动 delay(1); //实测至少间隔400us，取500us以上，此处为1ms Emm_V5_Synchronous_motion(4); //指定4号电机运动 //delay(5000); di(10); } //原地旋转，即4只电机同步、同向运动相同距离 void pos_xuanzhuan() { // uint8_t rxCmd[128] = {0}; uint8_t rxCount = 0; // 定义接收数据数组、接收数据长度 //(电机地址：0-255，方向：0顺时针/1逆时针，速度：0-5000，加速度：0-255、0为直接启动，脉冲数：0-2^32-1，运动模式：0相对运动1绝对值运动，多机同步功能：0不启用1启用) Emm_V5_Pos_Control(1, 0, 100, 0, 16000, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); //（电机2，方向，速度，直接启动无加速度设置，脉冲数，相对位置模式，启用多机协同）//接收串口缓存区数据，每写一个电机命令都需要缓存一次 Emm_V5_Pos_Control(2, 0, 100, 0, 16000, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(3, 0, 100, 0, 16000, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Pos_Control(4, 0, 100, 0, 16000, 0, 1); Emm_V5_Receive_Data(rxCmd, &rxCount); Emm_V5_Synchronous_motion(0); //电机全部开始动作。若指定特定几个电机动作，则每条动作指令对应填入一台电机编号，且间隔1ms以上，见fcn1和fcn2 di(10); } //========================================================以下，主要的几个电机库函数 /** * @brief 使能信号控制 * @param addr ：电机地址 * @param state ：使能状态 ，true为使能电机，false为关闭电机 * @param snF ：多机同步标志 ，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_En_Control(uint8_t addr, bool state, bool snF) //电机启用/禁用设置（默认启用） { 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; // 校验字节 // 发送命令 Serial2.write(cmd, 6); } /** * @brief 速度模式 * @param addr：电机地址 * @param dir ：方向 ，0为CW，其余值为CCW * @param vel ：速度 ，范围0 - 5000RPM * @param acc ：加速度 ，范围0 - 255，注意：0是直接启动 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF) { uint8_t 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; // 校验字节 // 发送命令 Serial2.write(cmd, 8); } /** * @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 Emm_V5_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF) { uint8_t 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; // 校验字节 // 发送命令 Serial2.write(cmd, 13); } /** * @brief 立即停止（所有控制模式都通用） * @param addr ：电机地址 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Stop_Now(uint8_t addr, bool snF) { uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFE; // 功能码 cmd[2] = 0x98; // 辅助码 cmd[3] = snF; // 多机同步运动标志 cmd[4] = 0x6B; // 校验字节 // 发送命令 Serial2.write(cmd, 5); } /** * @brief 多机同步运动 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Synchronous_motion(uint8_t addr) { uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0xFF; // 功能码 cmd[2] = 0x66; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 Serial2.write(cmd, 4); } /** * @brief 接收数据 * @param rxCmd : 接收到的数据缓存在该数组 * @param rxCount : 接收到的数据长度 * @retval 无 */ void Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t *rxCount) { int i = 0; unsigned long lTime; // 上一时刻的时间 unsigned long cTime; // 当前时刻的时间 lTime = cTime = millis(); // 记录当前的时间 while(1) // 开始接收数据 { if(Serial2.available() > 0) // 串口有数据进来 { if(i <= 128) // 防止数组溢出，该值需要小于数组的长度 { rxCmd[i++] = Serial2.read(); // 接收数据 lTime = millis(); // 更新上一时刻的时间 } } else // 串口有没有数据 { cTime = millis(); // 获取当前时刻的时间 if((int)(cTime - lTime) > 100) // 100毫秒内串口没有数据进来，就判定一帧数据接收结束 { *rxCount = i; // 数据长度 break; // 退出while(1)循环 } } } } //----------------------------------------------------------------以下，其它电机库函数 /** * @brief 将当前位置清零 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Reset_CurPos_To_Zero(uint8_t addr) { uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0A; // 功能码 cmd[2] = 0x6D; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 Serial2.write(cmd, 4); } /** * @brief 解除堵转保护 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Reset_Clog_Pro(uint8_t addr) { uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x0E; // 功能码 cmd[2] = 0x52; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 Serial2.write(cmd, 4); } /** * @brief 读取系统参数 * @param addr ：电机地址 * @param s ：系统参数类型 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Read_Sys_Params(uint8_t addr, SysParams_t s) { uint8_t i = 0; uint8_t cmd[16] = {0}; // 装载命令 cmd[i] = addr; ++i; // 地址 switch(s) // 功能码 { case S_VER : cmd[i] = 0x1F; ++i; break; case S_RL : cmd[i] = 0x20; ++i; break; case S_PID : cmd[i] = 0x21; ++i; break; case S_VBUS : cmd[i] = 0x24; ++i; break; case S_CPHA : cmd[i] = 0x27; ++i; break; case S_ENCL : cmd[i] = 0x31; ++i; break; case S_TPOS : cmd[i] = 0x33; ++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_FLAG : cmd[i] = 0x3A; ++i; break; case S_ORG : cmd[i] = 0x3B; ++i; break; case S_Conf : cmd[i] = 0x42; ++i; cmd[i] = 0x6C; ++i; break; case S_State: cmd[i] = 0x43; ++i; cmd[i] = 0x7A; ++i; break; default: break; } cmd[i] = 0x6B; ++i; // 校验字节 // 发送命令 Serial2.write(cmd, i); } /** * @brief 修改开环/闭环控制模式 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @param ctrl_mode：控制模式（对应屏幕上的P_Pul菜单），0是关闭脉冲输入引脚，1是开环模式，2是闭环模式，3是让En端口复用为多圈限位开关输入引脚，Dir端口复用为到位输出高电平功能 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode) { uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x46; // 功能码 cmd[2] = 0x69; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = ctrl_mode; // 控制模式（对应屏幕上的P_Pul菜单），0是关闭脉冲输入引脚，1是开环模式，2是闭环模式，3是让En端口复用为多圈限位开关输入引脚，Dir端口复用为到位输出高电平功能 cmd[5] = 0x6B; // 校验字节 // 发送命令 Serial2.write(cmd, 6); } /** * @brief 设置单圈回零的零点位置 * @param addr ：电机地址 * @param svF ：是否存储标志，false为不存储，true为存储 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Origin_Set_O(uint8_t addr, bool svF) { uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x93; // 功能码 cmd[2] = 0x88; // 辅助码 cmd[3] = svF; // 是否存储标志，false为不存储，true为存储 cmd[4] = 0x6B; // 校验字节 // 发送命令 Serial2.write(cmd, 5); } /** * @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 Emm_V5_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 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; // 校验字节 // 发送命令 Serial2.write(cmd, 20); } /** * @brief 触发回零 * @param addr ：电机地址 * @param o_mode ：回零模式，0为单圈就近回零，1为单圈方向回零，2为多圈无限位碰撞回零，3为多圈有限位开关回零 * @param snF ：多机同步标志，false为不启用，true为启用 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF) { 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; // 校验字节 // 发送命令 Serial2.write(cmd, 5); } /** * @brief 强制中断并退出回零 * @param addr ：电机地址 * @retval 地址 + 功能码 + 命令状态 + 校验字节 */ void Emm_V5_Origin_Interrupt(uint8_t addr) { uint8_t cmd[16] = {0}; // 装载命令 cmd[0] = addr; // 地址 cmd[1] = 0x9C; // 功能码 cmd[2] = 0x48; // 辅助码 cmd[3] = 0x6B; // 校验字节 // 发送命令 Serial2.write(cmd, 4); }