2014-11-25 16:10:29 +01:00
|
|
|
/** \file
|
|
|
|
* \brief Example code for Simple Open EtherCAT master
|
|
|
|
*
|
|
|
|
* Usage : simple_test [ifname1]
|
|
|
|
* ifname is NIC interface, f.e. eth0
|
|
|
|
*
|
|
|
|
* This is a minimal test.
|
|
|
|
*
|
|
|
|
* (c)Arthur Ketels 2010 - 2011
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <string.h>
|
|
|
|
//#include <Mmsystem.h>
|
|
|
|
|
|
|
|
#include "osal.h"
|
|
|
|
#include "ethercattype.h"
|
|
|
|
#include "nicdrv.h"
|
|
|
|
#include "ethercatbase.h"
|
|
|
|
#include "ethercatmain.h"
|
|
|
|
#include "ethercatdc.h"
|
|
|
|
#include "ethercatcoe.h"
|
|
|
|
#include "ethercatfoe.h"
|
|
|
|
#include "ethercatconfig.h"
|
|
|
|
#include "ethercatprint.h"
|
|
|
|
|
|
|
|
#define EC_TIMEOUTMON 500
|
|
|
|
|
|
|
|
char IOmap[4096];
|
|
|
|
OSAL_THREAD_HANDLE thread1;
|
|
|
|
int expectedWKC;
|
|
|
|
boolean needlf;
|
|
|
|
volatile int wkc;
|
|
|
|
volatile int rtcnt;
|
|
|
|
boolean inOP;
|
|
|
|
uint8 currentgroup = 0;
|
|
|
|
|
|
|
|
/* most basic RT thread for process data, just does IO transfer */
|
|
|
|
void CALLBACK RTthread(UINT uTimerID, UINT uMsg, DWORD_PTR dwUser, DWORD_PTR dw1, DWORD_PTR dw2)
|
|
|
|
{
|
|
|
|
IOmap[0]++;
|
|
|
|
|
|
|
|
ec_send_processdata();
|
|
|
|
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
|
|
|
rtcnt++;
|
|
|
|
/* do RT control stuff here */
|
|
|
|
}
|
|
|
|
|
|
|
|
int EL7031setup(uint16 slave)
|
|
|
|
{
|
|
|
|
int retval;
|
|
|
|
uint16 u16val;
|
|
|
|
|
|
|
|
// map velocity
|
|
|
|
uint16 map_1c12[4] = {0x0003, 0x1601, 0x1602, 0x1604};
|
|
|
|
uint16 map_1c13[3] = {0x0002, 0x1a01, 0x1a03};
|
|
|
|
|
|
|
|
retval = 0;
|
|
|
|
|
|
|
|
// Set PDO mapping using Complete Access
|
|
|
|
// Strange, writing CA works, reading CA doesn't
|
2015-11-04 13:02:33 +01:00
|
|
|
// This is a protocol error of the slave.
|
2014-11-25 16:10:29 +01:00
|
|
|
retval += ec_SDOwrite(slave, 0x1c12, 0x00, TRUE, sizeof(map_1c12), &map_1c12, EC_TIMEOUTSAFE);
|
|
|
|
retval += ec_SDOwrite(slave, 0x1c13, 0x00, TRUE, sizeof(map_1c13), &map_1c13, EC_TIMEOUTSAFE);
|
|
|
|
|
|
|
|
// bug in EL7031 old firmware, CompleteAccess for reading is not supported even if the slave says it is.
|
|
|
|
ec_slave[slave].CoEdetails &= ~ECT_COEDET_SDOCA;
|
|
|
|
|
|
|
|
// set some motor parameters, just as example
|
|
|
|
u16val = 1200; // max motor current in mA
|
|
|
|
// retval += ec_SDOwrite(slave, 0x8010, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE);
|
|
|
|
u16val = 150; // motor coil resistance in 0.01ohm
|
|
|
|
// retval += ec_SDOwrite(slave, 0x8010, 0x04, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE);
|
|
|
|
|
|
|
|
// set other nescessary parameters as needed
|
|
|
|
// .....
|
|
|
|
|
|
|
|
while(EcatError) printf("%s", ec_elist2string());
|
|
|
|
|
|
|
|
printf("EL7031 slave %d set, retval = %d\n", slave, retval);
|
|
|
|
return 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
int AEPsetup(uint16 slave)
|
|
|
|
{
|
|
|
|
int retval;
|
|
|
|
uint8 u8val;
|
|
|
|
uint16 u16val;
|
|
|
|
|
|
|
|
retval = 0;
|
|
|
|
|
|
|
|
u8val = 0;
|
|
|
|
retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
|
|
|
|
u16val = 0x1603;
|
|
|
|
retval += ec_SDOwrite(slave, 0x1c12, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);
|
|
|
|
u8val = 1;
|
|
|
|
retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
|
|
|
|
|
|
|
|
u8val = 0;
|
|
|
|
retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
|
|
|
|
u16val = 0x1a03;
|
|
|
|
retval += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);
|
|
|
|
u8val = 1;
|
|
|
|
retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
|
|
|
|
|
|
|
|
u8val = 8;
|
|
|
|
retval += ec_SDOwrite(slave, 0x6060, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
|
|
|
|
|
|
|
|
// set some motor parameters, just as example
|
|
|
|
u16val = 1200; // max motor current in mA
|
|
|
|
// retval += ec_SDOwrite(slave, 0x8010, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE);
|
|
|
|
u16val = 150; // motor coil resistance in 0.01ohm
|
|
|
|
// retval += ec_SDOwrite(slave, 0x8010, 0x04, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTSAFE);
|
|
|
|
|
|
|
|
// set other nescessary parameters as needed
|
|
|
|
// .....
|
|
|
|
|
|
|
|
while(EcatError) printf("%s", ec_elist2string());
|
|
|
|
|
|
|
|
printf("AEP slave %d set, retval = %d\n", slave, retval);
|
|
|
|
return 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
void simpletest(char *ifname)
|
|
|
|
{
|
|
|
|
int i, j, oloop, iloop, wkc_count, chk, slc;
|
|
|
|
UINT mmResult;
|
|
|
|
|
|
|
|
needlf = FALSE;
|
|
|
|
inOP = FALSE;
|
|
|
|
|
|
|
|
printf("Starting simple test\n");
|
2015-11-04 13:02:33 +01:00
|
|
|
|
2014-11-25 16:10:29 +01:00
|
|
|
/* initialise SOEM, bind socket to ifname */
|
|
|
|
if (ec_init(ifname))
|
2015-11-04 13:02:33 +01:00
|
|
|
{
|
2014-11-25 16:10:29 +01:00
|
|
|
printf("ec_init on %s succeeded.\n",ifname);
|
|
|
|
/* find and auto-config slaves */
|
|
|
|
|
|
|
|
|
|
|
|
if ( ec_config_init(FALSE) > 0 )
|
|
|
|
{
|
|
|
|
printf("%d slaves found and configured.\n",ec_slavecount);
|
|
|
|
|
|
|
|
if((ec_slavecount > 1))
|
|
|
|
{
|
|
|
|
for(slc = 1; slc <= ec_slavecount; slc++)
|
|
|
|
{
|
|
|
|
// beckhoff EL7031, using ec_slave[].name is not very reliable
|
|
|
|
if((ec_slave[slc].eep_man == 0x00000002) && (ec_slave[slc].eep_id == 0x1b773052))
|
|
|
|
{
|
|
|
|
printf("Found %s at position %d\n", ec_slave[slc].name, slc);
|
|
|
|
// link slave specific setup to preop->safeop hook
|
|
|
|
ec_slave[slc].PO2SOconfig = &EL7031setup;
|
|
|
|
}
|
|
|
|
// Copley Controls EAP, using ec_slave[].name is not very reliable
|
|
|
|
if((ec_slave[slc].eep_man == 0x000000ab) && (ec_slave[slc].eep_id == 0x00000380))
|
|
|
|
{
|
|
|
|
printf("Found %s at position %d\n", ec_slave[slc].name, slc);
|
|
|
|
// link slave specific setup to preop->safeop hook
|
|
|
|
ec_slave[slc].PO2SOconfig = &AEPsetup;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
ec_config_map(&IOmap);
|
|
|
|
|
|
|
|
ec_configdc();
|
|
|
|
|
|
|
|
printf("Slaves mapped, state to SAFE_OP.\n");
|
|
|
|
/* wait for all slaves to reach SAFE_OP state */
|
|
|
|
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
|
|
|
|
|
|
|
|
oloop = ec_slave[0].Obytes;
|
|
|
|
if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
|
|
|
|
if (oloop > 8) oloop = 8;
|
|
|
|
iloop = ec_slave[0].Ibytes;
|
|
|
|
if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
|
|
|
|
if (iloop > 8) iloop = 8;
|
|
|
|
|
|
|
|
printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]);
|
|
|
|
|
|
|
|
printf("Request operational state for all slaves\n");
|
|
|
|
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
|
|
|
|
printf("Calculated workcounter %d\n", expectedWKC);
|
|
|
|
ec_slave[0].state = EC_STATE_OPERATIONAL;
|
|
|
|
/* send one valid process data to make outputs in slaves happy*/
|
|
|
|
ec_send_processdata();
|
|
|
|
ec_receive_processdata(EC_TIMEOUTRET);
|
|
|
|
|
|
|
|
/* start RT thread as periodic MM timer */
|
|
|
|
mmResult = timeSetEvent(1, 0, RTthread, 0, TIME_PERIODIC);
|
|
|
|
|
|
|
|
/* request OP state for all slaves */
|
|
|
|
ec_writestate(0);
|
|
|
|
chk = 40;
|
|
|
|
/* wait for all slaves to reach OP state */
|
|
|
|
do
|
|
|
|
{
|
|
|
|
ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
|
|
|
|
}
|
|
|
|
while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
|
|
|
|
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
|
|
|
|
{
|
|
|
|
printf("Operational state reached for all slaves.\n");
|
|
|
|
wkc_count = 0;
|
|
|
|
inOP = TRUE;
|
|
|
|
|
2015-11-04 13:02:33 +01:00
|
|
|
|
2014-11-25 16:10:29 +01:00
|
|
|
/* cyclic loop, reads data from RT thread */
|
|
|
|
for(i = 1; i <= 500; i++)
|
|
|
|
{
|
|
|
|
if(wkc >= expectedWKC)
|
|
|
|
{
|
|
|
|
printf("Processdata cycle %4d, WKC %d , O:", rtcnt, wkc);
|
|
|
|
|
|
|
|
for(j = 0 ; j < oloop; j++)
|
|
|
|
{
|
|
|
|
printf(" %2.2x", *(ec_slave[0].outputs + j));
|
|
|
|
}
|
|
|
|
|
2015-11-04 13:02:33 +01:00
|
|
|
printf(" I:");
|
2014-11-25 16:10:29 +01:00
|
|
|
for(j = 0 ; j < iloop; j++)
|
|
|
|
{
|
|
|
|
printf(" %2.2x", *(ec_slave[0].inputs + j));
|
2015-11-04 13:02:33 +01:00
|
|
|
}
|
2014-11-25 16:10:29 +01:00
|
|
|
printf(" T:%lld\r",ec_DCtime);
|
|
|
|
needlf = TRUE;
|
|
|
|
}
|
|
|
|
osal_usleep(50000);
|
2015-11-04 13:02:33 +01:00
|
|
|
|
2014-11-25 16:10:29 +01:00
|
|
|
}
|
|
|
|
inOP = FALSE;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
printf("Not all slaves reached operational state.\n");
|
|
|
|
ec_readstate();
|
|
|
|
for(i = 1; i<=ec_slavecount ; i++)
|
|
|
|
{
|
|
|
|
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
|
|
|
|
{
|
|
|
|
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
|
|
|
|
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
|
|
|
|
}
|
|
|
|
}
|
2015-11-04 13:02:33 +01:00
|
|
|
}
|
2014-11-25 16:10:29 +01:00
|
|
|
|
|
|
|
/* stop RT thread */
|
|
|
|
timeKillEvent(mmResult);
|
|
|
|
|
|
|
|
printf("\nRequest init state for all slaves\n");
|
|
|
|
ec_slave[0].state = EC_STATE_INIT;
|
|
|
|
/* request INIT state for all slaves */
|
|
|
|
ec_writestate(0);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
printf("No slaves found!\n");
|
|
|
|
}
|
|
|
|
printf("End simple test, close socket\n");
|
|
|
|
/* stop SOEM, close socket */
|
|
|
|
ec_close();
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
printf("No socket connection on %s\nExcecute as root\n",ifname);
|
2015-11-04 13:02:33 +01:00
|
|
|
}
|
|
|
|
}
|
2014-11-25 16:10:29 +01:00
|
|
|
|
2015-11-04 13:02:33 +01:00
|
|
|
//DWORD WINAPI ecatcheck( LPVOID lpParam )
|
|
|
|
OSAL_THREAD_FUNC ecatcheck(void *lpParam)
|
2014-11-25 16:10:29 +01:00
|
|
|
{
|
|
|
|
int slave;
|
|
|
|
|
|
|
|
while(1)
|
|
|
|
{
|
|
|
|
if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
|
|
|
|
{
|
|
|
|
if (needlf)
|
|
|
|
{
|
|
|
|
needlf = FALSE;
|
|
|
|
printf("\n");
|
|
|
|
}
|
|
|
|
/* one ore more slaves are not responding */
|
|
|
|
ec_group[currentgroup].docheckstate = FALSE;
|
|
|
|
ec_readstate();
|
|
|
|
for (slave = 1; slave <= ec_slavecount; slave++)
|
|
|
|
{
|
|
|
|
if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
|
|
|
|
{
|
|
|
|
ec_group[currentgroup].docheckstate = TRUE;
|
|
|
|
if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
|
|
|
|
{
|
|
|
|
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
|
|
|
|
ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
|
|
|
|
ec_writestate(slave);
|
|
|
|
}
|
|
|
|
else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
|
|
|
|
{
|
|
|
|
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
|
|
|
|
ec_slave[slave].state = EC_STATE_OPERATIONAL;
|
2015-11-04 13:02:33 +01:00
|
|
|
ec_writestate(slave);
|
2014-11-25 16:10:29 +01:00
|
|
|
}
|
|
|
|
else if(ec_slave[slave].state > 0)
|
|
|
|
{
|
|
|
|
if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
|
|
|
|
{
|
|
|
|
ec_slave[slave].islost = FALSE;
|
2015-11-04 13:02:33 +01:00
|
|
|
printf("MESSAGE : slave %d reconfigured\n",slave);
|
2014-11-25 16:10:29 +01:00
|
|
|
}
|
2015-11-04 13:02:33 +01:00
|
|
|
}
|
2014-11-25 16:10:29 +01:00
|
|
|
else if(!ec_slave[slave].islost)
|
|
|
|
{
|
|
|
|
/* re-check state */
|
|
|
|
ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
|
|
|
|
if (!ec_slave[slave].state)
|
|
|
|
{
|
|
|
|
ec_slave[slave].islost = TRUE;
|
2015-11-04 13:02:33 +01:00
|
|
|
printf("ERROR : slave %d lost\n",slave);
|
2014-11-25 16:10:29 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (ec_slave[slave].islost)
|
|
|
|
{
|
|
|
|
if(!ec_slave[slave].state)
|
|
|
|
{
|
|
|
|
if (ec_recover_slave(slave, EC_TIMEOUTMON))
|
|
|
|
{
|
|
|
|
ec_slave[slave].islost = FALSE;
|
2015-11-04 13:02:33 +01:00
|
|
|
printf("MESSAGE : slave %d recovered\n",slave);
|
2014-11-25 16:10:29 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
ec_slave[slave].islost = FALSE;
|
2015-11-04 13:02:33 +01:00
|
|
|
printf("MESSAGE : slave %d found\n",slave);
|
2014-11-25 16:10:29 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if(!ec_group[currentgroup].docheckstate)
|
|
|
|
printf("OK : all slaves resumed OPERATIONAL.\n");
|
|
|
|
}
|
|
|
|
osal_usleep(10000);
|
2015-11-04 13:02:33 +01:00
|
|
|
}
|
2014-11-25 16:10:29 +01:00
|
|
|
|
|
|
|
return 0;
|
2015-11-04 13:02:33 +01:00
|
|
|
}
|
2014-11-25 16:10:29 +01:00
|
|
|
|
|
|
|
char ifbuf[1024];
|
|
|
|
|
|
|
|
int main(int argc, char *argv[])
|
|
|
|
{
|
2015-11-04 13:02:33 +01:00
|
|
|
ec_adaptert * adapter = NULL;
|
2014-11-25 16:10:29 +01:00
|
|
|
printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
|
|
|
|
|
|
|
|
if (argc > 1)
|
2015-11-04 13:02:33 +01:00
|
|
|
{
|
2014-11-25 16:10:29 +01:00
|
|
|
/* create thread to handle slave error handling in OP */
|
|
|
|
osal_thread_create(&thread1, 128000, &ecatcheck, (void*) &ctime);
|
|
|
|
strcpy(ifbuf, argv[1]);
|
|
|
|
/* start cyclic part */
|
|
|
|
simpletest(ifbuf);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
printf("Usage: simple_test ifname1\n");
|
|
|
|
/* Print the list */
|
|
|
|
printf ("Available adapters\n");
|
|
|
|
adapter = ec_find_adapters ();
|
|
|
|
while (adapter != NULL)
|
|
|
|
{
|
|
|
|
printf ("Description : %s, Device to use for wpcap: %s\n", adapter->desc,adapter->name);
|
|
|
|
adapter = adapter->next;
|
|
|
|
}
|
2015-11-04 13:02:33 +01:00
|
|
|
}
|
|
|
|
|
2014-11-25 16:10:29 +01:00
|
|
|
printf("End program\n");
|
|
|
|
return (0);
|
|
|
|
}
|