2014年9月30日 星期二

Ethercat + Raspberry Pi + ASDA A2 Servo Motor = ?

        終於成功在 Raspberry Pi 上透過 Ethercat 控制伺服馬達,其實程式寫起來並不會很複雜,是我自己卡在 ASDA A2 的 Profile Velocity Mode 的 OD 0x6080 (馬達最高轉速) 似乎不能改,所以馬達速度一直上不去,讓我一直誤認為程式有問題。 (我不確定是否和 realtime 有關,我認為應該不是 )

        這裡就直接公開可以成功運行的程式碼,我是直接從 Igh Ethercat 的 \example\user\main.c 下去改,使用 Profile Position Mode 。由於我沒有研究 Ethercat 命令流程,所以正確的命令流程可能不是我程式寫得那樣,不過 Ethercat 相關函式的呼叫應該相去不遠,有興趣可以再修改。

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>

/****************************************************************************/

#include "ecrt.h"

/****************************************************************************/

// Application parameters
#define FREQUENCY 100
#define PRIORITY 1

/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domainInput = NULL;
static ec_domain_state_t domainInput_state = {};

static ec_domain_t *domainOutput = NULL;
static ec_domain_state_t domainOutput_state = {};

static ec_slave_config_t *sc_motor = NULL;
static ec_slave_config_state_t sc_motor_state = {};

// Timer
static unsigned int sig_alarms = 0;
static unsigned int user_alarms = 0;

/****************************************************************************/

// process data
static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

#define MotorSlavePos 0, 0
#define Delta_ASDAA2 0x000001dd, 0x10305070

// offsets for PDO entries
static unsigned int asdaa2_cntlwd;
static unsigned int asdaa2_tarvel;
static unsigned int asdaa2_tarpos;
static unsigned int asdaa2_provel;
static unsigned int asdaa2_proacc;
static unsigned int asdaa2_prodec;
static unsigned int asdaa2_maxvel;
static unsigned int asdaa2_maxspd;
static unsigned int asdaa2_mottyp;
static unsigned int asdaa2_modeop;
static unsigned int asdaa2_status;
static unsigned int asdaa2_actpos;
static unsigned int asdaa2_actvel;
static unsigned int asdaa2_acttor;
static unsigned int asdaa2_modedp;

const static ec_pdo_entry_reg_t domainOutput_regs[] = {
 { MotorSlavePos, Delta_ASDAA2, 0x607F, 0, &asdaa2_maxvel },
 { MotorSlavePos, Delta_ASDAA2, 0x6080, 0, &asdaa2_maxspd },
 { MotorSlavePos, Delta_ASDAA2, 0x60FF, 0, &asdaa2_tarvel },
 { MotorSlavePos, Delta_ASDAA2, 0x607A, 0, &asdaa2_tarpos },
 { MotorSlavePos, Delta_ASDAA2, 0x6081, 0, &asdaa2_provel },
 { MotorSlavePos, Delta_ASDAA2, 0x6040, 0, &asdaa2_cntlwd },
 { MotorSlavePos, Delta_ASDAA2, 0x6086, 0, &asdaa2_mottyp },
 { MotorSlavePos, Delta_ASDAA2, 0x6060, 0, &asdaa2_modeop },
 {}
};

const static ec_pdo_entry_reg_t domainInput_regs[] = {
 { MotorSlavePos, Delta_ASDAA2, 0x6064, 0, &asdaa2_actpos },
 { MotorSlavePos, Delta_ASDAA2, 0x606C, 0, &asdaa2_actvel },
 { MotorSlavePos, Delta_ASDAA2, 0x6041, 0, &asdaa2_status },
 { MotorSlavePos, Delta_ASDAA2, 0x6077, 0, &asdaa2_acttor },
 { MotorSlavePos, Delta_ASDAA2, 0x6061, 0, &asdaa2_modedp },
 {}
};

static unsigned int counter = 0;

/*****************************************************************************/
static ec_pdo_entry_info_t asdaa2_pdo_entries_output[] = {
 { 0x607F, 0x00, 32 },
 { 0x6080, 0x00, 32 },
 { 0x60FF, 0x00, 32 },
 { 0x607A, 0x00, 32 },
 { 0x6081, 0x00, 32 },
 { 0x6040, 0x00, 16 },
 { 0x6086, 0x00, 16 },
 { 0x6060, 0x00, 8 },
};

static ec_pdo_entry_info_t asdaa2_pdo_entries_input[] = {
 { 0x6064, 0x00, 32 },
 { 0x606C, 0x00, 32 },
 { 0x6041, 0x00, 16 },
 { 0x6077, 0x00, 16 },
 { 0x6061, 0x00, 8 },
};

static ec_pdo_info_t asdaa2_pdo_1600[] = {
 { 0x1600, 8, asdaa2_pdo_entries_output },
};

static ec_pdo_info_t asdaa2_pdo_1a00[] = {
 { 0x1A00, 5, asdaa2_pdo_entries_input },
};

static ec_sync_info_t asdaa2_syncs[] = {
 { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE },
 { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },
 { 2, EC_DIR_OUTPUT, 1, asdaa2_pdo_1600, EC_WD_DISABLE },
 { 3, EC_DIR_INPUT, 1, asdaa2_pdo_1a00, EC_WD_DISABLE },
 { 0xff }
};

/*****************************************************************************/

void check_domain_state(void)
{
 ec_domain_state_t ds;

 ecrt_domain_state(domainOutput, &ds);

 if (ds.working_counter != domainOutput_state.working_counter)
  printf("domainOutput: WC %u.\n", ds.working_counter);
 if (ds.wc_state != domainOutput_state.wc_state)
  printf("domainOutput: State %u.\n", ds.wc_state);

 domainOutput_state = ds;

 ecrt_domain_state(domainInput, &ds);

 if (ds.working_counter != domainInput_state.working_counter)
  printf("domainInput: WC %u.\n", ds.working_counter);
 if (ds.wc_state != domainInput_state.wc_state)
  printf("domainInput: State %u.\n", ds.wc_state);

 domainInput_state = ds;
}

/*****************************************************************************/

void check_master_state(void)
{
 ec_master_state_t ms;

 ecrt_master_state(master, &ms);

 if (ms.slaves_responding != master_state.slaves_responding)
  printf("%u slave(s).\n", ms.slaves_responding);
 if (ms.al_states != master_state.al_states)
  printf("AL states: 0x%02X.\n", ms.al_states);
 if (ms.link_up != master_state.link_up)
  printf("Link is %s.\n", ms.link_up ? "up" : "down");

 master_state = ms;
}

/*****************************************************************************/

void check_slave_config_states(void)
{
 ec_slave_config_state_t s;
 ecrt_slave_config_state(sc_motor, &s);

 if (s.al_state != sc_motor_state.al_state)
  printf("Motor: State 0x%02X.\n", s.al_state);
 if (s.online != sc_motor_state.online)
  printf("Motor: %s.\n", s.online ? "online" : "offline");
 if (s.operational != sc_motor_state.operational)
  printf("Motor: %soperational.\n",
  s.operational ? "" : "Not ");

 sc_motor_state = s;

}

/****************************************************************************/

void cyclic_task()
{
 // receive process data
 ecrt_master_receive(master);
 ecrt_domain_process(domainOutput);
 ecrt_domain_process(domainInput);

 // check process data state (optional)
 check_domain_state();

 if (counter) {
  counter--;
 }
 else { // do this at 1 Hz
  counter = FREQUENCY;

  // check for master state (optional)
  check_master_state();

  // check for islave configuration state(s) (optional)
  check_slave_config_states();

  printf("pos = %d\n", EC_READ_S32(domainInput_pd + asdaa2_actpos));
  printf("vel = %d\n", EC_READ_S32(domainInput_pd + asdaa2_actvel));
 }

 static int state = -500;
 state++;
 switch (state){
 case -1:
  printf("-1\n");
  EC_WRITE_U16(domainOutput_pd + asdaa2_cntlwd, 0x80);
  break;
 case 0:
  printf("0\n");
  EC_WRITE_S8(domainOutput_pd + asdaa2_modeop, 1);
  EC_WRITE_U32(domainOutput_pd + asdaa2_provel, 9000000);
  break;
 case 3:
  printf("3\n");
  EC_WRITE_U16(domainOutput_pd + asdaa2_cntlwd, 0x06);
  break;
 case 4:
  printf("4\n");
  EC_WRITE_U16(domainOutput_pd + asdaa2_cntlwd, 0x07);
  break;
 case 5:
  printf("5\n");
  EC_WRITE_U16(domainOutput_pd + asdaa2_cntlwd, 0xF);
  break;
 }

 if (state > 100){
  if (state % 300 == 0)
   EC_WRITE_U16(domainOutput_pd + asdaa2_cntlwd, 0xB);
  if (state % 300 == 5 && state % 600 != 5){
   printf(" \n");
   EC_WRITE_S32(domainOutput_pd + asdaa2_tarpos, 0x11111111);
  }
  else if (state % 600 == 5){
   printf("-\n");
   EC_WRITE_S32(domainOutput_pd + asdaa2_tarpos, 0xFFFFFFFFEEEEEEEF);
  }
  if (state % 300 == 6)
   EC_WRITE_U16(domainOutput_pd + asdaa2_cntlwd, 0x7F);
 }

 // send process data
 ecrt_domain_queue(domainOutput);
 ecrt_domain_queue(domainInput);
 ecrt_master_send(master);
}

/****************************************************************************/
int main(int argc, char **argv)
{
 // Requests an EtherCAT master for realtime operation.
 master = ecrt_request_master(
  0); // Index of the master to request.
 if (!master)
  return -1;

 // Creates a new process data domain
 domainOutput = ecrt_master_create_domain(master);
 if (!domainOutput)
  return -1;
 domainInput = ecrt_master_create_domain(master);
 if (!domainInput)
  return -1;

 // Obtains a slave configuration
 if (!(sc_motor = ecrt_master_slave_config(
  master, MotorSlavePos, Delta_ASDAA2))) {
  fprintf(stderr, "Failed to get slave configuration.\n");
  return -1;
 }

 // Configuring PDOs
 printf("Configuring PDOs...\n");
 if (ecrt_slave_config_pdos(sc_motor, EC_END, asdaa2_syncs)) {
  fprintf(stderr, "Failed to configure PDOs.\n");
  return -1;
 }

 if (ecrt_domain_reg_pdo_entry_list(domainOutput, domainOutput_regs)) {
  fprintf(stderr, "PDO entry registration failed!\n");
  return -1;
 }

 if (ecrt_domain_reg_pdo_entry_list(domainInput, domainInput_regs)) {
  fprintf(stderr, "PDO entry registration failed!\n");
  return -1;
 }

 printf("Activating master...\n");
 if (ecrt_master_activate(master))
  return -1;

 if (!(domainOutput_pd = ecrt_domain_data(domainOutput))) {
  return -1;
 }

 if (!(domainInput_pd = ecrt_domain_data(domainInput))) {
  return -1;
 }

 printf("Started.\n");
 while (1) {
  usleep(1000000 / FREQUENCY);
  cyclic_task();
 }
 return 0;
}

/****************************************************************************/
     

        以下是此程式運行的影片


18 則留言:

  1. 您好
    我按照您的這篇文章操作
    好像main.c裡面有一些錯誤 沒有辦法正確編譯過, 例如以下
    printf("pos = %d\n", EC_READ_S32(domainInput_pd asdaa2_actpos));
    這個應該改成以下嗎?
    printf("pos = %d\n", EC_READ_S32(domainInput_pd + asdaa2_actpos));

    另外
    這個Sample Code會將Delta A2E Servo-ON嗎?

    回覆刪除
  2. 會 Servo-ON 哦! asdaa2_cntlwd 的部分就是在進行 Servo-ON 流程,
    這部分你可以比照一下台達的手冊,他有說明 Servo-ON 命令流程。
    然後 + 號不見是我轉貼到網頁的時候可能跑出亂碼消失了,感謝你提醒 ~

    回覆刪除
  3. 謝謝您,我還沒有捉到不能servoOn的原因
    畫面有看到有找到一個slave,可是沒有ServoOn
    位置速度也都是0,你覺得可能是什麼原因呢?

    回覆刪除
  4. 你可以先確定一下你的程式開啟後伺服馬達有沒有進入 OP mode。然後參考一下手冊 PP 模式的流程,他有說明 Servo ON 要下哪些 PDO Command,調整一下程式碼看看。或者先把下移動命令的那段程式碼刪掉,先試試看能不能 Servo ON/OFF 再加回去

    回覆刪除
  5. 您好, 我發現程式好像沒有正確地設定PDO Mapping, 我想請問您, 我根據您上一篇文章安裝了Ethercat Master, 且也確認了Master Status為running, 除此之外 我需要再作哪個步驟呢?我需要A2E的XML檔案嗎?

    回覆刪除
    回覆
    1. 不用,伺服馬達通常有支援可以變更 PDO mapping 的功能,在這種情況下 igh EtherCAT master 可以自動 config slave
      如果不行的話你試試看用 A2E 預設的幾個 PDO mapping (0x1600 ...) ,用那幾個設定看看

      刪除
  6. 安裝平台: 虛擬機 VMware Ubuntu
    VMware Network Adapter:
    Bridged: Connected directly to the physical network (Replicate physical network connection state)
    實體網路卡MAC: 10-BF-48-74-01-41
    Ubuntu: 00:0C:29:77:E9:F5 (eth0)
    VMware: 00-50-56-C0-00-08
    MASTER0_DEVICE要設定成哪個MAC Address?

    IgH master我已經安裝完成,請問要怎麼編譯sample code?
    我編譯IgH原始程式(/ethercat-hg/examples/user內的main.c):
    gcc main.c -o output
    回報以下錯誤:
    main.c:41:18: fatal error: ecrt.h: No such file or directory compilation terminated.

    將main改成板大的仍然回報錯誤,編譯指令需要打什麼?
    謝謝

    回覆刪除
  7. IgH Doc.安裝搞很久,謝謝分享安裝的步驟

    回覆刪除
    回覆
    1. 不好意思, 上面的問題已經解決.
      想請問你兩個問題:
      (1) 我目前沒有接任何slave, 所以才會沒有任何EtherCAT封包資料傳送出去? (我用wireshark沒發現任何EtherCAT封包)
      (2) 以上方程式範例, 請問發送控制馬達速度的封包, 是哪個函數指令? 我想把它改成, 即使沒有slave仍然會發送控制封包。
      (3) 你都是直接文字編輯程式, 還是使用CodeBlock等IDE開發, 並且設定lib link

      刪除
    2. 恩...我抓到ethercat封包了 走layer 2我一直以為layer 3; 所以篩選ip src

      刪除
    3. EtherCAT 是 layer 2 哦,他的 EtherType 我記得是 0x88A4,wireshark 應該可以分辨 EtherCAT 封包

      刪除
  8. 您好,請問state變量在初始化為-500后,就再沒有什麼地方賦值了。這樣是不是又問題,難道state一直沒變過?

    回覆刪除
    回覆
    1. 感謝提醒,程式碼的 + 號貼到部落格後全部消失了,之前有修正了一些,這一個忘掉了 XD

      刪除
  9. 請問您對igh ethercat的使用主要參考的什麼資料,看到您的代碼收穫很多。但是感覺要系統的學習還是很睏難,期待您的指點。

    回覆刪除
    回覆
    1. 他有一個 document 可以看,建議那個手冊可以先都看過,然後再找 EtherCAT protocol 相關的資料,就會知道每一個東西是在設定什麼了

      刪除
  10. 樓主您好 我最近也在研究關於EtherCAT的東西 但一直有個問題想跟您請教一下
    就是關於EtherCAT的CoE(Canopen Over EtherCAT)
    他這個功能的意思是他把Canopen的東西當作基礎 延伸出EtherCAT的功能
    還是他是指說只要是支援EtherCAT的主站 不管是Ethercat或是Canopen的從站都可以連接呢
    謝謝

    回覆刪除
  11. 我也編譯IgH原始程式(/ethercat-hg/examples/user內的main.c)
    出現 main.c:41:18: fatal error: ecrt.h: No such file or directory
    請教各位該如何解決?

    回覆刪除