CANOpen DS402伺服电机控制项目总结

article/2025/9/16 19:50:43

公司的电机控制要统一到CANOpen上面来，协议栈用的CanFestival，配合RTThread进行控制。

要控制伺服电机，最先搞明白的就是控制字6040和状态字6041。一共使用过三个厂家的伺服电机，控制字都是一样的，但是状态字各家有自己的处理，有一点小差别。如下是其中一家的文档。

有些厂家把状态字的bit9在CANOpen控制的时候是置1的，认为CANOpen就是远程控制，而有些厂家则无此处理。如果按照状态字单个bit的解释，貌似每家的都没什么问题。所以如果按照上图中先判断状态字，然后再发送控制字控制电机进入使能模式的话就要注意这些差别了。

下面是伺服电机的控制模式：

#define NO_MODE					0x00
#define PP_MODE					0x01		// Profile position mode
#define VL_MODE					0x02		// Velocity mode
#define PV_MODE					0x03        // Profile velocity mode
#define TQ_MODE					0x04        // Torque profile mode
#define HM_MODE					0x06        // Homing mode
#define IP_MODE					0x07        // Interpolated position mode
#define CSP_MODE				0x08        // Cyclic synchronous position mode
#define CSV_MODE				0x09        // Cyclic synchronous velocity mode
#define CST_MODE				0x0A		// Cyclic synchronous torque mode

厂家一般是不会全部实现的，我们项目中用到了PP_MODE,VL_MODE和TQ_MODE。

PP_MODE的控制是最复杂的，除了要给定位置和速度，还要控制控制字中的bit4和bit5才能更新指令。

/** 设置目标位置和速度
*/
motor_err_t Motor_Set_TargetPos(uint8_t nodeId,int32_t pos,uint32_t vel)
{CHECK_ID(nodeId);CHECK_STATUS(nodeId);	if(canopenMotors[nodeId - 1].modesOfOperation != PP_MODE){return MODEERR;}canopenMotors[nodeId - 1].targetPos = pos;canopenMotors[nodeId - 1].profileVel = vel;sendOnePDOevent(motor_od,(nodeId - 1)*4 + 1);rt_thread_delay(1);canopenMotors[nodeId - 1].controlWord = 0x2F;		// 清零 new_set_point bitsendOnePDOevent(motor_od,(nodeId -1)*4);rt_thread_delay(2);canopenMotors[nodeId - 1].controlWord = 0x3F;		// 设置 new_set_point bitsendOnePDOevent(motor_od,(nodeId -1)*4);return NOERR;
}

上面的控制方式是周期性的发位置指令给伺服，旧的指令还未到位的时候新指令已经下发，则伺服会立即更新指令，不会等到上一条指令执行完，两条指令中间也就不会有停顿。0x2F和0x3F的发送顺序不影响指令的执行，但是中间最好有延时，否则伺服可能更新不了位置。

速度模式和位置模式相对比较简单，直接发送指令控制就行。

/** 设置目标速度
*/
motor_err_t Motor_Set_TargetVel(uint8_t nodeId,int32_t  vel)
{CHECK_ID(nodeId);CHECK_STATUS(nodeId);if(canopenMotors[nodeId - 1].modesOfOperation != PV_MODE){return MODEERR;}canopenMotors[nodeId - 1].targetVel = vel;sendOnePDOevent(motor_od,(nodeId - 1)*4 + 1);return NOERR;
}
/** 设置目标力矩
*/
motor_err_t Motor_Set_TargetTorque(uint8_t nodeId,int16_t torque)
{CHECK_ID(nodeId);CHECK_STATUS(nodeId);if(canopenMotors[nodeId - 1].modesOfOperation != TQ_MODE){return MODEERR;}canopenMotors[nodeId - 1].targetTorque  = torque;sendOnePDOevent(motor_od,(nodeId - 1)*4 + 1);return NOERR;}

对于相对编码器而言，上电之后编码器位置被定义为0，逆时针旋转增大，顺时针旋转减小，数据类型是int32类型。那么对于一直超同一个方向旋转的应用来说，数据早晚会溢出。那么我们希望有一个指令，可以让伺服把记录的编码器值清零，下面就是通过原点回零模式的一种方法。