本文重点讨论利用CANopen-SDO通信的方式去实现Elmo驱动器的速度模式和位置模式,CANopen的SDO通信效率是较低,实时性不高,因此只适用于不会频繁修改电机速度和位置的场景,属于Elmo驱动器的简单使用。本文最主要的内容是 已移植了CANopen开源库 CANfestival的GD32嵌入式的代码。
一、配置Elmo驱动器的Node-ID和CAN通信波特率
在刚拿到Elmo驱动器时,它的Node-ID是未知的,不知道驱动器的Node-ID,控制器是无法与之进行SDO和PDO通信的,控制器的对象字典是以驱动器Node-ID为基础去配置。由此可见,驱动器的Node-ID是第一个需要解决的问题。配置Elmo驱动器的Node-ID通过Elmo上位机(Elmo Application Studio II 64bit) 去配置。当然,为保证控制器与驱动器正常通信, CAN通信波特率配置成一致。
参考链接:
1 Node-ID配置
1.1 进入Elmo驱动器的命令行
1.2 Node-ID查询/修改/保存的指令
2 CAN通信波特率配置
2.1 进入Elmo驱动器命令行
2.2 CAN通信波特率 查询/修改/保存的指令
3 总结
驱动器的Node-ID暂时设置成0x02,CAN通信波特率设置成1MHz。
请注意:嵌入式这边的CAN波特率也需要设置成1MHz
二、对象词典设置
三、嵌入式代码
嵌入式代码部分呢,主要就是通过CANopen与驱动器进行通信。给驱动器这边发送一些指令什么的。
由于发送SDO给驱动器的步骤具有很多重复性的代码,因此我们做了些简单的封装。
1、初始化
首先来看初始化函数
void InitializeDrive(void)
{
// Set the NODE_ID of the Master to 0x01 and switch its state to 'Operational'
setNodeId(&master_Data, 0x01);
setState(&master_Data, Initialisation);
setState(&master_Data, Pre_operational);
setState(&master_Data, Operational);
// Start the driver with NODE_ID=0x02 through the NMT service
// and switch its state to 'PreOperational'
masterSendNMTstateChange(&master_Data, 0x02, NMT_Reset_Comunication);
masterSendNMTstateChange(&master_Data, 0x02, NMT_Start_Node);
masterSendNMTstateChange(&master_Data, 0x02, NMT_Enter_PreOperational);
// Waiting for the driver to be awakened
delay_1ms(2000); // This program is very [important]
}
初始化函数
1 把控制板的NodeID设置成0x01,然后把状态设置成Operational
2 通过CANopen协议的NMT服务把CAN网络中,节点为0x02的节点启动起来,然后让它进入PreOperational,熟悉CANopen协议的朋友这里应该能够敏锐的闪过一个念头,PreOperational已经可以支持SDO通信了。那么节点ID为0x02的设备是谁呢?就是我们上一节专门去设置NodeID的Elmo电机驱动器。(内心戏好多,这CAN网络里面也就是只有控制板和Elmo驱动器)。
3 这个最后这一句延迟2s的程序,专门强调一下。 你发送了这个启动节点ID为0x02的从节点的指令之后,控制器/驱动器可能需要反应一会,不能立马执行后续的操作。因此这里必须延迟2s。 不延迟的话,SDO指令会报0x80的错误。这个地方调了很久。
2、设置驱动器工作模式为PV(profile velocity模式)
void SetDriveVelocityMode(void)
{
unsigned long abortcode=0;
char size=1;
char sendData[4]={0};
// Send SDO command 602, 22, 6060, 00, 03 00 00 00
// Velocity mode
size = 4;
sendData[0]=0x03;
sendData[1]=0x00;
sendData[2]=0x00;
sendData[3]=0x00;
writeNetworkDict(&master_Data, 0x02, 0x6060, 0x00, 4, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode) != SDO_FINISHED)
{
AddSysLog("Send SDO command 6060,00, 03 00 00 00 [Velocity mode] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
这里是发送一条 SDO指令到驱动器, 往 索引/子索引为6060/00的地址写入 03。
3、设置驱动器工作模式为PP(profile position模式)
void SetDrivePositionMode(void)
{
unsigned long abortcode=0;
char size=1;
char sendData[4]={0};
// Send SDO command 602, 22, 6060, 00, 01 00 00 00
// Velocity mode
size = 4;
sendData[0]=0x01;
sendData[1]=0x00;
sendData[2]=0x00;
sendData[3]=0x00;
writeNetworkDict(&master_Data, 0x02, 0x6060, 0x00, 4, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode) != SDO_FINISHED)
{
AddSysLog("Send SDO command 6060,00, 01 00 00 00 [Position mode] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
这里和上面PV类似,往 索引/子索引为6060/00的地址写入 01。
6060/00的参数含义如下
4、使能/失能电机
4.1 使能电机
void EnableDrive(void)
{
unsigned long abortcode=0;
char size=1;
char sendData[4]={0};
// Send SDO command 602, 23, 4060, 00, 06 00 00 00
// Send SDO command 602, 23, 4060, 00, 07 00 00 00
// Send SDO command 602, 23, 4060, 00, 0F 00 00 00
// Enable the Driver with NODE_ID=0x02
size = 1;
sendData[0] = 0x06;
writeNetworkDict(&master_Data, 0x02, 0x6040, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 4060, 00, 06 00 00 00 [Enable driver] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
size = 1;
sendData[0] = 0x07;
writeNetworkDict(&master_Data, 0x02, 0x6040, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 4060, 00, 07 00 00 00 [Enable driver] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
size = 1;
sendData[0] = 0x0F;
writeNetworkDict(&master_Data, 0x02, 0x6040, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 4060, 00, 0F 00 00 00 [Enable driver] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
往 索引/子索引为6040/00的地址依次写入 06 07 0F,共3条指令。
4.2 失能电机
void DisableDrive(void)
{
unsigned long abortcode = 0;
char size = 1;
char sendData[4] = {0};
// Send SDO command 602, 23, 4060, 00, 00 00 00 00
// Disable the Driver with NODE_ID=0x02
size = 1;
sendData[0] = 0x00;
writeNetworkDict(&master_Data, 0x02, 0x6040, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 4060, 00, 00 00 00 00 [Disable driver] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
往 索引/子索引为6040/00的地址写入 0x00,共1条指令。
5、PV模式-设置目标速度
void SetVelModeTargetVelocity(float velocity)
{
float usedVelocityFloat = 0;
long usedVelocityLong = 0;
char sendData[4] = {0};
unsigned long abortcode = 0;
char size = 4;
if(velocity > 360)
{
usedVelocityFloat = 360;
}
else if(velocity < -360)
{
usedVelocityFloat = -360;
}
else
{
usedVelocityFloat = velocity;
}
//usedVelocityLong = (long)(usedVelocityFloat * 186413.51111);
usedVelocityLong = (long)(usedVelocityFloat * 5555.555555555);
sendData[0] = usedVelocityLong & 0xFF;
sendData[1] = (usedVelocityLong >> 8) & 0xFF;
sendData[2] = (usedVelocityLong >> 16) & 0xFF;
sendData[3] = (usedVelocityLong >> 24) & 0xFF;
writeNetworkDict(&master_Data, 0x02, 0x60FF, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command FF60, 00, XX XX XX XX [Set target velocity] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
这里做了一个float到 字节数组的转换。驱动器要的是Counts值作为单位,这里角度值到Counts值的转换关系要根据编码器来。比如我们用的是增量式编码器,50w线的,那么50w x 4 = 200w个Counts就代表一圈,即360°。那么1°是多少个Counts呢? 即200w / 360° = 5555.5555555556。如果我们用的绝对式编码器,26位的。 那么一圈有2^26 = 67108864个Counts。 那么1°是多少个Counts呢? 67108864/360°= 186413.5111。
注意:上述的角度(degree)To Counts的转换因子,是同时针对角位置/角速度/角加速度的。以增量式编码器这个为例,如果SDO往驱动器某个地址发送的数据是5555。那么这个5555,可能代表的是1°,也可能是1°/s,也可以是1°/s^2。
转换得到Counts数之后,用4个字节 有符号整型数 表示(数据是带正负的)。 然后把4个字节取出来,往索引/子索引为60FF/00的地址,发送你需要设定的目标速度。 设置目标速度也是一条SDO指令。
6、PP模式-设置最大速度和目标位置
6.1 设置PP模式的最大速度
void SetPosModeTargetVelocity(float velocity)
{
float usedVelocityFloat = 0;
long usedVelocityLong = 0;
char sendData[4] = {0};
unsigned long abortcode = 0;
char size = 4;
if(velocity > 360)
{
usedVelocityFloat = 360;
}
else if(velocity < -360)
{
usedVelocityFloat = -360;
}
else
{
usedVelocityFloat = velocity;
}
//usedVelocityLong = (long)(usedVelocityFloat * 186413.51111);
usedVelocityLong = (long)(usedVelocityFloat * 5555.555555555);
sendData[0] = usedVelocityLong & 0xFF;
sendData[1] = (usedVelocityLong >> 8) & 0xFF;
sendData[2] = (usedVelocityLong >> 16) & 0xFF;
sendData[3] = (usedVelocityLong >> 24) & 0xFF;
writeNetworkDict(&master_Data, 0x02, 0x6081, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 8160, 00, XX XX XX XX [Set target velocity] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
设置在PP模式下的最大速度,也有人说这个叫梯形速度。 即PP模式的位置环是一个近似时间最优的控制策略。到达某一个位置环,速度的变化是呈梯形的变化规律,先加速,再匀速,再减速。 上述函数设定的是匀速过程的最大速度。
将速度转换成Counts值之后, 通过SDO往索引为0x6081/00的地址发送4字节的数据。 注意,这里PP模式设定的速度和PV模式下设定目标速度不是同一个东西,地址也不一样。设置PP模式的最大速度也是一条SDO指令。
6.2 设置PP模式的目标位置
void SetPosModeTargetPosition(float position)
{
float usedPositionFloat = 0;
long usedPositionLong = 0;
char sendData[4] = {0};
unsigned long abortcode = 0;
char size = 4;
if(position > 180)
{
usedPositionFloat = 180;
}
else if(position < -180)
{
usedPositionFloat = -180;
}
else
{
usedPositionFloat = position;
}
// usedPositionLong = (long)(usedPositionFloat * 186413.51111);
usedPositionLong = (long)(usedPositionFloat * 5555.555555555);
sendData[0] = usedPositionLong & 0xFF;
sendData[1] = (usedPositionLong >> 8) & 0xFF;
sendData[2] = (usedPositionLong >> 16) & 0xFF;
sendData[3] = (usedPositionLong >> 24) & 0xFF;
writeNetworkDict(&master_Data, 0x02, 0x607A, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 7A60, 00, XX XX XX XX [Set target position] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
size = 2;
sendData[0] = 0x2F;
sendData[1] = 0x00;
writeNetworkDict(&master_Data, 0x02, 0x6040, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 4060, 00, 1F 00 [Set target position] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
size = 2;
sendData[0] = 0x3F;
sendData[1] = 0x00;
writeNetworkDict(&master_Data, 0x02, 0x6040, 0x00, size, 0, sendData, 0);
if(getWriteResultNetworkDict(&master_Data, 0x02, &abortcode)!=SDO_FINISHED)
{
AddSysLog("Send SDO command 4060, 00, 0F 00 [Set target position] to driver 0x02 failed");
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
delay_1ms(25);
closeSDOtransfer(&master_Data, 0x02, SDO_CLIENT);
}
PP模式下,目标位置的索引/子索引是 0x607A/00。角度到Counts的转换就不赘述了。这是第一条SDO指令
值得注意的是,PP模式设定的目标位置,要立马生效,且下次调用本函数时,设置另一个目标位置也要立马生效。
这需要后面的第二条和第三条SDO指令了,分别往索引/子索引 0x6040/00写入一个0x2F和一个0x3F。
0 0 1 0 1 1 1 1,
0 0 1 1 1 1 1 1, bit4 有个上升沿。 什么意思呢? New set-point,新的点,还是比较好理解的。
7、例子Example
// Example
// PP mode
InitializeDrive();
SetDrivePositionMode();
EnableDrive();
SetPosModeTargetVelocity(360);
SetPosModeTargetPosition(0);
delay_1ms(5000);
SetPosModeTargetPosition(90);
delay_1ms(5000);
SetPosModeTargetPosition(180);
delay_1ms(5000);
SetPosModeTargetPosition(90);
delay_1ms(5000);
SetPosModeTargetPosition(0);
delay_1ms(5000);
SetPosModeTargetPosition(-90);
delay_1ms(5000);
SetPosModeTargetPosition(-180);
delay_1ms(5000);
DisableDrive();
delay_1ms(2000);
// PV mode
SetDriveVelocityMode();
EnableDrive();
SetVelModeTargetVelocity(180);
delay_1ms(5000);
SetVelModeTargetVelocity(-180);
delay_1ms(5000);
DisableDrive();
delay_1ms(2000);
// PP mode
SetDrivePositionMode();
EnableDrive();
SetPosModeTargetVelocity(360);
SetPosModeTargetPosition(0);
delay_1ms(10000);
SetPosModeTargetPosition(90);
delay_1ms(5000);
SetPosModeTargetPosition(180);
delay_1ms(5000);
SetPosModeTargetPosition(90);
delay_1ms(5000);
SetPosModeTargetPosition(0);
delay_1ms(5000);
SetPosModeTargetPosition(-90);
delay_1ms(5000);
SetPosModeTargetPosition(-180);
delay_1ms(5000);
这个例子
是先把电机设置成pp模式,正/负角度都设置一下。
然后把电机设置成pv模式,正/负速度都设置一下。
再把电机又设置成pp模式,正/负角度都设置一下。
这个例子实现了,那么就可以通过上位机与控制板通信,然后切换模式,更新目标速度/目标位置了。