ethercat一拖多驱动控制

ethercat主站开发之-----一拖多驱动控制

ethercat伺服电机驱动器使用过很多品牌vx18820959358，从最早起的松下、台达、韩国Ls，到后续国产的汇川、清能德创、英威腾、高创、禾川、伊莱斯、睿能、新时达、摩通等。这些年几乎是一步步见证国内ethercat驱动的蓬勃发展。早期一拖一的驱动是主流，随着机器人行业的成本竞争的白热化，一拖多驱动驱动由于成本优势，逐渐成为机器人行业的主流应用选择。

ethercat伺服驱动基本遵循CIA402协议，约束0x6000范围的对象字典的定义和功能。

比如：汇川一拖一驱动，常用的对象字典和PDO映射如下

ec_pdo_entry_info_t inovance_entries[] = {

    //write

    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )

    {0x607A, 0x00, 32}, //目标位置

    {0x6060, 0x00, 8}, //控制模式

    {0x60B1, 0x00, 32},// 速度漂移

    {0x60B2, 0x00, 16},//扭矩漂移

    // read

    {0x6041, 0x00, 16},//状态字

    {0x6064, 0x00, 32},//实际位置(编码器值)

    {0x606C, 0x00, 32}, //实际速度

    {0x6077, 0x00, 16}, //实际扭矩

    {0x60F4, 0x00, 32}, // 实际跟随误差

    {0x603F, 0x00, 16},//错误码

};

ec_pdo_info_t inovance_pdos[] = {

    {0x1600, 5, inovance_entries + 0},

    {0x1a00, 6, inovance_entries + 5}

};

ec_sync_info_t inovance_syncs[] = {

    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

    {2, EC_DIR_OUTPUT, 1, inovance_pdos + 0, EC_WD_ENABLE},

    {3, EC_DIR_INPUT, 1, inovance_pdos + 1, EC_WD_DISABLE},

    {0xff}

};

通过读写这些对象字典，就可以实现对伺服电机单个轴的控制。

清能德创一拖多的驱动器，实际控制上面和一拖一的驱动器是一样的。因为它驱动器内部一个电机仍然存在一个独立的ethercat节点。只是外观上用大铁盒封在了一起而已。

其它品牌一拖多的驱动器，比如：摩通、新时达等，一个ethercat节点可以控制2个以上的驱动器。那控制上面怎么处理呢？

下面是新时达一拖四驱动器，使用的PDO映射。

ec_pdo_entry_info_t step4channel_pdo_entries[] = {

    {0x6040, 0x00, 16}, /* Control Word */

    {0x6060, 0x00, 8}, /* Modes of Operation */

    {0x6098, 0x00, 8}, /* Homing method */

    {0x607a, 0x00, 32}, /* Target Position */

    {0x6840, 0x00, 16}, /* Control Word */

    {0x6860, 0x00, 8}, /* Modes of Operation */

    {0x6898, 0x00, 8}, /* Homing method */

    {0x687a, 0x00, 32}, /* Target Position */

    {0x7040, 0x00, 16}, /* Control Word */

    {0x7060, 0x00, 8}, /* Modes of Operation */

    {0x7098, 0x00, 8}, /* Homing method */

    {0x707a, 0x00, 32}, /* Target Position */

    {0x7840, 0x00, 16}, /* Control Word */

    {0x7860, 0x00, 8}, /* Modes of Operation */

    {0x7898, 0x00, 8}, /* Homing method */

    {0x787a, 0x00, 32}, /* Target Position */

    {0x6041, 0x00, 16}, /* Status Word */

    {0x603f, 0x00, 16}, /* Error Code */

    {0x6061, 0x00, 8}, /* Modes of Operation Display */

    {0x6064, 0x00, 32}, /* Position Actual Value */

    {0x606c, 0x00, 32}, /* Velocity Actual Value */

    {0x6841, 0x00, 16}, /* Status Word */

    {0x683f, 0x00, 16}, /* Error Code */

    {0x6861, 0x00, 8}, /* Modes of Operation Display */

    {0x6864, 0x00, 32}, /* Position Actual Value */

    {0x686c, 0x00, 32}, /* Velocity Actual Value */

    {0x7041, 0x00, 16}, /* Status Word */

    {0x703f, 0x00, 16}, /* Error Code */

    {0x7061, 0x00, 8}, /* Modes of Operation Display */

    {0x7064, 0x00, 32}, /* Position Actual Value */

    {0x706c, 0x00, 32}, /* Velocity Actual Value */

    {0x7841, 0x00, 16}, /* Status Word */

    {0x783f, 0x00, 16}, /* Error Code */

    {0x7861, 0x00, 8}, /* Modes of Operation Display */

    {0x7864, 0x00, 32}, /* Position Actual Value */

    {0x786c, 0x00, 32}, /* Velocity Actual Value */

};

ec_pdo_info_t step4channel_pdos[] = {

    {0x1600, 4, step4channel_pdo_entries + 0}, /* RxPDO0 */

    {0x1601, 4, step4channel_pdo_entries + 4}, /* RxPDO1 */

    {0x1602, 4, step4channel_pdo_entries + 8}, /* RxPDO2 */

    {0x1603, 4, step4channel_pdo_entries + 12}, /* RxPDO3 */

    {0x1a00, 5, step4channel_pdo_entries + 16}, /* TxPDO0 */

    {0x1a01, 5, step4channel_pdo_entries + 21}, /* TxPDO1 */

    {0x1a02, 5, step4channel_pdo_entries + 26}, /* TxPDO2 */

    {0x1a03, 5, step4channel_pdo_entries + 31}, /* TxPDO3 */

};

ec_sync_info_t step4channel_syncs[] = {

    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

    {2, EC_DIR_OUTPUT, 4, step4channel_pdos + 0, EC_WD_ENABLE},

    {3, EC_DIR_INPUT, 4, step4channel_pdos + 4, EC_WD_DISABLE},

    {0xff}

};

我们可以看到规律，它基于标准CIA402的规定:

第二个轴的对象字典地址+0x800

第三个轴的对象字典地址+0x1000

第四个轴的对象字典地址+0x1800

通过这样的扩展，实现通过一个ethercat节点，控制多个独立电机。从控制逻辑上和一拖一的驱动器是一样的，只是映射地址有偏移而已。

在比如摩通一托二驱动器PDO映射如下：

ec_pdo_entry_info_t motong2ch_pdo_entries[] = {

    {0x6040, 0x00, 16}, /* 0#Control Word */

    {0x607a, 0x00, 32}, /* Target Position */

    {0x6840, 0x00, 16}, /* 1#Control Word */

    {0x687a, 0x00, 32}, /* Target Position */

    {0x6041, 0x00, 16}, /* 0#Status Word */

    {0x603f, 0x00, 16}, /* Error Code */

    {0x6064, 0x00, 32}, /* Position Actual Value */

    {0x60f4, 0x00, 32}, /* Following Error Actual Value */

    {0x606c, 0x00, 32}, /* Velocity Actual Value */

    {0x6077, 0x00, 16}, /* Torque Actual Value */

    {0x6841, 0x00, 16}, /* 1#Status Word */

    {0x683f, 0x00, 16}, /* Error Code */

    {0x6864, 0x00, 32}, /* Position Actual Value */

    {0x68f4, 0x00, 32}, /* Following Error Actual Value */

    {0x686c, 0x00, 32}, /* Velocity Actual Value */

    {0x6877, 0x00, 16}, /* Torque Actual Value */

};

ec_pdo_info_t motong2ch_pdos[] = {

    {0x1603, 2, motong2ch_pdo_entries + 0}, /* csp RxPDO */

    {0x1613, 2, motong2ch_pdo_entries + 2}, /* csp RxPDO */

    {0x1a03, 6, motong2ch_pdo_entries + 4}, /* csp TxPDO */

    {0x1a13, 6, motong2ch_pdo_entries + 10}, /* csp TxPDO */

};

ec_sync_info_t motong2ch_syncs[] = {

    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},

    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},

    {2, EC_DIR_OUTPUT, 2, motong2ch_pdos + 0, EC_WD_ENABLE},

    {3, EC_DIR_INPUT, 2, motong2ch_pdos + 2, EC_WD_DISABLE},

    {0xff}

};

仍然第二个轴的对象字典地址+0x800

代码实现：

在代码实现上面，做算法控制的人是不想关心底层驱动到底是一托一还是一托多的驱动，所以在软件构架实现上面要实现分层隔离。我们构建了逻辑轴和物理轴的概念：

逻辑轴：控制算法的控制对象和控制接口是逻辑轴。比如目标位置数据等。

物理轴：是实现控制数据和物理电机的传达交互。比如将目标位置执行到电机的实际角度上。

我们首先利用robotInfo.xml配置文件定义逻辑轴和物理轴的映射关系：

80

这样一个xml格式的节点代表逻辑轴1对应ethercat协议的别名80的驱动器的通道0的电机。

创建逻辑轴类：

class LogicMotor

{

public:

    LogicMotor(E_DEVICE_PROTOCOL protocolIn, int aliasIn,int busIdIn,E_VIRTUAL_CTRL virtualCtrlFlagIn);

}

再创建物理轴类：

class EcatMotor

{

public:

    EcatMotor(E_DEVICE_PROTOCOL protocolIn, int aliasIn,int busIdIn,int maxChannelIn,

              E_VIRTUAL_CTRL virtualCtrlFlagIn);

LogicMotor* logicMotors[D_MAX_MOTOR_CHANNEL];

}

物理轴类有逻辑轴的成员变量。

程序首先初始化逻辑轴类，然后根据映射关系创建物理轴类。

整个控制流程：定时周期线程，算法计算一步控制逻辑轴，物理轴类实现一次数据交互，将电机反馈数据写入逻辑轴，将逻辑轴的数据写入单个电机vx18820959358。