黄工无刷电机学习
直播中

张玉珍

7年用户 1041经验值
私信 关注
[问答]

EtherCAT igh主站是如何控制松下伺服(csp模式)的

EtherCAT igh主站是如何控制松下伺服(csp模式)的?怎样去编写其代码?

回帖(1)

李淑兰

2021-10-8 09:43:44
  完整代码
  #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 《sys/mman.h》
  #include 《rtdm/rtdm.h》
  #include 《native/task.h》
  #include 《native/sem.h》
  #include 《native/mutex.h》
  #include 《native/timer.h》
  #include 《rtdk.h》
  #include 《pthread.h》
  #include 《math.h》
  #define PI 3.1415926535898
  #include “ecrt.h”
  #define Bool int
  #define false 0
  #define true 1
  #define ETHERCAT_STATUS_OP 0x08
  #define STATUS_SERVO_ENABLE_BIT (0x04)
  //master status
  typedef enum _SysWorkingStatus
  {
  SYS_WORKING_POWER_ON,
  SYS_WORKING_SAFE_MODE,
  SYS_WORKING_OP_MODE,
  SYS_WORKING_LINK_DOWN,
  SYS_WORKING_IDLE_STATUS //系统空闲
  }SysWorkingStatus;
  typedef struct _GSysRunningParm
  {
  SysWorkingStatus m_gWorkStatus;
  }GSysRunningParm;
  GSysRunningParm gSysRunning;
  RT_TASK InterpolationTask;
  int run = 1;
  int ecstate = 0;
  #define CLOCK_TO_USE CLOCK_REALTIME
  #define NSEC_PER_SEC (1000000000L)
  #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
  static int64_t system_time_base = 0LL;
  //获取当前系统时间
  RTIME system_time_ns(void)
  {
  struct timespec rt_time;
  clock_gettime(CLOCK_TO_USE, &rt_time);
  RTIME time = TIMESPEC2NS(rt_time);
  return time - system_time_base;
  }
  /****************************************************************************/
  // EtherCAT
  ec_master_t *master = NULL;
  static ec_master_state_t master_state = {};
  static ec_domain_t *domainServoInput = NULL;
  static ec_domain_state_t domainServoInput_state = {};
  static ec_domain_t *domainServoOutput = NULL;
  static ec_domain_state_t domainServoOutput_state = {};
  static uint8_t *domainOutput_pd = NULL;
  static uint8_t *domainInput_pd = NULL;
  static ec_slave_config_t *sc_estun;
  static ec_slave_config_state_t sc_estun_state;
  /****************************************************************************/
  #define estun_Pos0 0, 0
  //#define estun 0x0000060a, 0x00000001
  #define estun 0x0000066f, 0x60380006
  // offsets for PDO entries
  static unsigned int cntlwd;
  static unsigned int ipData;
  static unsigned int modes_of_operation;
  static unsigned int status;
  static unsigned int actpos;
  static unsigned int modes_of_operation_display;
  static unsigned int Homing_method;
  static unsigned int speed_during_search_for_switch;
  static unsigned int speed_during_search_for_zero;
  static unsigned int homing_acceleration;
  static unsigned int home_offset;
  static unsigned int cur_status;
  static unsigned int cur_mode;
  // process data
  ec_pdo_entry_reg_t domainServoOutput_regs[] = {
  {estun_Pos0, estun, 0x6040, 0x00, &cntlwd, NULL},
  {estun_Pos0, estun, 0x607a, 0x00, &ipData, NULL},
  {estun_Pos0, estun, 0x6060, 0x00, &modes_of_operation, NULL}, //6060 模式选择
  {}
  };
  ec_pdo_entry_reg_t domainServoInput_regs[] = {
  {estun_Pos0, estun, 0x6064, 0x00, &actpos, NULL},
  {estun_Pos0, estun, 0x6041, 0x00, &status, NULL},
  {estun_Pos0, estun, 0x6061, 0x00, &modes_of_operation_display, NULL},
  {}
  };
  /****************************************************************************/
  /* Master 0, Slave 0, “MBDLN25BE”
  * Vendor ID: 0x0000066f
  * Product code: 0x60380006
  * Revision number: 0x00010000
  */
  ec_pdo_entry_info_t estun_pdo_entries[] = {
  {0x6040, 0x00, 16}, /* Controlword */
  {0x6060, 0x00, 8}, /* Modes of operation */
  {0x607a, 0x00, 32}, /* Target position */
  {0x60b8, 0x00, 16}, /* Touch probe function */
  {0x603f, 0x00, 16}, /* Error code */
  {0x6041, 0x00, 16}, /* Statusword */
  {0x6061, 0x00, 8}, /* Modes of operation display */
  {0x6064, 0x00, 32}, /* Position actual value */
  {0x60b9, 0x00, 16}, /* Touch probe status */
  {0x60ba, 0x00, 32}, /* Touch probe pos1 pos value */
  {0x60f4, 0x00, 32}, /* Following error actual value */
  {0x60fd, 0x00, 32}, /* Digital inputs */
  };
  ec_pdo_info_t estun_pdos[] = {
  {0x1600, 4, estun_pdo_entries + 0}, /* Receive PDO mapping 1 */
  {0x1a00, 8, estun_pdo_entries + 4}, /* Transmit PDO mapping 1 */
  };
  ec_sync_info_t estun_syncs[] = {
  {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
  {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
  {2, EC_DIR_OUTPUT, 1, estun_pdos + 0, EC_WD_ENABLE},
  {3, EC_DIR_INPUT, 1, estun_pdos + 1, EC_WD_DISABLE},
  {0xff}
  };
  /****************************************************************************/
  int ConfigPDO()
  {
  /********************/
  printf(“xenomai Configuring PDOs.。.n”);
  domainServoOutput = ecrt_master_create_domain(master);
  if (!domainServoOutput) {
  return -1;
  }
  domainServoInput = ecrt_master_create_domain(master);
  if (!domainServoInput) {
  return -1;
  }
  /********************/
  printf(“xenomai Creating slave configurations.。.n”);
  sc_estun =
  ecrt_master_slave_config(master, estun_Pos0, estun);
  if (!sc_estun) {
  fprintf(stderr, “Failed to get slave configuration.n”);
  return -1;
  }
  /********************/
  if (ecrt_slave_config_pdos(sc_estun, EC_END, estun_syncs)) {
  fprintf(stderr, “Failed to configure PDOs.n”);
  return -1;
  }
  /********************/
  if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
  fprintf(stderr, “PDO entry registration failed!n”);
  return -1;
  }
  if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
  fprintf(stderr, “PDO entry registration failed!n”);
  return -1;
  }
  fprintf(stderr, “Creating SDO requests.。.n”);
  // ecrt_slave_config_sdo8(sc_estun, 0x6060, 0, 8);
  // ecrt_slave_config_sdo8(sc_estun, 0x60C2, 1, 1);
  return 0;
  }
  /*****************************************************************************
  * Realtime task
  ****************************************************************************/
  void rt_check_domain_state(void)
  {
  ec_domain_state_t ds = {};
  ec_domain_state_t ds1 = {};
  //domainServoInput
  ecrt_domain_state(domainServoInput, &ds);
  if (ds.working_counter != domainServoInput_state.working_counter) {
  rt_printf(“domainServoInput: WC %u.n”, ds.working_counter);
  }
  if (ds.wc_state != domainServoInput_state.wc_state) {
  rt_printf(“domainServoInput: State %u.n”, ds.wc_state);
  }
  domainServoInput_state = ds;
  //domainServoOutput
  ecrt_domain_state(domainServoOutput, &ds1);
  if (ds1.working_counter != domainServoOutput_state.working_counter) {
  rt_printf(“domainServoOutput: WC %u.n”, ds1.working_counter);
  }
  if (ds1.wc_state != domainServoOutput_state.wc_state) {
  rt_printf(“domainServoOutput: State %u.n”, ds1.wc_state);
  }
  domainServoOutput_state = ds1;
  }
  /****************************************************************************/
  void rt_check_master_state(void)
  {
  ec_master_state_t ms;
  ecrt_master_state(master, &ms);
  if (ms.slaves_responding != master_state.slaves_responding) {
  rt_printf(“%u slave(s)。n”, ms.slaves_responding);
  }
  if (ms.al_states != master_state.al_states) {
  rt_printf(“AL states: 0x%02X.n”, ms.al_states);
  }
  if (ms.link_up != master_state.link_up) {
  rt_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_estun,&s);
  if (s.al_state != sc_estun_state.al_state)
  printf(“sc_estun_state: State 0x%02X.n”, s.al_state);
  if (s.online != sc_estun_state.online)
  printf(“sc_estun_state: %s.n”, s.online ? “online” : “offline”);
  if (s.operational != sc_estun_state.operational)
  printf(“sc_estun_state: %soperational.n”,s.operational ? “” : “Not ”);
  sc_estun_state = s;
  }
  /****************************************************************************/
  void ReleaseMaster()
  {
  if(master)
  {
  printf(“xenomai End of Program, release mastern”);
  ecrt_release_master(master);
  master = NULL;
  }
  }
  /****************************************************************************/
  int ActivateMaster()
  {
  int ret;
  printf(“xenomai Requesting master.。.n”);
  if(master)
  return 0;
  master = ecrt_request_master(0);
  if (!master) {
  return -1;
  }
  ConfigPDO();
  // configure SYNC signals for this slave
  ecrt_slave_config_dc(sc_estun, 0x0300, 1000000, 0, 0, 0);
  // ecrt_slave_config_dc(sc_estun, 0x0300, 4000000, 4000000/2, 0, 0);
  ecrt_master_application_time(master, system_time_ns());
  ret = ecrt_master_select_reference_clock(master, NULL);
  if (ret 《 0) {
  fprintf(stderr, “xenomai Failed to select reference clock: %sn”,
  strerror(-ret));
  return ret;
  }
  /********************/
  printf(“xenomai Activating master.。.n”);
  if (ecrt_master_activate(master)) {
  printf(“xenomai Activating master.。.failedn”);
  return -1;
  }
  /********************/
  if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
  fprintf(stderr, “xenomai Failed to get domain data pointer.n”);
  return -1;
  }
  if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
  fprintf(stderr, “xenomai Failed to get domain data pointer.n”);
  return -1;
  }
  printf(“xenomai Activating master.。.successn”);
  return 0;
  }
  /****************************************************************************/
  void DriverEtherCAT()
  {
  static int curpos = 0;
  static int curpos_offset = 0;
  static int i = 0;
  //处于刚开机(需要等待其他操作完成),返回等待下次周期
  if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
  return ;
  static int cycle_counter = 0;
  cycle_counter++;
  if(cycle_counter 》= 600*1000){
  cycle_counter = 0;
  run = 0;
  }
  // receive EtherCAT frames
  ecrt_master_receive(master);
  ecrt_domain_process(domainServoOutput);
  ecrt_domain_process(domainServoInput);
  rt_check_domain_state();
  if (!(cycle_counter % 500)) {
  rt_check_master_state();
  check_slave_config_states();
  }
  //状态机操作
  switch (gSysRunning.m_gWorkStatus)
  {
  case SYS_WORKING_SAFE_MODE:{
  //检查主站是否处于 OP 模式, 若不是,则调整为 OP 模式
  rt_check_master_state();
  check_slave_config_states();
  if((master_state.al_states & ETHERCAT_STATUS_OP))
  {
  int tmp = true;
  if(sc_estun_state.al_state != ETHERCAT_STATUS_OP)
  {
  tmp = false;
  break ;
  }
  if(tmp)
  {
  ecstate = 0;
  gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
  printf(“xenomai SYS_WORKING_OP_MODEn”);
  EC_WRITE_U16(domainOutput_pd + cntlwd, 0x80); //错误复位
  }
  }
  }break;
  case SYS_WORKING_OP_MODE:
  {
  EC_WRITE_U8(domainOutput_pd + modes_of_operation, 8);
  cur_mode= EC_READ_U8(domainInput_pd + modes_of_operation_display);
  printf(“curMode: %dt”, cur_mode); //当前操作模式
  cur_status = EC_READ_U16(domainInput_pd + status);
  printf(“curStatus: %dn”, cur_status);
  if((cur_status & 0x004f) == 0x0040)
  {
  EC_WRITE_U16(domainOutput_pd + cntlwd, 0x06);
  // cur_mode= EC_READ_U8(domainInput_pd + modes_of_operation_display);
  // printf(“curMode: %dn”, cur_mode); //当前操作模式
  printf(“0x06n”);
  }
  else if((cur_status & 0x006f) == 0x0021)
  {
  EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);
  printf(“0x07n”);
  }
  else if((cur_status & 0x006f) == 0x023)
  {
  EC_WRITE_U16(domainOutput_pd + cntlwd, 0x0F);
  printf(“0x0Fn”);
  }
  else if((cur_status & 0x006f) == 0x0027)
  {
  EC_WRITE_U16(domainOutput_pd + cntlwd, 0x001f);
  printf(“0x1fn”);
  curpos = EC_READ_S32(domainInput_pd + actpos);
  // EC_WRITE_S32(domainOutput_pd + ipData, EC_READ_S32(domainInput_pd + actpos));
  printf(“x@rtITP 》》》 Axis current position = %dn”, curpos);
  int tmp = true;
  if((EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)) == 0)
  {
  tmp = false;
  break ;
  }
  if(tmp)
  {
  ecstate = 0;
  gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
  printf(“xenomai SYS_WORKING_IDLE_STATUSn”);
  }
  }
  }break;
  default:
  {
  cur_status = EC_READ_U16(domainInput_pd + status);
  if (!(cycle_counter % 1000)) {
  printf(“curpos = %dt”,curpos);
  printf(“actpos.。. %dn”,EC_READ_S32(domainInput_pd + actpos));
  }
  curpos += 10000;
  EC_WRITE_S32(domainOutput_pd + ipData, curpos);
  }break;
  }
  // write application time to master
  ecrt_master_application_time(master, system_time_ns());
  ecrt_master_sync_reference_clock(master);
  ecrt_master_sync_slave_clocks(master);
  // send process data
  ecrt_domain_queue(domainServoOutput);
  ecrt_domain_queue(domainServoInput);
  ecrt_master_send(master);
  }
  /****************************************************************************/
  void InterpolationThread(void *arg)
  {
  RTIME wait, previous;
  previous = rt_timer_read();
  wait = previous;
  while (run) {
  wait += 1000000; //1ms
  //Delay the calling task (absolute).Delay the execution of the calling task until a given date is reached.
  rt_task_sleep_until(wait);
  DriverEtherCAT();
  }
  }
  /****************************************************************************
  * Signal handler
  ***************************************************************************/
  void signal_handler(int sig)
  {
  run = 0;
  }
  /****************************************************************************
  * Main function
  ***************************************************************************/
  int main(int argc, char *argv[])
  {
  int ret;
  /* Perform auto-init of rt_print buffers if the task doesn‘t do so */
  rt_print_auto_init(1);
  signal(SIGTERM, signal_handler);
  signal(SIGINT, signal_handler);
  mlockall(MCL_CURRENT | MCL_FUTURE);
  gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
  if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
  {
  ActivateMaster();
  ecstate = 0;
  gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
  printf(“xenomai SYS_WORKING_SAFE_MODEn”);
  }
  ret = rt_task_create(&InterpolationTask, “InterpolationTask”, 0, 99, T_FPU);
  if (ret 《 0) {
  fprintf(stderr, “xenomai Failed to create task: %sn”, strerror(-ret));
  return -1;
  }
  printf(“Starting InterpolationTask.。.n”);
  ret = rt_task_start(&InterpolationTask, &InterpolationThread, NULL);
  if (ret 《 0) {
  fprintf(stderr, “xenomai Failed to start task: %sn”, strerror(-ret));
  return -1;
  }
  while (run) {
  rt_task_sleep(50000000);
  }
  printf(“xenomai Deleting realtime InterpolationTask task.。.n”);
  rt_task_delete(&InterpolationTask);
  ReleaseMaster();
  return 0;
  }
举报

更多回帖

发帖
×
20
完善资料,
赚取积分