static void youbot_cleanup(ubx_block_t *b)
{
ec_slave[0].state = EC_STATE_PRE_OP;
ec_writestate(0);
ec_close();
free((struct youbot_info*) b->private_data);
}
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;
}
static int youbot_start(ubx_block_t *b)
{
DBG(" ");
int ret=-1, i;
struct youbot_info *inf=b->private_data;
/* reset old commands */
if(inf->base.detected && base_prepare_start(&inf->base) != 0) goto out;
if(inf->arm1.detected && arm_prepare_start(&inf->arm1) != 0) goto out;
if(inf->arm2.detected && arm_prepare_start(&inf->arm2) != 0) goto out;
/* requesting operational for all slaves */
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_writestate(0);
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
if(ec_slave[0].state != EC_STATE_OPERATIONAL) {
ERR("not all slaves reached state operational:");
for (i=0; i<=ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_OPERATIONAL) {
ERR("\tslave %d (%s) is in state=0x%x, ALstatuscode=0x%x",
i, ec_slave[i].name, ec_slave[i].state, ec_slave[i].ALstatuscode);
}
}
goto out;
}
/* get things rolling */
if (ec_send_processdata() <= 0) {
inf->pd_send_err++;
ERR("sending initial process data failed");
}
/* cache port pointers */
assert(inf->base.p_control_mode = ubx_port_get(b, "base_control_mode"));
assert(inf->base.p_cmd_twist = ubx_port_get(b, "base_cmd_twist"));
assert(inf->base.p_cmd_vel = ubx_port_get(b, "base_cmd_vel"));
assert(inf->base.p_cmd_cur = ubx_port_get(b, "base_cmd_cur"));
assert(inf->base.p_msr_odom = ubx_port_get(b, "base_msr_odom"));
assert(inf->base.p_msr_twist = ubx_port_get(b, "base_msr_twist"));
assert(inf->base.p_base_motorinfo = ubx_port_get(b, "base_motorinfo"));
assert(inf->arm1.p_control_mode = ubx_port_get(b, "arm1_control_mode"));
assert(inf->arm1.p_calibrate_cmd = ubx_port_get(b, "arm1_calibrate_cmd"));
assert(inf->arm1.p_cmd_pos = ubx_port_get(b, "arm1_cmd_pos"));
assert(inf->arm1.p_cmd_vel = ubx_port_get(b, "arm1_cmd_vel"));
assert(inf->arm1.p_cmd_cur = ubx_port_get(b, "arm1_cmd_cur"));
assert(inf->arm1.p_cmd_eff = ubx_port_get(b, "arm1_cmd_eff"));
assert(inf->arm1.p_arm_state = ubx_port_get(b, "arm1_state"));
assert(inf->arm1.p_arm_motorinfo = ubx_port_get(b, "arm1_motorinfo"));
assert(inf->arm1.p_gripper = ubx_port_get(b, "arm1_gripper"));
ret=0;
out:
return ret;
}
//switch the state of state machine
bool EcMaster::switchState (ec_state state)
{
bool reachState=false;
/* Request the desired state for all slaves */
ec_slave[0].state = state;
ec_writestate (0);
/* wait for all slaves to reach the desired state */
ec_statecheck (0, state, EC_TIMEOUTSTATE * 4);
//check if all slave reached the desired state
if (ec_slave[0].state == state)
{
reachState = true;
}
return reachState;
}
///closes the ethercat connection
bool EthercatMaster::closeEthercat() {
// Bouml preserved body begin 00041271
// Request safe operational state for all slaves
ec_slave[0].state = EC_STATE_SAFE_OP;
/* request SAFE_OP state for all slaves */
ec_writestate(0);
//stop SOEM, close socket
ec_close();
return true;
// Bouml preserved body end 00041271
}
void Drive::close()
{
if (isOpened) {
setPower(false);
timer.stop();
// request init state for all slaves
ec_slave[0].state = EC_STATE_INIT;
ec_writestate(0);
if (timerEventId) {
timeKillEvent(timerEventId);
timerEventId = 0;
}
ec_close();
isOpened = false;
}
}
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 ecatcheck( void *ptr )
{
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;
ec_writestate(slave);
}
else if(ec_slave[slave].state > 0)
{
if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
{
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d reconfigured\n",slave);
}
}
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;
printf("ERROR : slave %d lost\n",slave);
}
}
}
if (ec_slave[slave].islost)
{
if(!ec_slave[slave].state)
{
if (ec_recover_slave(slave, EC_TIMEOUTMON))
{
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d recovered\n",slave);
}
}
else
{
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d found\n",slave);
}
}
}
if(!ec_group[currentgroup].docheckstate)
printf("OK : all slaves resumed OPERATIONAL.\n");
}
usleep(10000);
}
}
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);
}
}
//TODO: ADD B_I AND I_B TRANSITIONS
int GetTransition(void)
{
int EcatTransition=0;
if(ec_slave[0].state!=ec_slaveMore[0].reqState)
{
if (ec_slave[0].state==EC_STATE_NONE || ec_slave[0].state==EC_STATE_INIT)
{
switch (ec_slaveMore[0].reqState)
{
case EC_STATE_INIT:
ec_slave[0].state=EC_STATE_INIT;
ec_writestate(0);
EcatTransition =0; //it's not necessary beacuse in this case CurrentState=EC_STATE_INIT=RequestState
break;
case EC_STATE_PRE_OP:
EcatTransition = ECAT_INITCMD_I_P;
break;
case EC_STATE_SAFE_OP:
EcatTransition = ECAT_INITCMD_I_P|ECAT_INITCMD_P_S; //to go from init to safe-op we have to execute both IP and PS transitions
break;
case EC_STATE_OPERATIONAL:
EcatTransition = ECAT_INITCMD_I_P|ECAT_INITCMD_P_S|ECAT_INITCMD_S_O;
break;
}
}
if (ec_slave[0].state==EC_STATE_PRE_OP)
{
switch (ec_slaveMore[0].reqState)
{
case EC_STATE_INIT:
EcatTransition =ECAT_INITCMD_P_I;
break;
case EC_STATE_PRE_OP:
EcatTransition = 0;
break;
case EC_STATE_SAFE_OP:
EcatTransition = ECAT_INITCMD_P_S;
break;
case EC_STATE_OPERATIONAL:
EcatTransition = ECAT_INITCMD_P_S|ECAT_INITCMD_S_O;
break;
}
}
if (ec_slave[0].state==EC_STATE_SAFE_OP)
{
switch (ec_slaveMore[0].reqState)
{
case EC_STATE_INIT:
EcatTransition =ECAT_INITCMD_S_I;
break;
case EC_STATE_PRE_OP:
EcatTransition = ECAT_INITCMD_S_P;
break;
case EC_STATE_SAFE_OP:
EcatTransition = 0;
break;
case EC_STATE_OPERATIONAL:
EcatTransition = ECAT_INITCMD_S_O;
break;
}
}
if (ec_slave[0].state==EC_STATE_OPERATIONAL)
{
switch (ec_slaveMore[0].reqState)
{
case EC_STATE_INIT:
EcatTransition =ECAT_INITCMD_O_I;
break;
case EC_STATE_PRE_OP:
EcatTransition = ECAT_INITCMD_O_P;
break;
case EC_STATE_SAFE_OP:
EcatTransition = ECAT_INITCMD_O_S;
break;
case EC_STATE_OPERATIONAL:
EcatTransition = 0;
break;
}
}
}
return EcatTransition;
}
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 boottest(char *ifname, uint16 slave, char *filename)
{
printf("Starting firmware update example\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);
printf("Request init state for slave %d\n", slave);
ec_slave[slave].state = EC_STATE_INIT;
ec_writestate(slave);
/* wait for slave to reach INIT state */
ec_statecheck(slave, EC_STATE_INIT, EC_TIMEOUTSTATE * 4);
printf("Slave %d state to INIT.\n", slave);
/* read BOOT mailbox data, master -> slave */
data = ec_readeeprom(slave, ECT_SII_BOOTRXMBX, EC_TIMEOUTEEP);
ec_slave[slave].SM[0].StartAddr = (uint16)LO_WORD(data);
ec_slave[slave].SM[0].SMlength = (uint16)HI_WORD(data);
/* store boot write mailbox address */
ec_slave[slave].mbx_wo = (uint16)LO_WORD(data);
/* store boot write mailbox size */
ec_slave[slave].mbx_l = (uint16)HI_WORD(data);
/* read BOOT mailbox data, slave -> master */
data = ec_readeeprom(slave, ECT_SII_BOOTTXMBX, EC_TIMEOUTEEP);
ec_slave[slave].SM[1].StartAddr = (uint16)LO_WORD(data);
ec_slave[slave].SM[1].SMlength = (uint16)HI_WORD(data);
/* store boot read mailbox address */
ec_slave[slave].mbx_ro = (uint16)LO_WORD(data);
/* store boot read mailbox size */
ec_slave[slave].mbx_rl = (uint16)HI_WORD(data);
printf(" SM0 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[0].StartAddr, ec_slave[slave].SM[0].SMlength,
(int)ec_slave[slave].SM[0].SMflags);
printf(" SM1 A:%4.4x L:%4d F:%8.8x\n", ec_slave[slave].SM[1].StartAddr, ec_slave[slave].SM[1].SMlength,
(int)ec_slave[slave].SM[1].SMflags);
/* program SM0 mailbox in for slave */
ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM0, sizeof(ec_smt), &ec_slave[slave].SM[0], EC_TIMEOUTRET);
/* program SM1 mailbox out for slave */
ec_FPWR (ec_slave[slave].configadr, ECT_REG_SM1, sizeof(ec_smt), &ec_slave[slave].SM[1], EC_TIMEOUTRET);
printf("Request BOOT state for slave %d\n", slave);
ec_slave[slave].state = EC_STATE_BOOT;
ec_writestate(slave);
/* wait for slave to reach BOOT state */
if (ec_statecheck(slave, EC_STATE_BOOT, EC_TIMEOUTSTATE * 10) == EC_STATE_BOOT)
{
printf("Slave %d state to BOOT.\n", slave);
if (input_bin(filename, &filesize))
{
printf("File read OK, %d bytes.\n",filesize);
printf("FoE write....");
j = ec_FOEwrite(slave, filename, 0, filesize , &filebuffer, EC_TIMEOUTSTATE);
printf("result %d.\n",j);
printf("Request init state for slave %d\n", slave);
ec_slave[slave].state = EC_STATE_INIT;
ec_writestate(slave);
}
else
printf("File not read OK.\n");
}
}
else
{
printf("No slaves found!\n");
}
printf("End firmware update example, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n",ifname);
}
}
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);
}
}
///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
}
ConnManager::~ConnManager() {
ec_slave[0].state = EC_STATE_INIT;
ec_writestate(0);
ec_close();
}
bool Drive::open(const QString& adapterName)
{
qDebug() << "drive: open " << adapterName;
bool ok = true;
if (isOpened) {
assert(false && "repeated call is not supported");
ok = false;
}
if (ok) {
strcpy_s(adapterNameBuffer, adapterName.toStdString().c_str()); // TODO уязвимость
// initialise SOEM, bind socket to adapterNameBuffer
if (ec_init(adapterNameBuffer) <= 0) {
qDebug() << "ошибка ec_init";
ok = false;
}
}
// find and auto-config slaves
if (ok) {
isOpened = true; // TODO правильное ли место?
const auto slaveCount = ec_config_init(0);
if (slaveCount != 1) {
qDebug() << "количество найденных устройств равно " << slaveCount;
if (slaveCount == EC_OTHERFRAME)
qDebug() << "Ошибка: unknown frame received";
else if (slaveCount == EC_NOFRAME)
qDebug() << "Ошибка: no frame returned";
ok = false;
}
}
if (ok) {
// проверка, что подключен именно наш мотор
auto slave = &ec_slave[1];
if (slave->eep_man != 599 || slave->eep_id != 41220) {
qDebug() << "eep_man: " << slave->eep_man << ", slave->eep_id: " << slave->eep_id;
ok = false;
}
ec_slave[1].PO2SOconfig = &myPdoSetup;
}
if (ok) {
// Map all PDOs from slaves to iomap
ec_config_map(&iomap);
// wait for all slaves to reach SAFE_OP state
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
ec_slave[0].state = EC_STATE_OPERATIONAL; // TODO правильно ли, что это здесь?
// send one valid process data to make outputs in slaves happy
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
// start I/O transfer thread as periodic multimedia timer
globalErrorFlag.store(GLOBAL_OK);
timerEventId = timeSetEvent(1, 0, ioTransferThread, 0, TIME_PERIODIC); // TODO рассмотреть переход на CreateTimerQueueTimer
if (timerEventId == NULL) {
qDebug() << "Не удалось создать поток ввода/вывода";
ok = false;
}
}
if (ok) {
// request OP state for all slaves
ec_writestate(0);
// wait for all slaves to reach OP state
int chk = 40;
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) {// не удадось перевести метор в состояние OPERATIONAL
qDebug() << "не удадось перевести мотор в состояние OPERATIONAL";
ok = false;
}
while(EcatError)
qDebug("%s", ec_elist2string());
}
if (ok) {
int result = ec_SDOwrite(1, 0x4003, 0x01, FALSE, sizeof(deviceMode), &deviceMode, EC_TIMEOUTRXM); // Device Mode
if (result != 1)
qDebug() << "ошибка ec_SDOwrite: " << result;
while(EcatError)
qDebug("%s", ec_elist2string());
ok = (result == 1);
}
if (ok) {
timer.start(updatePeriod);
setPower(true);
}
else
close();
return ok;
}
