fp@0: //---------------------------------------------------------------
fp@0: //
fp@0: //  m a i n . c
fp@0: //
fp@0: //  $LastChangedDate$
fp@0: //  $Author$
fp@0: //
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: #include <stdio.h>
fp@0: #include <string.h> // memset()
fp@0: #include <unistd.h> // usleep()
fp@0: #include <signal.h>
fp@0: 
fp@0: #include "ec_globals.h"
fp@0: #include "ec_master.h"
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void signal_handler(int);
fp@0: void write_data(unsigned char *);
fp@0: 
fp@0: int continue_running;
fp@0: unsigned short int word;
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int main(int argc, char **argv)
fp@0: {
fp@0:   EtherCAT_master_t master;
fp@0:   EtherCAT_command_t *cmd, *cmd2;
fp@0:   unsigned char data[256];
fp@0:   unsigned int i, number;
fp@0:   struct sigaction sa;
fp@0:   
fp@0:   sa.sa_handler = signal_handler;
fp@0:   sigaction(SIGINT, &sa, NULL);
fp@0: 
fp@0:   printf("CatEther-Testprogramm.\n");
fp@0: 
fp@0:   EtherCAT_master_init(&master, "eth1");
fp@0: 
fp@0:   if (EtherCAT_check_slaves(&master, NULL, 0) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR while searching for slaves!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (master.slave_count == 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: No slaves found!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   for (i = 0; i < master.slave_count; i++)
fp@0:   {
fp@0:     printf("Slave found: Type %02X, Revision %02X, Build %04X\n",
fp@0:            master.slaves[i].type, master.slaves[i].revision, master.slaves[i].build);
fp@0:   }
fp@0: 
fp@0:   printf("Writing Station addresses.\n");
fp@0: 
fp@0:   for (i = 0; i < master.slave_count; i++)
fp@0:   {
fp@0:     data[0] = i & 0x00FF;
fp@0:     data[1] = (i & 0xFF00) >> 8;
fp@0: 
fp@0:     cmd = EtherCAT_position_write(&master, 0 - i, 0x0010, 2, data);
fp@0:     
fp@0:     EtherCAT_send_receive(&master);
fp@0:     
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       fprintf(stderr, "ERROR: Slave did'n repond!\n");
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     EtherCAT_remove_command(&master, cmd);
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   for (i = 0; i < master.slave_count; i++)
fp@0:   {
fp@0:     printf("\nKlemme %i:\n", i);
fp@0: 
fp@0:     EtherCAT_read_slave_information(&master, i, 0x0008, &number);
fp@0:     printf("Vendor ID: 0x%04X (%i)\n", number, number);
fp@0: 
fp@0:     EtherCAT_read_slave_information(&master, i, 0x000A, &number);
fp@0:     printf("Product Code: 0x%04X (%i)\n", number, number);
fp@0: 
fp@0:     EtherCAT_read_slave_information(&master, i, 0x000E, &number);
fp@0:     printf("Revision Number: 0x%04X (%i)\n", number, number);
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("\nResetting FMMU's.\n");
fp@0: 
fp@0:   memset(data, 0x00, 256);
fp@0:   cmd = EtherCAT_broadcast_write(&master, 0x0600, 256, data);
fp@0:   EtherCAT_send_receive(&master);
fp@0: 
fp@0:   if (cmd->working_counter != master.slave_count)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n",
fp@0:             cmd->working_counter, master.slave_count);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(&master, cmd);
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Resetting Sync Manager channels.\n");
fp@0: 
fp@0:   memset(data, 0x00, 256);
fp@0:   cmd = EtherCAT_broadcast_write(&master, 0x0800, 256, data);
fp@0:   EtherCAT_send_receive(&master);
fp@0: 
fp@0:   if (cmd->working_counter != master.slave_count)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Not all slaves responded (%i of %i)!\n",
fp@0:             cmd->working_counter, master.slave_count);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(&master, cmd);
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting INIT state for devices.\n");
fp@0: 
fp@0:   if (EtherCAT_broadcast_state_change(&master, EC_STATE_INIT) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set INIT state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting PREOP state for bus coupler.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_PREOP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting Sync managers 0 and 1 of device 1.\n");
fp@0: 
fp@0:   data[0] = 0x00;
fp@0:   data[1] = 0x18;
fp@0:   data[2] = 0xF6;
fp@0:   data[3] = 0x00;
fp@0:   data[4] = 0x26;
fp@0:   data[5] = 0x00;
fp@0:   data[6] = 0x01;
fp@0:   data[7] = 0x00;
fp@0:   cmd = EtherCAT_write(&master, 0x0001, 0x0800, 8, data);
fp@0: 
fp@0:   data[0] = 0xF6;
fp@0:   data[1] = 0x18;
fp@0:   data[2] = 0xF6;
fp@0:   data[3] = 0x00;
fp@0:   data[4] = 0x22;
fp@0:   data[5] = 0x00;
fp@0:   data[6] = 0x01;
fp@0:   data[7] = 0x00;
fp@0:   cmd2 = EtherCAT_write(&master, 0x0001, 0x0808, 8, data);
fp@0: 
fp@0:   EtherCAT_send_receive(&master);
fp@0: 
fp@0:   if (cmd->working_counter != 1 || cmd2->working_counter != 1)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0: 
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(&master, cmd);
fp@0:   EtherCAT_remove_command(&master, cmd2);
fp@0: 
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting PREOP state for device 1.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_PREOP))
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting PREOP state for device 4.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_PREOP))
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0: #if 1
fp@0:   printf("Setting FMMU 0 of device 1.\n");
fp@0: 
fp@0:   data[0] = 0x00; // Logical start address [4]
fp@0:   data[1] = 0x00;
fp@0:   data[2] = 0x00;
fp@0:   data[3] = 0x00;
fp@0:   data[4] = 0x04; // Length [2]
fp@0:   data[5] = 0x00;
fp@0:   data[6] = 0x00; // Start bit
fp@0:   data[7] = 0x07; // End bit
fp@0:   data[8] = 0x00; // Physical start address [2]
fp@0:   data[9] = 0x10;
fp@0:   data[10] = 0x00; // Physical start bit
fp@0:   data[11] = 0x02; // Read/write enable
fp@0:   data[12] = 0x01; // channel enable [2]
fp@0:   data[13] = 0x00;
fp@0:   data[14] = 0x00; // Reserved [2]
fp@0:   data[15] = 0x00;
fp@0:   cmd = EtherCAT_write(&master, 0x0001, 0x0600, 16, data);
fp@0:   EtherCAT_send_receive(&master);
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",
fp@0:             cmd->working_counter);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(&master, cmd);
fp@0: #endif
fp@0: 
fp@0:   //----------
fp@0: 
fp@0: #if 1
fp@0:   printf("Setting FMMU 0 of device 4.\n");
fp@0: 
fp@0:   data[0] = 0x04; // Logical start address [4]
fp@0:   data[1] = 0x00;
fp@0:   data[2] = 0x00;
fp@0:   data[3] = 0x00;
fp@0:   data[4] = 0x01; // Length [2]
fp@0:   data[5] = 0x00;
fp@0:   data[6] = 0x00; // Start bit
fp@0:   data[7] = 0x07; // End bit
fp@0:   data[8] = 0x00; // Physical start address [2]
fp@0:   data[9] = 0x0F;
fp@0:   data[10] = 0x00; // Physical start bit
fp@0:   data[11] = 0x02; // Read/write enable
fp@0:   data[12] = 0x01; // channel enable [2]
fp@0:   data[13] = 0x00;
fp@0:   data[14] = 0x00; // Reserved [2]
fp@0:   data[15] = 0x00;
fp@0:   cmd = EtherCAT_write(&master, 0x0004, 0x0600, 16, data);
fp@0:   EtherCAT_send_receive(&master);
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",
fp@0:             cmd->working_counter);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(&master, cmd);
fp@0: #endif
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting Sync manager 2 of device 1.\n");
fp@0: 
fp@0:   data[0] = 0x00;
fp@0:   data[1] = 0x10;
fp@0:   data[2] = 0x04;
fp@0:   data[3] = 0x00;
fp@0:   data[4] = 0x24;
fp@0:   data[5] = 0x00;
fp@0:   data[6] = 0x01;
fp@0:   data[7] = 0x00;
fp@0:   cmd = EtherCAT_write(&master, 0x0001, 0x0810, 8, data);
fp@0:   EtherCAT_send_receive(&master);
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", cmd->working_counter);
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(&master, cmd);
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting Sync manager 0 for device 4.\n");
fp@0: 
fp@0:   data[0] = 0x00;
fp@0:   data[1] = 0x0F;
fp@0:   data[2] = 0x01;
fp@0:   data[3] = 0x00;
fp@0:   data[4] = 0x46; // 46
fp@0:   data[5] = 0x00;
fp@0:   data[6] = 0x01;
fp@0:   data[7] = 0x00;
fp@0:   cmd = EtherCAT_write(&master, 0x0004, 0x0800, 8, data);
fp@0: 
fp@0:   EtherCAT_send_receive(&master);
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0: 
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(&master, cmd);
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting SAVEOP state for bus coupler.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, 0x0000, EC_STATE_SAVEOP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting SAVEOP state for device 1.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_SAVEOP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting SAVEOP state for device 4.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_SAVEOP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting OP state for bus coupler.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[0], EC_STATE_OP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting OP state for device 1.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[1], EC_STATE_OP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Setting OP state for device 4.\n");
fp@0: 
fp@0:   if (EtherCAT_state_change(&master, &master.slaves[4], EC_STATE_OP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   word = 0;
fp@0: 
fp@0:   printf("Starting thread...\n");
fp@0: 
fp@0:   if (EtherCAT_start(&master, 5, write_data, NULL, 10000) != 0)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   continue_running = 1;
fp@0: 
fp@0:   while (continue_running)
fp@0:   {
fp@0:     usleep(200000);
fp@0: 
fp@0:     word += 1000;
fp@0:     word = word & 0x7FFF;
fp@0:   }
fp@0: 
fp@0:   //----------
fp@0: 
fp@0:   printf("Stopping master thread...\n");
fp@0:   EtherCAT_stop(&master);
fp@0: 
fp@0:   EtherCAT_master_clear(&master);
fp@0: 
fp@0:   printf("Finished.\n");
fp@0:   
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void write_data(unsigned char *data)
fp@0: {
fp@0:   data[0] = word & 0xFF;
fp@0:   data[1] = (word & 0xFF00) >> 8;
fp@0:   data[2] = word & 0xFF;
fp@0:   data[3] = (word & 0xFF00) >> 8;
fp@0: 
fp@0:   data[4] = 0x01;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void signal_handler(int signum)
fp@0: {
fp@0:   if (signum == SIGINT || signum == SIGTERM)
fp@0:   {
fp@0:     continue_running = 0;
fp@0:   }
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------