1 //---------------------------------------------------------------

     2 //

     3 //  m a i n . c

     4 //

     5 //  $LastChangedDate$

     6 //  $Author$

     7 //

     8 //---------------------------------------------------------------

     9 

    10 #include <stdio.h>

    11 #include <string.h> // memset()

    12 #include <unistd.h> // usleep()

    13 #include <signal.h>

    14 

    15 #include "ec_globals.h"

    16 #include "ec_master.h"

    17 

    18 //---------------------------------------------------------------

    19 

    20 void signal_handler(int);

    21 void write_data(unsigned char *);

    22 

    23 int continue_running;

    24 unsigned short int word;

    25 

    26 //---------------------------------------------------------------

    27 

    28 int main(int argc, char **argv)

    29 {

    30   EtherCAT_master_t master;

    31   EtherCAT_command_t *cmd, *cmd2;

    32   unsigned char data[256];

    33   unsigned int i, number;

    34   struct sigaction sa;

    35   

    36   sa.sa_handler = signal_handler;

    37   sigaction(SIGINT, &sa, NULL);

    38 

    39   printf("CatEther-Testprogramm.\n");

    40 

    41   EtherCAT_master_init(&master, "eth1");

    42 

    43   if (EtherCAT_check_slaves(&master, NULL, 0) != 0)

    44   {

    45     fprintf(stderr, "ERROR while searching for slaves!\n");

    46     return -1;

    47   }

    48 

    49   if (master.slave_count == 0)

    50   {

    51     fprintf(stderr, "ERROR: No slaves found!\n");

    52     return -1;

    53   }

    54 

    55   for (i = 0; i < master.slave_count; i++)

    56   {

    57     printf("Slave found: Type %02X, Revision %02X, Build %04X\n",

    58            master.slaves[i].type, master.slaves[i].revision, master.slaves[i].build);

    59   }

    60 

    61   printf("Writing Station addresses.\n");

    62 

    63   for (i = 0; i < master.slave_count; i++)

    64   {

    65     data[0] = i & 0x00FF;

    66     data[1] = (i & 0xFF00) >> 8;

    67 

    68     cmd = EtherCAT_position_write(&master, 0 - i, 0x0010, 2, data);

    69     

    70     EtherCAT_send_receive(&master);

    71     

    72     if (cmd->working_counter != 1)

    73     {

    74       fprintf(stderr, "ERROR: Slave did'n repond!\n");

    75       return -1;

    76     }

    77     

    78     EtherCAT_remove_command(&master, cmd);

    79   }

    80 

    81   //----------

    82 

    83   for (i = 0; i < master.slave_count; i++)

    84   {

    85     printf("\nKlemme %i:\n", i);

    86 

    87     EtherCAT_read_slave_information(&master, i, 0x0008, &number);

    88     printf("Vendor ID: 0x%04X (%i)\n", number, number);

    89 

    90     EtherCAT_read_slave_information(&master, i, 0x000A, &number);

    91     printf("Product Code: 0x%04X (%i)\n", number, number);

    92 

    93     EtherCAT_read_slave_information(&master, i, 0x000E, &number);

    94     printf("Revision Number: 0x%04X (%i)\n", number, number);

    95   }

    96 

    97   //----------

    98 

    99   printf("\nResetting FMMU's.\n");

   100 

   101   memset(data, 0x00, 256);

   102   cmd = EtherCAT_broadcast_write(&master, 0x0600, 256, data);

   103   EtherCAT_send_receive(&master);

   104 

   105   if (cmd->working_counter != master.slave_count)

   106   {

   107     fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n",

   108             cmd->working_counter, master.slave_count);

   109     return -1;

   110   }

   111 

   112   EtherCAT_remove_command(&master, cmd);

   113 

   114   //----------

   115 

   116   printf("Resetting Sync Manager channels.\n");

   117 

   118   memset(data, 0x00, 256);

   119   cmd = EtherCAT_broadcast_write(&master, 0x0800, 256, data);

   120   EtherCAT_send_receive(&master);

   121 

   122   if (cmd->working_counter != master.slave_count)

   123   {

   124     fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n",

   125             cmd->working_counter, master.slave_count);

   126     return -1;

   127   }

   128 

   129   EtherCAT_remove_command(&master, cmd);

   130 

   131   //----------

   132 

   133   printf("Setting INIT state for devices.\n");

   134 

   135   if (EtherCAT_broadcast_state_change(&master, EC_STATE_INIT) != 0)

   136   {

   137     fprintf(stderr, "ERROR: Could not set INIT state!\n");

   138     return -1;

   139   }

   140 

   141   //----------

   142 

   143   printf("Setting PREOP state for bus coupler.\n");

   144 

   145   if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_PREOP) != 0)

   146   {

   147     fprintf(stderr, "ERROR: Could not set state!\n");

   148     return -1;

   149   }

   150 

   151   //----------

   152 

   153   printf("Setting Sync managers 0 and 1 of device 1.\n");

   154 

   155   data[0] = 0x00;

   156   data[1] = 0x18;

   157   data[2] = 0xF6;

   158   data[3] = 0x00;

   159   data[4] = 0x26;

   160   data[5] = 0x00;

   161   data[6] = 0x01;

   162   data[7] = 0x00;

   163   cmd = EtherCAT_write(&master, 0x0001, 0x0800, 8, data);

   164 

   165   data[0] = 0xF6;

   166   data[1] = 0x18;

   167   data[2] = 0xF6;

   168   data[3] = 0x00;

   169   data[4] = 0x22;

   170   data[5] = 0x00;

   171   data[6] = 0x01;

   172   data[7] = 0x00;

   173   cmd2 = EtherCAT_write(&master, 0x0001, 0x0808, 8, data);

   174 

   175   EtherCAT_send_receive(&master);

   176 

   177   if (cmd->working_counter != 1 || cmd2->working_counter != 1)

   178   {

   179     fprintf(stderr, "ERROR: Not all slaves responded!\n");

   180 

   181     return -1;

   182   }

   183 

   184   EtherCAT_remove_command(&master, cmd);

   185   EtherCAT_remove_command(&master, cmd2);

   186 

   187 

   188   //----------

   189 

   190   printf("Setting PREOP state for device 1.\n");

   191 

   192   if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_PREOP))

   193   {

   194     fprintf(stderr, "ERROR: Could not set state!\n");

   195     return -1;

   196   }

   197 

   198   //----------

   199 

   200   printf("Setting PREOP state for device 4.\n");

   201 

   202   if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_PREOP))

   203   {

   204     fprintf(stderr, "ERROR: Could not set state!\n");

   205     return -1;

   206   }

   207 

   208   //----------

   209 

   210 #if 1

   211   printf("Setting FMMU 0 of device 1.\n");

   212 

   213   data[0] = 0x00; // Logical start address [4]

   214   data[1] = 0x00;

   215   data[2] = 0x00;

   216   data[3] = 0x00;

   217   data[4] = 0x04; // Length [2]

   218   data[5] = 0x00;

   219   data[6] = 0x00; // Start bit

   220   data[7] = 0x07; // End bit

   221   data[8] = 0x00; // Physical start address [2]

   222   data[9] = 0x10;

   223   data[10] = 0x00; // Physical start bit

   224   data[11] = 0x02; // Read/write enable

   225   data[12] = 0x01; // channel enable [2]

   226   data[13] = 0x00;

   227   data[14] = 0x00; // Reserved [2]

   228   data[15] = 0x00;

   229   cmd = EtherCAT_write(&master, 0x0001, 0x0600, 16, data);

   230   EtherCAT_send_receive(&master);

   231 

   232   if (cmd->working_counter != 1)

   233   {

   234     fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",

   235             cmd->working_counter);

   236     return -1;

   237   }

   238 

   239   EtherCAT_remove_command(&master, cmd);

   240 #endif

   241 

   242   //----------

   243 

   244 #if 1

   245   printf("Setting FMMU 0 of device 4.\n");

   246 

   247   data[0] = 0x04; // Logical start address [4]

   248   data[1] = 0x00;

   249   data[2] = 0x00;

   250   data[3] = 0x00;

   251   data[4] = 0x01; // Length [2]

   252   data[5] = 0x00;

   253   data[6] = 0x00; // Start bit

   254   data[7] = 0x07; // End bit

   255   data[8] = 0x00; // Physical start address [2]

   256   data[9] = 0x0F;

   257   data[10] = 0x00; // Physical start bit

   258   data[11] = 0x02; // Read/write enable

   259   data[12] = 0x01; // channel enable [2]

   260   data[13] = 0x00;

   261   data[14] = 0x00; // Reserved [2]

   262   data[15] = 0x00;

   263   cmd = EtherCAT_write(&master, 0x0004, 0x0600, 16, data);

   264   EtherCAT_send_receive(&master);

   265 

   266   if (cmd->working_counter != 1)

   267   {

   268     fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",

   269             cmd->working_counter);

   270     return -1;

   271   }

   272 

   273   EtherCAT_remove_command(&master, cmd);

   274 #endif

   275 

   276   //----------

   277 

   278   printf("Setting Sync manager 2 of device 1.\n");

   279 

   280   data[0] = 0x00;

   281   data[1] = 0x10;

   282   data[2] = 0x04;

   283   data[3] = 0x00;

   284   data[4] = 0x24;

   285   data[5] = 0x00;

   286   data[6] = 0x01;

   287   data[7] = 0x00;

   288   cmd = EtherCAT_write(&master, 0x0001, 0x0810, 8, data);

   289   EtherCAT_send_receive(&master);

   290 

   291   if (cmd->working_counter != 1)

   292   {

   293     fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", cmd->working_counter);

   294     return -1;

   295   }

   296 

   297   EtherCAT_remove_command(&master, cmd);

   298 

   299   //----------

   300 

   301   printf("Setting Sync manager 0 for device 4.\n");

   302 

   303   data[0] = 0x00;

   304   data[1] = 0x0F;

   305   data[2] = 0x01;

   306   data[3] = 0x00;

   307   data[4] = 0x46; // 46

   308   data[5] = 0x00;

   309   data[6] = 0x01;

   310   data[7] = 0x00;

   311   cmd = EtherCAT_write(&master, 0x0004, 0x0800, 8, data);

   312 

   313   EtherCAT_send_receive(&master);

   314 

   315   if (cmd->working_counter != 1)

   316   {

   317     fprintf(stderr, "ERROR: Not all slaves responded!\n");

   318 

   319     return -1;

   320   }

   321 

   322   EtherCAT_remove_command(&master, cmd);

   323 

   324   //----------

   325 

   326   printf("Setting SAVEOP state for bus coupler.\n");

   327 

   328   if (EtherCAT_state_change(&master, 0x0000, EC_STATE_SAVEOP) != 0)

   329   {

   330     fprintf(stderr, "ERROR: Could not set state!\n");

   331     return -1;

   332   }

   333 

   334   //----------

   335 

   336   printf("Setting SAVEOP state for device 1.\n");

   337 

   338   if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_SAVEOP) != 0)

   339   {

   340     fprintf(stderr, "ERROR: Could not set state!\n");

   341     return -1;

   342   }

   343 

   344   //----------

   345 

   346   printf("Setting SAVEOP state for device 4.\n");

   347 

   348   if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_SAVEOP) != 0)

   349   {

   350     fprintf(stderr, "ERROR: Could not set state!\n");

   351     return -1;

   352   }

   353 

   354   //----------

   355 

   356   printf("Setting OP state for bus coupler.\n");

   357 

   358   if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_OP) != 0)

   359   {

   360     fprintf(stderr, "ERROR: Could not set state!\n");

   361     return -1;

   362   }

   363 

   364   //----------

   365 

   366   printf("Setting OP state for device 1.\n");

   367 

   368   if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_OP) != 0)

   369   {

   370     fprintf(stderr, "ERROR: Could not set state!\n");

   371     return -1;

   372   }

   373 

   374   //----------

   375 

   376   printf("Setting OP state for device 4.\n");

   377 

   378   if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_OP) != 0)

   379   {

   380     fprintf(stderr, "ERROR: Could not set state!\n");

   381     return -1;

   382   }

   383 

   384   //----------

   385 

   386   word = 0;

   387 

   388   printf("Starting thread...\n");

   389 

   390   if (EtherCAT_start(&master, 5, write_data, NULL, 10000) != 0)

   391   {

   392     return -1;

   393   }

   394 

   395   continue_running = 1;

   396 

   397   while (continue_running)

   398   {

   399     usleep(200000);

   400 

   401     word += 1000;

   402     word = word & 0x7FFF;

   403   }

   404 

   405   //----------

   406 

   407   printf("Stopping master thread...\n");

   408   EtherCAT_stop(&master);

   409 

   410   EtherCAT_master_clear(&master);

   411 

   412   printf("Finished.\n");

   413   

   414   return 0;

   415 }

   416 

   417 //---------------------------------------------------------------

   418 

   419 void write_data(unsigned char *data)

   420 {

   421   data[0] = word & 0xFF;

   422   data[1] = (word & 0xFF00) >> 8;

   423   data[2] = word & 0xFF;

   424   data[3] = (word & 0xFF00) >> 8;

   425 

   426   data[4] = 0x01;

   427 }

   428 

   429 //---------------------------------------------------------------

   430 

   431 void signal_handler(int signum)

   432 {

   433   if (signum == SIGINT || signum == SIGTERM)

   434   {

   435     continue_running = 0;

   436   }

   437 }

   438 

   439 //---------------------------------------------------------------