ethercat主站開發(fā)之-----一拖多驅(qū)動(dòng)控制

ethercat伺服電機(jī)驅(qū)動(dòng)器使用過(guò)很多品牌vx18820959358，從最早起的松下、臺(tái)達(dá)、韓國(guó)Ls，到后續(xù)國(guó)產(chǎn)的匯川、清能德創(chuàng)、英威騰、高創(chuàng)、禾川、伊萊斯、睿能、新時(shí)達(dá)、摩通等。這些年幾乎是一步步見證國(guó)內(nèi)ethercat驅(qū)動(dòng)的蓬勃發(fā)展。早期一拖一的驅(qū)動(dòng)是主流，隨著機(jī)器人行業(yè)的成本競(jìng)爭(zhēng)的白熱化，一拖多驅(qū)動(dòng)驅(qū)動(dòng)由于成本優(yōu)勢(shì)，逐漸成為機(jī)器人行業(yè)的主流應(yīng)用選擇。

ethercat伺服驅(qū)動(dòng)基本遵循CIA402協(xié)議，約束0x6000范圍的對(duì)象字典的定義和功能。

比如：匯川一拖一驅(qū)動(dòng)，常用的對(duì)象字典和PDO映射如下

ec_pdo_entry_info_t inovance_entries[] = {

    //write

    {0x6040, 0x00, 16}, //控制字( 這個(gè)必須寫在開頭 )

    {0x607A, 0x00, 32}, //目標(biāo)位置

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

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

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

    // read

    {0x6041, 0x00, 16},//狀態(tài)字

    {0x6064, 0x00, 32},//實(shí)際位置(編碼器值)

    {0x606C, 0x00, 32}, //實(shí)際速度

    {0x6077, 0x00, 16}, //實(shí)際扭矩

    {0x60F4, 0x00, 32}, // 實(shí)際跟隨誤差

    {0x603F, 0x00, 16},//錯(cuò)誤碼

};

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}

};

通過(guò)讀寫這些對(duì)象字典，就可以實(shí)現(xiàn)對(duì)伺服電機(jī)單個(gè)軸的控制。

清能德創(chuàng)一拖多的驅(qū)動(dòng)器，實(shí)際控制上面和一拖一的驅(qū)動(dòng)器是一樣的。因?yàn)樗?qū)動(dòng)器內(nèi)部一個(gè)電機(jī)仍然存在一個(gè)獨(dú)立的ethercat節(jié)點(diǎn)。只是外觀上用大鐵盒封在了一起而已。

其它品牌一拖多的驅(qū)動(dòng)器，比如：摩通、新時(shí)達(dá)等，一個(gè)ethercat節(jié)點(diǎn)可以控制2個(gè)以上的驅(qū)動(dòng)器。那控制上面怎么處理呢？

下面是新時(shí)達(dá)一拖四驅(qū)動(dòng)器，使用的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}

};

我們可以看到規(guī)律，它基于標(biāo)準(zhǔn)CIA402的規(guī)定:

第二個(gè)軸的對(duì)象字典地址+0x800

第三個(gè)軸的對(duì)象字典地址+0x1000

第四個(gè)軸的對(duì)象字典地址+0x1800

通過(guò)這樣的擴(kuò)展，實(shí)現(xiàn)通過(guò)一個(gè)ethercat節(jié)點(diǎn)，控制多個(gè)獨(dú)立電機(jī)。從控制邏輯上和一拖一的驅(qū)動(dòng)器是一樣的，只是映射地址有偏移而已。

在比如摩通一托二驅(qū)動(dòng)器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}

};

仍然第二個(gè)軸的對(duì)象字典地址+0x800

代碼實(shí)現(xiàn)：

在代碼實(shí)現(xiàn)上面，做算法控制的人是不想關(guān)心底層驅(qū)動(dòng)到底是一托一還是一托多的驅(qū)動(dòng)，所以在軟件構(gòu)架實(shí)現(xiàn)上面要實(shí)現(xiàn)分層隔離。我們構(gòu)建了邏輯軸和物理軸的概念：

邏輯軸：控制算法的控制對(duì)象和控制接口是邏輯軸。比如目標(biāo)位置數(shù)據(jù)等。

物理軸：是實(shí)現(xiàn)控制數(shù)據(jù)和物理電機(jī)的傳達(dá)交互。比如將目標(biāo)位置執(zhí)行到電機(jī)的實(shí)際角度上。

我們首先利用robotInfo.xml配置文件定義邏輯軸和物理軸的映射關(guān)系：

80

這樣一個(gè)xml格式的節(jié)點(diǎn)代表邏輯軸1對(duì)應(yīng)ethercat協(xié)議的別名80的驅(qū)動(dòng)器的通道0的電機(jī)。

創(chuàng)建邏輯軸類：

class LogicMotor

{

public:

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

}

再創(chuàng)建物理軸類：

class EcatMotor

{

public:

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

              E_VIRTUAL_CTRL virtualCtrlFlagIn);

LogicMotor* logicMotors[D_MAX_MOTOR_CHANNEL];

}

物理軸類有邏輯軸的成員變量。

程序首先初始化邏輯軸類，然后根據(jù)映射關(guān)系創(chuàng)建物理軸類。

整個(gè)控制流程：定時(shí)周期線程，算法計(jì)算一步控制邏輯軸，物理軸類實(shí)現(xiàn)一次數(shù)據(jù)交互，將電機(jī)反饋數(shù)據(jù)寫入邏輯軸，將邏輯軸的數(shù)據(jù)寫入單個(gè)電機(jī)vx18820959358。