void simpletest(char *ifname)
{
int i, j, oloop, iloop, wkc_count, chk;
needlf = FALSE;
inOP = FALSE;
printf("Starting simple test\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname))
{
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);
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);
/* request OP state for all slaves */
ec_writestate(0);
chk = 40;
/* wait for all slaves to reach OP state */
do
{
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
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;
/* cyclic loop */
for(i = 1; i <= 10000; i++)
{
ec_send_processdata();
wkc = ec_receive_processdata(EC_TIMEOUTRET);
if(wkc >= expectedWKC)
{
printf("Processdata cycle %4d, WKC %d , O:", i, wkc);
for(j = 0 ; j < oloop; j++)
{
printf(" %2.2x", *(ec_slave[0].outputs + j));
}
printf(" I:");
for(j = 0 ; j < iloop; j++)
{
printf(" %2.2x", *(ec_slave[0].inputs + j));
}
printf(" T:%lld\r",ec_DCtime);
needlf = TRUE;
}
usleep(5000);
}
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));
}
}
}
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);
}
}
void redtest(char *ifname, char *ifname2)
{
int cnt, i, j, oloop, iloop;
printf("Starting Redundant test\n");
/* initialise SOEM, bind socket to ifname */
// if (ec_init_redundant(ifname, ifname2))
if (ec_init(ifname))
{
printf("ec_init on %s succeeded.\n",ifname);
/* find and auto-config slaves */
if ( ec_config(FALSE, &IOmap) > 0 )
{
printf("%d slaves found and configured.\n",ec_slavecount);
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
/* configure DC options for every DC capable slave found in the list */
ec_configdc();
/* read indevidual slave state and store in ec_slave[] */
ec_readstate();
for(cnt = 1; cnt <= ec_slavecount ; cnt++)
{
printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
printf(" Out:%8.8x,%4d In:%8.8x,%4d\n",
(int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes);
/* check for EL2004 or EL2008 */
if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052)))
{
digout = ec_slave[cnt].outputs;
}
}
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
printf("Request operational state for all slaves\n");
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* request OP state for all slaves */
ec_writestate(0);
/* activate cyclic process data */
dorun = 1;
/* wait for all slaves to reach OP state */
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
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;
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
{
printf("Operational state reached for all slaves.\n");
inOP = TRUE;
/* acyclic loop 5000 x 20ms = 10s */
for(i = 1; i <= 5000; i++)
{
printf("Processdata cycle %5d , Wck %3d, DCtime %12lld, dt %12lld, O:",
dorun, wkc , ec_DCtime, gl_delta);
for(j = 0 ; j < oloop; j++)
{
printf(" %2.2x", *(ec_slave[0].outputs + j));
}
printf(" I:");
for(j = 0 ; j < iloop; j++)
{
printf(" %2.2x", *(ec_slave[0].inputs + j));
}
printf("\r");
fflush(stdout);
osal_usleep(20000);
}
dorun = 0;
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));
}
}
}
printf("Request safe operational state for all slaves\n");
ec_slave[0].state = EC_STATE_SAFE_OP;
/* request SAFE_OP state for all slaves */
ec_writestate(0);
}
else
{
printf("No slaves found!\n");
}
printf("End redundant test, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
void simpletest(char *ifname)
{
int i, j, oloop, iloop, wkc_count, chk, slc;
UINT mmResult;
needlf = FALSE;
inOP = FALSE;
printf("Starting simple test\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname))
{
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;
/* 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));
}
printf(" I:");
for(j = 0 ; j < iloop; j++)
{
printf(" %2.2x", *(ec_slave[0].inputs + j));
}
printf(" T:%lld\r",ec_DCtime);
needlf = TRUE;
}
osal_usleep(50000);
}
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));
}
}
}
/* 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);
}
}
void slaveinfo(char *ifname)
{
int cnt, i, j, nSM;
int ctx_count;
uint16 ssigen;
int expectedWKC[2];
int wkc_count;
int chk;
volatile int wkc[2];
boolean inOP;
printf("Starting slaveinfo\n");
/* initialise SOEM, bind socket to ifname */
if (ecx_init(&ctx[0],"ie1g1") && ecx_init(&ctx[1],"ie1g0"))
{
printf("ec_init on %s succeeded.\n",ifname);
/* find and auto-config slaves */
if ( ecx_config_init(&ctx[0],FALSE) > 0 && ecx_config_init(&ctx[1],FALSE) > 0 )
{
for (ctx_count = 0; ctx_count < 2; ctx_count++)
{
ecx_config_map_group(&ctx[ctx_count], IOmap, 0);
ecx_configdc(&ctx[ctx_count]);
while(*(ctx[ctx_count].ecaterror)) printf("%s", ecx_elist2string(&ctx[ctx_count]));
printf("%d slaves found and configured.\n",*(ctx[ctx_count].slavecount));
expectedWKC[ctx_count] = ( ctx[ctx_count].grouplist[0].outputsWKC * 2) + ctx[ctx_count].grouplist[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC[ctx_count]);
/* wait for all slaves to reach SAFE_OP state */
ecx_statecheck(&ctx[ctx_count],0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 3);
if (ctx[ctx_count].slavelist[ctx_count].state != EC_STATE_SAFE_OP )
{
printf("Not all slaves reached safe operational state.\n");
ecx_readstate(&ctx[ctx_count]);
for(i = 1; i <= *(ctx[ctx_count].slavecount) ; i++)
{
if(ctx[ctx_count].slavelist[i].state != EC_STATE_SAFE_OP)
{
printf("Slave %d State=%2x StatusCode=%4x : %s\n",
i, ctx[ctx_count].slavelist[i].state, ctx[ctx_count].slavelist[i].ALstatuscode, ec_ALstatuscode2string(ctx[ctx_count].slavelist[i].ALstatuscode));
}
}
}
ecx_readstate(&ctx[ctx_count]);
for( cnt = 1 ; cnt <= *(ctx[ctx_count].slavecount) ; cnt++)
{
printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n",
cnt, ctx[ctx_count].slavelist[cnt].name, ctx[ctx_count].slavelist[cnt].Obits, ctx[ctx_count].slavelist[cnt].Ibits,
ctx[ctx_count].slavelist[cnt].state, ctx[ctx_count].slavelist[cnt].pdelay, ctx[ctx_count].slavelist[cnt].hasdc);
if (ctx[ctx_count].slavelist[cnt].hasdc)
{
printf(" DCParentport:%d\n", ctx[ctx_count].slavelist[cnt].parentport);
}
printf(" Activeports:%d.%d.%d.%d\n", (ctx[ctx_count].slavelist[cnt].activeports & 0x01) > 0 ,
(ctx[ctx_count].slavelist[cnt].activeports & 0x02) > 0 ,
(ctx[ctx_count].slavelist[cnt].activeports & 0x04) > 0 ,
(ctx[ctx_count].slavelist[cnt].activeports & 0x08) > 0 );
printf(" Configured address: %4.4x\n", ctx[ctx_count].slavelist[cnt].configadr);
printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ctx[ctx_count].slavelist[cnt].eep_man,
(int)ctx[ctx_count].slavelist[cnt].eep_id, (int)ctx[ctx_count].slavelist[cnt].eep_rev);
for(nSM = 0 ; nSM < EC_MAXSM ; nSM++)
{
if(ctx[ctx_count].slavelist[cnt].SM[nSM].StartAddr > 0)
printf(" SM%1d A:%4.4x L:%4d F:%8.8x Type:%d\n",nSM, ctx[ctx_count].slavelist[cnt].SM[nSM].StartAddr,
ctx[ctx_count].slavelist[cnt].SM[nSM].SMlength,(int)ctx[ctx_count].slavelist[cnt].SM[nSM].SMflags,
ctx[ctx_count].slavelist[cnt].SMtype[nSM]);
}
for(j = 0 ; j < ctx[ctx_count].slavelist[cnt].FMMUunused ; j++)
{
printf(" FMMU%1d Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j,
(int)ctx[ctx_count].slavelist[cnt].FMMU[j].LogStart, ctx[ctx_count].slavelist[cnt].FMMU[j].LogLength,
ctx[ctx_count].slavelist[cnt].FMMU[j].LogStartbit, ctx[ctx_count].slavelist[cnt].FMMU[j].LogEndbit,
ctx[ctx_count].slavelist[cnt].FMMU[j].PhysStart, ctx[ctx_count].slavelist[cnt].FMMU[j].PhysStartBit,
ctx[ctx_count].slavelist[cnt].FMMU[j].FMMUtype, ctx[ctx_count].slavelist[cnt].FMMU[j].FMMUactive);
}
printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",
ctx[ctx_count].slavelist[cnt].FMMU0func, ctx[ctx_count].slavelist[cnt].FMMU2func, ctx[ctx_count].slavelist[cnt].FMMU2func,
ctx[ctx_count].slavelist[cnt].FMMU3func);
printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ctx[ctx_count].slavelist[cnt].mbx_l,
ctx[ctx_count].slavelist[cnt].mbx_rl, ctx[ctx_count].slavelist[cnt].mbx_proto);
ssigen = ecx_siifind(&ctx[ctx_count], cnt, ECT_SII_GENERAL);
/* SII general section */
if (ssigen)
{
ctx[ctx_count].slavelist[cnt].CoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x07);
ctx[ctx_count].slavelist[cnt].FoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x08);
ctx[ctx_count].slavelist[cnt].EoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x09);
ctx[ctx_count].slavelist[cnt].SoEdetails = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0a);
if((ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0d) & 0x02) > 0)
{
ctx[ctx_count].slavelist[cnt].blockLRW = 1;
ctx[ctx_count].slavelist[0].blockLRW++;
}
ctx[ctx_count].slavelist[cnt].Ebuscurrent = ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0e);
ctx[ctx_count].slavelist[cnt].Ebuscurrent += ecx_siigetbyte(&ctx[ctx_count], cnt, ssigen + 0x0f) << 8;
ctx[ctx_count].slavelist[0].Ebuscurrent += ctx[ctx_count].slavelist[cnt].Ebuscurrent;
}
printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n",
ctx[ctx_count].slavelist[cnt].CoEdetails, ctx[ctx_count].slavelist[cnt].FoEdetails, ctx[ctx_count].slavelist[cnt].EoEdetails, ctx[ctx_count].slavelist[cnt].SoEdetails);
printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n",
ctx[ctx_count].slavelist[cnt].Ebuscurrent, ctx[ctx_count].slavelist[cnt].blockLRW);
}
}
inOP = FALSE;
printf("Request operational state for all slaves\n");
expectedWKC[0] = (ctx[0].grouplist[0].outputsWKC * 2) + ctx[0].grouplist[0].inputsWKC;
expectedWKC[1] = (ctx[1].grouplist[0].outputsWKC * 2) + ctx[1].grouplist[0].inputsWKC;
printf("Calculated workcounter master 1 %d\n", expectedWKC[0]);
printf("Calculated workcounter master 2 %d\n", expectedWKC[1]);
ctx[0].slavelist[0].state = EC_STATE_OPERATIONAL;
ctx[1].slavelist[0].state = EC_STATE_OPERATIONAL;
/* send one valid process data to make outputs in slaves happy*/
ecx_send_processdata(&ctx[0]);
ecx_send_processdata(&ctx[1]);
ecx_receive_processdata(&ctx[0],EC_TIMEOUTRET);
ecx_receive_processdata(&ctx[1],EC_TIMEOUTRET);
/* request OP state for all slaves */
ecx_writestate(&ctx[0], 0);
ecx_writestate(&ctx[1], 0);
chk = 40;
/* wait for all slaves to reach OP state */
do
{
ecx_send_processdata(&ctx[0]);
ecx_send_processdata(&ctx[1]);
ecx_receive_processdata(&ctx[0],EC_TIMEOUTRET);
ecx_receive_processdata(&ctx[1],EC_TIMEOUTRET);
ecx_statecheck(&ctx[0],0, EC_STATE_OPERATIONAL, 50000);
ecx_statecheck(&ctx[1],0, EC_STATE_OPERATIONAL, 50000);
}
while (chk-- && (ctx[0].slavelist[0].state != EC_STATE_OPERATIONAL) && (ctx[1].slavelist[0].state != EC_STATE_OPERATIONAL));
*(ctx[0].slavelist[6].outputs) = 0xFF;
*(ctx[1].slavelist[1].outputs) = 0x0F;
if (ctx[0].slavelist[0].state == EC_STATE_OPERATIONAL && ctx[1].slavelist[0].state == EC_STATE_OPERATIONAL )
{
printf("Operational state reached for all slaves.\n");
/* cyclic loop */
start_RT_trace (1);
wkc_count = 0;
inOP = TRUE;
for(i = 1; i <= 10000; i++)
{
for (ctx_count = 0; ctx_count < 2; ctx_count++)
{
/* Receive processdata */
log_RT_event('R',(WORD)(1 + ctx_count * 10));
wkc[ctx_count] = ecx_receive_processdata(&ctx[ctx_count], 0);
log_RT_event('R',(WORD)(99 + ctx_count * 100));
}
for (ctx_count = 0; ctx_count < 2; ctx_count++)
{
/* Send processdata */
log_RT_event('S',(WORD)(1 + ctx_count * 10));
if (!ecx_send_processdata(&ctx[ctx_count]))
{
printf (" Frame no: %d on master %d,Not sentOK\n", i,ctx_count);
}
log_RT_event('S',(WORD)(99 + ctx_count * 100));
}
knRtSleep(1);
for (ctx_count = 0; ctx_count < 2; ctx_count++)
{
if(wkc[ctx_count] >= expectedWKC[ctx_count])
{
}
else
{
printf (" Frame no: %d on master %d , wc not wkc >= expectedWKC, %d\n",i, ctx_count, wkc[ctx_count]);
}
}
}
stop_RT_trace ();
inOP = FALSE;
}
}
else
{
printf("No slaves found!\n");
}
printf("End slaveinfo, close socket\n");
/* stop SOEM, close socket */
ecx_close(&ctx[0]);
ecx_close(&ctx[1]);
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
void slaveinfo(char *ifname)
{
int cnt, i, j, nSM;
uint16 ssigen, dtype;
printf("Starting slaveinfo\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname))
{
printf("ec_init on %s succeeded.\n",ifname);
/* find and auto-config slaves */
if ( ec_config(FALSE, &IOmap) > 0 )
{
printf("%d slaves found and configured.\n",ec_slavecount);
printf("Calculated workcounter %d\n",ec_group[0].expectedWKC);
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 3);
if (ec_slave[0].state != EC_STATE_SAFE_OP )
{
printf("Not all slaves reached safe operational state.\n");
ec_readstate();
for(i = 1; i<=ec_slavecount ; i++)
{
if(ec_slave[i].state != EC_STATE_SAFE_OP)
{
printf("Slave %d State=%2x StatusCode=%4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
ec_configdc();
ec_readstate();
for( cnt = 1 ; cnt <= ec_slavecount ; cnt++)
{
printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n",
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
if (ec_slave[cnt].hasdc) printf(" DCParentport:%d\n", ec_slave[cnt].parentport);
printf(" Activeports:%d.%d.%d.%d\n", (ec_slave[cnt].activeports & 0x01) > 0 ,
(ec_slave[cnt].activeports & 0x02) > 0 ,
(ec_slave[cnt].activeports & 0x04) > 0 ,
(ec_slave[cnt].activeports & 0x08) > 0 );
printf(" Configured address: %4.4x\n", ec_slave[cnt].configadr);
printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ec_slave[cnt].eep_man, (int)ec_slave[cnt].eep_id, (int)ec_slave[cnt].eep_rev);
for(nSM = 0 ; nSM < EC_MAXSM ; nSM++)
{
if(ec_slave[cnt].SM[nSM].StartAddr > 0)
printf(" SM%1d A:%4.4x L:%4d F:%8.8x Type:%d\n",nSM, ec_slave[cnt].SM[nSM].StartAddr, ec_slave[cnt].SM[nSM].SMlength,
(int)ec_slave[cnt].SM[nSM].SMflags, ec_slave[cnt].SMtype[nSM]);
}
for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++)
{
printf(" FMMU%1d Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j,
(int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit,
ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit,
ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive);
}
printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",
ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func);
printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_rl, ec_slave[cnt].mbx_proto);
ssigen = ec_siifind(cnt, ECT_SII_GENERAL);
/* SII general section */
if (ssigen)
{
ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07);
ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08);
ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09);
ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a);
if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0)
{
ec_slave[cnt].blockLRW = 1;
ec_slave[0].blockLRW++;
}
ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e);
ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8;
ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent;
}
printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n",
ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails);
printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n",
ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW);
if ((ec_slave[cnt].mbx_proto & 0x04) && printSDO)
{
ODlist.Entries = 0;
memset(&ODlist, 0, sizeof(ODlist));
if( ec_readODlist(cnt, &ODlist))
{
printf(" CoE Object Description found, %d entries.\n",ODlist.Entries);
for( i = 0 ; i < ODlist.Entries ; i++)
{
ec_readODdescription(i, &ODlist);
while(EcatError)
{
printf("%s", ec_elist2string());
}
printf(" Index: %4.4x Datatype: %4.4x Objectcode: %2.2x Name: %s\n",
ODlist.Index[i], ODlist.DataType[i], ODlist.ObjectCode[i], ODlist.Name[i]);
memset(&OElist, 0, sizeof(OElist));
ec_readOE(i, &ODlist, &OElist);
while(EcatError)
{
printf("%s", ec_elist2string());
}
for( j = 0 ; j < ODlist.MaxSub[i]+1 ; j++)
{
if ((OElist.DataType[j] > 0) && (OElist.BitLength[j] > 0))
{
printf(" Sub: %2.2x Datatype: %4.4x Bitlength: %4.4x Obj.access: %4.4x Name: %s\n",
j, OElist.DataType[j], OElist.BitLength[j], OElist.ObjAccess[j], OElist.Name[j]);
if ((OElist.ObjAccess[j] & 0x0007))
{
dtype = OElist.DataType[j];
printf(" Value :%s\n", SDO2string(cnt, ODlist.Index[i], j, OElist.DataType[j]));
}
}
}
}
}
else
{
while(EcatError)
{
printf("%s", ec_elist2string());
}
}
}
}
}
else
{
printf("No slaves found!\n");
}
printf("End slaveinfo, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
///establishes the ethercat connection
void EthercatMaster::initializeEthercat() {
// Bouml preserved body begin 000410F1
/* initialise SOEM, bind socket to ifname */
if (ec_init(ethernetDevice.c_str())) {
LOG(info) << "Initializing EtherCAT on " << ethernetDevice;
/* find and auto-config slaves */
if (ec_config(TRUE, &IOmap_) > 0) {
LOG(trace) << ec_slavecount << " slaves found and configured.";
/* wait for all slaves to reach Pre OP state */
/*ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE);
if (ec_slave[0].state != EC_STATE_PRE_OP ){
printf("Not all slaves reached pre operational state.\n");
ec_readstate();
//If not all slaves operational find out which one
for(int i = 1; i<=ec_slavecount ; i++){
if(ec_slave[i].state != EC_STATE_PRE_OP){
printf("Slave %d State=%2x StatusCode=%4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
*/
/* distributed clock is not working
//Configure distributed clock
if(!ec_configdc()){
LOG(warning) << "no distributed clock is available";
}else{
uint32 CyclTime = 4000000;
uint32 CyclShift = 0;
for (int i = 1; i <= ec_slavecount; i++) {
ec_dcsync0(i, true, CyclTime, CyclShift);
}
}
*/
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
if (ec_slave[0].state != EC_STATE_SAFE_OP) {
LOG(warning) << "Not all slaves reached safe operational state.";
ec_readstate();
//If not all slaves operational find out which one
for (int i = 1; i <= ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_SAFE_OP) {
LOG(info) << "Slave " << i << " State=" << ec_slave[i].state << " StatusCode=" << ec_slave[i].ALstatuscode << " : " << ec_ALstatuscode2string(ec_slave[i].ALstatuscode);
}
}
}
//Read the state of all slaves
//ec_readstate();
LOG(trace) << "Request operational state for all slaves";
ec_slave[0].state = EC_STATE_OPERATIONAL;
// request OP state for all slaves
/* send one valid process data to make outputs in slaves happy*/
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
/* request OP state for all slaves */
ec_writestate(0);
// wait for all slaves to reach OP state
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
LOG(trace) << "Operational state reached for all slaves.";
} else {
throw std::runtime_error("Not all slaves reached operational state.");
}
} else {
throw std::runtime_error("No slaves found!");
}
} else {
throw std::runtime_error("No socket connection on " + ethernetDevice + "\nExcecute as root");
}
std::string baseJointControllerName = "TMCM-174";
std::string manipulatorJointControllerName = "TMCM-174";
std::string actualSlaveName;
nrOfSlaves = 0;
YouBotSlaveMsg emptySlaveMsg;
quantity<si::current> maxContinuousCurrentBase = 3.54 * ampere;
quantity<si::time> thermalTimeConstantWindingBase = 16.6 * second;
quantity<si::time> thermalTimeConstantMotorBase = 212 * second;
quantity<si::current> maxContinuousCurrentJoint13 = 2.36 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint13 = 16.6 * second;
quantity<si::time> thermalTimeConstantMotorJoint13 = 212 * second;
quantity<si::current> maxContinuousCurrentJoint4 = 1.07 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint4 = 13.2 * second;
quantity<si::time> thermalTimeConstantMotorJoint4 = 186 * second;
quantity<si::current> maxContinuousCurrentJoint5 = 0.49 * ampere;
quantity<si::time> thermalTimeConstantWindingJoint5 = 8.1 * second;
quantity<si::time> thermalTimeConstantMotorJoint5 = 108 * second;
int manipulatorNo = 0;
configfile->readInto(baseJointControllerName, "BaseJointControllerName");
configfile->readInto(manipulatorJointControllerName, "ManipulatorJointControllerName");
//reserve memory for all slave with a input/output buffer
for (int cnt = 1; cnt <= ec_slavecount; cnt++) {
// printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
// cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
// ec_slave[cnt].state, (int) ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
ethercatSlaveInfo.push_back(ec_slave[cnt]);
actualSlaveName = ec_slave[cnt].name;
if ((actualSlaveName == baseJointControllerName || actualSlaveName == manipulatorJointControllerName) && ec_slave[cnt].Obits > 0 && ec_slave[cnt].Ibits > 0) {
nrOfSlaves++;
// joints.push_back(YouBotJoint(nrOfSlaves));
firstBufferVector.push_back(emptySlaveMsg);
secondBufferVector.push_back(emptySlaveMsg);
ethercatOutputBufferVector.push_back((SlaveMessageOutput*) (ec_slave[cnt].outputs));
ethercatInputBufferVector.push_back((SlaveMessageInput*) (ec_slave[cnt].inputs));
YouBotSlaveMailboxMsg emptyMailboxSlaveMsg(cnt);
firstMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
secondMailboxBufferVector.push_back(emptyMailboxSlaveMsg);
newOutputDataFlagOne.push_back(false);
newOutputDataFlagTwo.push_back(false);
newMailboxDataFlagOne.push_back(false);
newMailboxDataFlagTwo.push_back(false);
newMailboxInputDataFlagOne.push_back(false);
newMailboxInputDataFlagTwo.push_back(false);
pendingMailboxMsgsReply.push_back(false);
int i = 0;
bool b = false;
upperLimit.push_back(i);
lowerLimit.push_back(i);
limitActive.push_back(b);
jointLimitReached.push_back(b);
inverseMovementDirection.push_back(b);
if (actualSlaveName == baseJointControllerName) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentBase,
thermalTimeConstantWindingBase,
thermalTimeConstantMotorBase));
}
if (actualSlaveName == manipulatorJointControllerName) {
manipulatorNo++;
if (manipulatorNo >= 1 && manipulatorNo <= 3) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint13,
thermalTimeConstantWindingJoint13,
thermalTimeConstantMotorJoint13));
}
if (manipulatorNo == 4) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint4,
thermalTimeConstantWindingJoint4,
thermalTimeConstantMotorJoint4));
}
if (manipulatorNo == 5) {
motorProtections.push_back(MotorProtection(maxContinuousCurrentJoint5,
thermalTimeConstantWindingJoint5,
thermalTimeConstantMotorJoint5));
manipulatorNo = 0;
}
}
}
}
if (nrOfSlaves != motorProtections.size()) {
throw std::runtime_error("Insufficient motor protections loaded");
}
if (nrOfSlaves > 0) {
LOG(info) << nrOfSlaves << " EtherCAT slaves found" ;
} else {
throw std::runtime_error("No EtherCAT slave could be found");
return;
}
stopThread = false;
threads.create_thread(boost::bind(&EthercatMaster::updateSensorActorValues, this));
SLEEP_MILLISEC(10); //needed to start up thread and EtherCAT communication
return;
// Bouml preserved body end 000410F1
}
