EtherCAT Application - Elmo & Beckhoff DIO

From Neuromeka Wiki
Jump to: navigation, search


Overview

This section is a reference for experienced users who got used to working with EtherCAT and NRMK EtherCAT Configuration Tool. It shows a little explanation about generated System Interface and control code in C++ in order for user to modify the code according to particular cases. For consistency, the explained code in this section is from the Eclipse project generated in NRMKEtherCAT Configuration Tool - Tutorials.

Fundamentally, except for C++ header files for socket implementation, those files below are the most important ones:

  • Real-time Control Code
    • main.cpp
  • EtherCAT System Interface
    • SystemInterface_EtherCAT_ElmoDIO.h
  • PDO Configuration Files
    • Beckhoff_EL1008_PDO.h + Beckhoff_EL1008_PDO.c
    • Beckhoff_EL2008_PDO.h + Beckhoff_EL2008_PDO.c
    • Elmo_GCON_PDO.h + Elmo_GCON_PDO.c

Real-time Control Code

This code section is designed for users who want to focus only on high-level application related to control algorithm or real-time operation but not the EtherCAT protocol at lower layer. From this approach, we defined the term "Axis" which refers to a motor, a joint or a DOF of robot. In this example, each axis represents each EtherCAT slave.

At first, the main() function are called to create and initialize all important components of the control application.

....................................................................................

RT_TASK Elmo_GCON_task;
RT_TASK print_task;
RT_TASK plot_task;
RT_TASK control_task;

....................................................................................

int main(int argc, char **argv)
{
	// Perform auto-init of rt_print buffers if the task doesn't do so
    rt_print_auto_init(1);

	signal(SIGINT, signal_handler);
	signal(SIGTERM, signal_handler);

	/* Avoids memory swapping for this program */
	mlockall(MCL_CURRENT|MCL_FUTURE);

	// TO DO: Specify the cycle period (cycle_ns) here, or use default value (1ms)
	cycle_ns=1000000; //1000us: 1kHz	
	period=((double) cycle_ns)/((double) NSEC_PER_SEC);	//period in second unit

	// For trajectory interpolation
	initAxes();
	
	// Set the operation mode for EtherCAT servo drives
	
	// For CST (cyclic synchronous torque) control
	//_systemInterface_EtherCAT_Elmo_GCON.init(OP_MODE_CYCLIC_SYNC_TORQUE, cycle_ns);
	
	// For CSP (cyclic synchronous position) control
	_systemInterface_EtherCAT_Elmo_GCON.init(OP_MODE_CYCLIC_SYNC_POSITION, cycle_ns);

	// TO DO: Create data socket server
	datasocket.setPeriod(period);

	if (datasocket.startServer(SOCK_TCP, NRMK_PORT_DATA))
		printf("Data server started at IP of : %s on Port: %d\n", datasocket.getAddress(), NRMK_PORT_DATA);

	printf("Waiting for Data Scope to connect...\n");
	datasocket.waitForConnection(0);
	
	// TO DO: Create control socket server
	if (controlsocket.startServer(SOCK_TCP, 6868))
		printf("Control server started at IP of : %s on Port: %d\n", controlsocket.getAddress(), 6868);

	printf("Waiting for Control Tool to connect...\n");
	controlsocket.waitForConnection(0);

	// Elmo_GCON_task: create and start
	printf("Now running rt task ...\n");

	rt_task_create(&Elmo_GCON_task, "Elmo_GCON_task", 0, 99, 0);
	rt_task_start(&Elmo_GCON_task, &Elmo_GCON_run, NULL);

	// printing: create and start
	rt_task_create(&print_task, "printing", 0, 80, 0);
	rt_task_start(&print_task, &print_run, NULL);
	
	// plotting: data socket comm
	rt_task_create(&plot_task, "plotting", 0, 80, 0);
	rt_task_start(&plot_task, &plot_run, NULL);

	// controlling: control socket
	rt_task_create(&control_task, "controlling", 0, 85, 0);
	rt_task_start(&control_task, &control_run, NULL);
	
	// Must pause here
	pause();

	// Finalize
	signal_handler();

    return 0;
}

As shown in the above code snippet, there are four tasks running simultaneously named Elmo_GCON_task, print_task, plot_task and control_task.

  • Elmo_GCON_task is the most important one because it takes responsibility for real-time communication with EtherCAT system as well as control algorithm.
  • print_task prints user-defined messages to console.
  • plot_task uses datasocket to transfer control data to DataScope - the plotting tool from Neuromeka. (* datascope is a server socket and is initialized in main() as well).
  • control_task handles communication with NRMKEcatTool (System Monitor). It receives command from System Monitor and sends all system status (EtherCAT status, Actual Position, Actual Value, etc.)

In addition, the system interface for EtherCAT is also initialized with operation mode (default as Cyclic Sync Position) and a period of working cycle (as same as real-time control interval).

ElmoDIO_task

As mentioned above, this must satisfy all requirements for real-time operation because it handles all EtherCAT communication and robot control algorithm. The implementation of this task is put in ElmoDIO_run() function. The following code snippet shows the content of Elmo_GCON_run() function in which a compute() function is called between the calls to readBuffer() and writeBuffer() functions of system interface. For more details, the readBuffer() functions return feedback values including actual position (from encoder), velocity, torque or value of digital input from EtherCAT slaves. Then, those feedback data should be used for control algorithm (implemented in compute() function). Finally, computed control data such as target position, target torque or digital output will be sent to real EtherCAT slaves via writeBuffer() functions.

  • Note that all data in this function are in encoder's unit (count or pulse).
// Elmo_GCON_task	
void Elmo_GCON_run(void *arg)
{
	unsigned int runcount=0;
	RTIME now, previous;
	
	// Synchronize EtherCAT Master (for Distributed Clock Mode)
	_systemInterface_EtherCAT_Elmo_GCON.syncEcatMaster();
	
	/* Arguments: &task (NULL=self),
	 *            start time,
	 *            period
	 */
	rt_task_set_periodic(NULL, TM_NOW, cycle_ns);

	while (run)
	{
		rt_task_wait_period(NULL); 	//wait for next cycle

		runcount++;

		if (!run)
		{
			break;
		}

		previous = rt_timer_read();

		/// TO DO: read data from sensors in EtherCAT system interface
		_systemInterface_EtherCAT_Elmo_GCON.processTxDomain();
		_systemInterface_EtherCAT_Elmo_GCON.readBuffer(COE_POSITION, ActualPos);
		_systemInterface_EtherCAT_Elmo_GCON.readBuffer(COE_VELOCITY, ActualVel);
		_systemInterface_EtherCAT_Elmo_GCON.readBuffer(COE_TORQUE, ActualTor);
		_systemInterface_EtherCAT_Elmo_GCON.readBuffer(DATA_IN, DataIn);
		
		/// TO DO: Main computation routine...
		compute();
		
		/// TO DO: write data to actuators in EtherCAT system interface
		_systemInterface_EtherCAT_Elmo_GCON.writeBuffer(COE_POSITION, TargetPos);
		_systemInterface_EtherCAT_Elmo_GCON.writeBuffer(COE_VELOCITY, TargetVel);
		_systemInterface_EtherCAT_Elmo_GCON.writeBuffer(COE_TORQUE, TargetTor);
		_systemInterface_EtherCAT_Elmo_GCON.writeBuffer(DATA_OUT, DataOut);
		_systemInterface_EtherCAT_Elmo_GCON.processRxDomain();
			
		// For EtherCAT performance statistics
		now = rt_timer_read();
		ethercat_time = (long) now - previous;

		if (_systemInterface_EtherCAT_Elmo_GCON.isSystemReady() && (runcount>WAKEUP_TIME*(NSEC_PER_SEC/cycle_ns)) )
		{
			system_ready=1;	//all drives have been done

			gt+= period;
			
			if (worst_time<ethercat_time) worst_time=ethercat_time;
			if(ethercat_time > max_time)
				++fault_count;
		}
	}
}

Control Algorithm Implementation

This section explains a few lines of code in compute() function where users can implement their own control algorithm. In the following code snippet, system_ready is a flag to inform us whether all EtherCAT slaves are turned on and ready to execute control command. In this example, after EtherCAT system is ready (system_ready = true), we

  • update axes' states with encoder's values (actual pos, actual vel, actual torque),
  • call a simple interpolated trajectory at current time gt
  • set target values at computed values.
int compute()
{	
	// For Command Control	
	for (int i=0; i<NUM_AXIS; ++i)
	{
		if ((!system_ready) && (InitFlag[i] == 0))
		{
			if (_systemInterface_EtherCAT_Elmo_GCON.getModeOperation(i) == OP_MODE_CYCLIC_SYNC_POSITION)
			{
				if (ActualPos[i] != 0)
				{
					TargetPos[i] = ActualPos[i];
					Axis[i].setTarPosInCnt(ActualPos[i]);
					InitFlag[i] = 1;
				}
			}
			else
			{
				TargetVel[i] = 0;
				TargetTor[i] = 0;
				InitFlag[i] = 1;
			}
		}
		// trajectory
		else if (system_ready)
		{
			Axis[i].setCurrentPosInCnt(ActualPos[i]);
			Axis[i].setCurrentVelInCnt(ActualVel[i]);
			Axis[i].setCurrentTorInCnt(ActualTor[i]);
			Axis[i].setDataIn(DataIn[i]);

			Axis[i].setCurrentTime(gt);

			TargetPos[i] = Axis[i].getDesPosInCnt();
			TargetVel[i] = Axis[i].getDesVelInCnt();
			TargetTor[i] = Axis[i].getDesTorInCnt();
			DataOut[i] = Axis[i].getDataOut();
		}
	}
	
	return 0;
}

PDO Configuration Files

Those configuration files contain configured (or mapped) PDO in each category of EtherCAT slaves.

For example, the content of Elmo GOLD Whistle configuration files is shown below:

#pragma once 

// Ethercat Master---- 
#include "ecrt.h" 

// Vendor ID & Product Code 
#define Elmo 0x0000009A 
#define Elmo_GCON 0x00030924 

ec_pdo_entry_info_t Elmo_GCON_pdo_entries[] = 
{ 
	{0x607A,	0,	32},	/* TargetPosition */
	{0x60FF,	0,	32},	/* TargetVelocity */
	{0x6071,	0,	16},	/* TargetTorque */
	{0x6072,	0,	16},	/* MaxTorque */
	{0x6040,	0,	16},	/* ControlWord */
	{0x6060,	0,	8},	/* ModeOfOperation */
	{0,	0,	8},	/* null */
	{0x6064,	0,	32},	/* PositionActualValue */
	{0x60F4,	0,	32},	/* PositionFollowingErrrorActualValue */
	{0x6077,	0,	16},	/* TorqueActualValue */
	{0x6041,	0,	16},	/* StatusWord */
	{0x6061,	0,	8},	/* ModeOfOperationDisplay */
	{0,	0,	8},	/* null */
	{0x606C,	0,	32},	/* VelocityActualValue */
};

ec_pdo_info_t Elmo_GCON_pdos[] = 
{ 
	{0x1605, 7, Elmo_GCON_pdo_entries + 0},	/* RxPDO RxPDO_Elmo_GCON_0x1605 */
	{0x1A04, 6, Elmo_GCON_pdo_entries + 7},	/* TxPDO TxPDO_Elmo_GCON_0x1A04 */
	{0x1A11, 1, Elmo_GCON_pdo_entries + 13},/* TxPDO TxPDO_Elmo_GCON_0x1A11 */
};

ec_sync_info_t Elmo_GCON_syncs[] = 
{ 
	{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE}, 
	{1, EC_DIR_INPUT, 0, NULL, EC_WD_ENABLE}, 
	{2, EC_DIR_OUTPUT, 1, Elmo_GCON_pdos + 0, EC_WD_ENABLE}, 
	{3, EC_DIR_INPUT, 2, Elmo_GCON_pdos + 1, EC_WD_ENABLE}, 
	{0xff} 
 };
  • In the above code, Elmo_GCON_syncs means SyncManager of Elmo CoE slaves, Elmo_GCON_pdos means the configured PDO in Elmo and Elmo_GCON_pdo_entries means all available entries to be mapped in EtherCAT domains. In detail, there are three usable PDOs (0x1605, 0x1A04 and 0x1A11) distributed into the last two SyncManager.