bool ConnManager::initialize() {
// Send the EtherCAT slaves into OP
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_writestate(0);
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
// We are now in OP.
// Configure the distributed clocks for each slave.
for (int i = 1; i <= ec_slavecount; i++) {
ec_dcsync0(i, true, CONTROLLER_LOOP_PERIOD_NS,
CONTROLLER_LOOP_PERIOD_NS - CONTROLLER_LOOP_OFFSET_NS);
}
// Update ec_DCtime so we can calculate stop time below.
cycleECat();
// Send a barrage of packets to set up the DC clock.
int64_t stoptime = ec_DCtime + 200000000;
// SOEM automatically updates ec_DCtime.
while (ec_DCtime < stoptime) {
cycleECat();
}
// We now have data.
// Configure our medullas
eCatConn->getMedullaManager()->start(ec_slave, ec_slavecount);
targetTime = RTT::os::TimeService::Instance()->getNSecs();
done = false;
midCycle = false;
return !done;
}
void eboxtest(char *ifname)
{
int cnt, i;
printf("Starting E/BOX 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);
// check if first slave is an E/BOX
if (( ec_slavecount >= 1 ) &&
(strcmp(ec_slave[1].name,"E/BOX") == 0))
{
// reprogram PDO mapping to set slave in stream mode
// this can only be done in pre-OP state
os=sizeof(ob2); ob2 = 0x1601;
ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
os=sizeof(ob2); ob2 = 0x1a01;
ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
}
ec_config_map(&IOmap);
ec_configdc();
/* 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 */
printf("DC capable : %d\n",ec_configdc());
/* check configuration */
if (( ec_slavecount >= 1 ) &&
(strcmp(ec_slave[1].name,"E/BOX") == 0)
)
{
printf("E/BOX found.\n");
/* connect struct pointers to slave I/O pointers */
in_EBOX = (in_EBOX_streamt*) ec_slave[1].inputs;
out_EBOX = (out_EBOX_streamt*) ec_slave[1].outputs;
/* 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("Request operational state for all slaves\n");
/* send one processdata cycle to init SM in slaves */
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* 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 )
{
printf("Operational state reached for all slaves.\n");
ain[0] = 0;
ain[1] = 0;
ainc = 0;
dorun = 1;
usleep(100000); // wait for linux to sync on DC
ec_dcsync0(1, TRUE, SYNC0TIME, 0); // SYNC0 on slave 1
/* acyclic loop 20ms */
for(i = 1; i <= 200; i++)
{
/* read DC difference register for slave 2 */
// ec_FPRD(ec_slave[1].configadr, ECT_REG_DCSYSDIFF, sizeof(DCdiff), &DCdiff, EC_TIMEOUTRET);
// if(DCdiff<0) { DCdiff = - (int32)((uint32)DCdiff & 0x7ffffff); }
printf("PD cycle %5d DCtime %12lld Cnt:%3d Data: %6d %6d %6d %6d %6d %6d %6d %6d \n",
cyclecount, ec_DCtime, in_EBOX->counter, in_EBOX->stream[0], in_EBOX->stream[1],
in_EBOX->stream[2], in_EBOX->stream[3], in_EBOX->stream[4], in_EBOX->stream[5],
in_EBOX->stream[98], in_EBOX->stream[99]);
usleep(20000);
}
dorun = 0;
// printf("\nCnt %d : Ain0 = %f Ain2 = %f\n", ainc, ain[0] / ainc, ain[1] / ainc);
}
else
{
printf("Not all slaves reached operational state.\n");
}
}
else
{
printf("E/BOX not found in slave configuration.\n");
}
ec_dcsync0(1, FALSE, 8000, 0); // SYNC0 off
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);
/* wait for all slaves to reach state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
ec_slave[0].state = EC_STATE_PRE_OP;
/* request SAFE_OP state for all slaves */
ec_writestate(0);
/* wait for all slaves to reach state */
ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE);
if (( ec_slavecount >= 1 ) &&
(strcmp(ec_slave[1].name,"E/BOX") == 0))
{
// restore PDO to standard mode
// this can only be done is pre-op state
os=sizeof(ob2); ob2 = 0x1600;
ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
os=sizeof(ob2); ob2 = 0x1a00;
ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
}
printf("Streampos %d\n", streampos);
output_cvs("stream.txt", streampos);
}
else
{
printf("No slaves found!\n");
}
printf("End E/BOX, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
