Example for ELMO drives

From Neuromeka Wiki
Jump to: navigation, search


ELMO provides EtherCAT drives. We will implement an application controlling motors by GOLD DC Whistle drives Gold DC Whistle using EtherCAT interfaces based on NRMK EtherCAT framework. This time we will use NRMKEcatTool for EtherCAT interface generation.

Note that Successful operation of EtherCAT drives need more effort aside from setup of EtherCAT interface, such as servo-on/off by proper state transition. This example will be an example to build your own EtherCAT interfaces using EtherCAT drives by CoE (CANopen-over-EtherCAT).

Generating Slave interface by NRMKEcatTool

  • By command ./ethercattool slaves one can identify the five ELMO drives on the bus with alias '0' and position from 0 to 5.
Slave configurations of ELMO drives

Slave configurations of ELMO drives

  • Generate C interface files, e.g. Elmo_GCON_PDO.c and Elmo_GCON_PDO.h, by command ./ethercattool cstruct -p0.
#include "Elmo_GCON_PDO.h" 

ec_pdo_entry_info_t Elmo_GCON_pdo_entries[] = 
{ 
	{0x607A, 0, 32},	/* Target Position */
	{0x60FF, 0, 32},	/* Target Velocity */
	{0x6071, 0, 16},	/* Target Torque */
	{0x6072, 0, 16},	/* Max. Torque */
	{0x6040, 0, 16},	/* Control Word */
	{0x6060, 0, 8},	/* Mode Of Operation */
	{0, 0, 8},	/* null */
	{0x6064, 0, 32},	/* Position Actual Value */
	{0x6077, 0, 16},	/* Torque Actual Value */
	{0x6041, 0, 16},	/* Status Word */
	{0x6061, 0, 8},	/* Mode Of Operation Display */
	{0, 0, 8},	/* null */
};

ec_pdo_info_t Elmo_GCON_pdos[] = 
{ 
	{0x1605, 7, Elmo_GCON_pdo_entries + 0},	/* RxPDO RxPDO_Elmo_GCON_0x1605 */
	{0x1A02, 5, Elmo_GCON_pdo_entries + 7},	/* TxPDO TxPDO_Elmo_GCON_0x1A02 */
};

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, 1, Elmo_GCON_pdos + 1, EC_WD_ENABLE}, 
	{0xff} 
 };
#pragma once 

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

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

extern ec_pdo_entry_info_t Elmo_GCON_pdo_entries[]; 
extern ec_pdo_info_t  Elmo_GCON_pdos[]; 
extern ec_sync_info_t   Elmo_GCON_syncs[]; 


EtherCAT Master for Elmo

  • Implement EtherCAT interface file, e.g. EtherCAT_ELMO_Test.h.
  1 #pragma once
  2 
  3 //Ethercat Master----
  4 #include "ecrt.h"
  5 #include <rtdk.h>		//The rdtk real-time printing library
  6 
  7 #include "CoE.h"
  8 #include "Elmo_GCON_PDO.h" 
  9 
 10 #include "NRMKEcatMasterDCBase.h"
 11 
 12 namespace NRMKHelper
 13 {
 14 	class EcatMaster_Elmo_GCON : public NRMKEcatMasterDCBase
 15 	{
 16 	public:
 17 		enum
 18 		{
 19 			NUM_ELMO_GCON_SLAVES = 4,
 20 
 21 			NUM_DCDOMAIN_ENTRIES = 40,			
 22 
 23 		};
 24 
 25 		EcatMaster_Elmo_GCON() : NRMKEcatMasterDCBase()
 26 		{
 27 		}
 28 
 29 		~EcatMaster_Elmo_GCON()
 30 		{
 31 		}
 32 
 33 		int startDCDomain()
 34 		{
 35 			ecrt_master_receive(_getMaster()); //RECEIVE A FRAME
 36 			_dcDomain->process();
 37 
 38 			return 0;
 39 		}
 40 		template<typename T>
 41 		int readDCDomain(T * Value, UINT32 SlaveAlias, UINT32 SlavePosition, UINT16 EntryIndex, UINT8 EntrySubIndex)
 42 		{
 43 			_dcDomain->readData(Value, SlaveAlias, SlavePosition, EntryIndex, EntrySubIndex);
 44 			return 0;
 45 		}
 46 		template<typename T>
 47 		int writeDCDomain(T Value, UINT32 SlaveAlias, UINT32 SlavePosition, UINT16 EntryIndex, UINT8 EntrySubIndex)
 48 		{
 49 			_dcDomain->writeData(Value, SlaveAlias, SlavePosition, EntryIndex, EntrySubIndex);
 50 			return 0;
 51 		}
 52 		int endDCDomain()
 53 		{
 54 			_dcDomain->queue();
 55 			
 56 			// sync distributed clock just before master_send to set
 57 		    // most accurate master clock time
 58 		    _syncDistClocks();
 59 
 60 			ecrt_master_send(_getMaster()); //SEND ALL QUEUED DATAGRAMS
 61 
 62 			_updateMasterClock();
 63 
 64 			return 0;
 65 		}
 66 		
 67 		
 68 	protected:
 69 		inline int _initSlaves()
 70 		{
 71 			_elmo_GCON[0].setSlaveInfo(Elmo, Elmo_GCON, 0, 0);
 72 			_elmo_GCON[1].setSlaveInfo(Elmo, Elmo_GCON, 0, 1);
 73 			_elmo_GCON[2].setSlaveInfo(Elmo, Elmo_GCON, 0, 2);
 74 			_elmo_GCON[3].setSlaveInfo(Elmo, Elmo_GCON, 0, 3);			
 75 
 76 			for (int i=0; i<NUM_ELMO_GCON_SLAVES; i++)
 77 			{
 78 				_elmo_GCON[i].setConfig(ecrt_master_slave_config(_getMaster(),
 79 												_elmo_GCON[i].getAlias(),
 80 												_elmo_GCON[i].getPosition(),
 81 												_elmo_GCON[i].getVendor(),
 82 												_elmo_GCON[i].getProductCode()));
 83 				_elmo_GCON[i].setSyncManagers(Elmo_GCON_syncs);
 84 				_elmo_GCON[i].init();
 85 			}
 86 		
 87 
 88 			return 0;
 89 		}
 90 		inline int _initDomains()
 91 		{
 92 			// Create Domain
 93 			_dcDomain = new NRMKEcatDomain<NUM_DCDOMAIN_ENTRIES>(ecrt_master_create_domain(_getMaster()));
 94 			if (!_dcDomain->isValid())
 95 			{
 96 				fprintf(stderr, "Unable to create process data working domain .\n");
 97 				deinit();
 98 				return -1;
 99 			}
100 			
101 
102 			// Register Entries
103 			for (int i=0; i<NUM_ELMO_GCON_SLAVES; i++)
104 			{
105 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x607A, 0);	// TargetPosition
106 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x60FF, 0);	// TargetVelocity
107 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6071, 0);	// TargetTorque
108 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6072, 0);	// MaxTorque
109 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6040, 0);	// ControlWord
110 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6060, 0);	// ModeOfOperation
111 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6064, 0);	// PositionActualValue
112 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6077, 0);	// TorqueActualValue
113 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6041, 0);	// StatusWord
114 				_dcDomain->registerPDOEntry(_elmo_GCON[i], 0x6061, 0);	// ModeOfOperationDisplay
115 				
116 			}
117 			
118 
119 			// Init Domain
120 			if (_dcDomain->init() == -1)
121 				return -1;
122 			
123 
124 			if (activate() == -1)
125 				return -1;
126 
127 			if (_dcDomain->getDomainPtr() == -1)
128 				return -1;
129 			
130 
131 			return 0;
132 		}
133 		inline int _configDCMode()
134 		{
135 			for (int i=0; i<NUM_ELMO_GCON_SLAVES; i++)
136 				_elmo_GCON[i].configDC(getCycleDC());
137 		
138 
139 			int ret = ecrt_master_select_reference_clock(_getMaster(), _elmo_GCON[0].getConfig());
140 			if (ret < 0)
141 			{
142 				fprintf(stderr, "Failed to select reference clock: %s\n", strerror(-ret));
143 				return ret;
144 			}
145 
146 			return 0;
147 		}
148 
149 	private:
150 		// Domains
151 		NRMKEcatDomain<NUM_DCDOMAIN_ENTRIES> *_dcDomain;
152 		
153 
154 		// Slaves
155 		NRMKEcatSlave _elmo_GCON[NUM_ELMO_GCON_SLAVES];
156 		
157 
158 	};
159 }


Implementing System Interface

  • Implement System interface file, e.g. SystemInterface_EtherCAT_ELMO_GCON.h.
  1 #pragma once 
  2 
  3 #include "CoE.h"
  4 #include "SystemInterface_EtherCAT.h"
  5 #include "EcatMaster_Elmo_GCON.h"
  6 
  7 namespace NRMKHelper 
  8 { 
  9 	typedef unsigned int UINT32;
 10 	typedef int32_t INT32;
 11 	typedef int16_t INT16;
 12 	typedef uint16_t UINT16;
 13 	typedef uint8_t UINT8;
 14 	typedef int8_t INT8;
 15 
 16 	class SystemInterface_EtherCAT_Elmo_GCON : public SystemInterfaceBase
 17 	{ 
 18 		private:
 19 			enum
 20 			{
 21 				NUM_ELMO_GCON_AXES = 4,
 22 				NUM_MOTION_AXIS = NUM_ELMO_GCON_AXES,
 23 				MAX_TORQUE = 500
 24 			};
 25 
 26 			typedef struct _Elmo_GCON_AxisIn
 27 			{
 28 				INT32		TargetPosition;  	// 0x607A
 29 				INT32		TargetVelocity;		// 0x60FF
 30 				INT16		TargetTorque;		// 0x6071
 31 				UINT16		MaxTorque;		// 0x6072
 32 				UINT16		ControlWord;		// 0x6040
 33 				UINT8		ModeOfOperation;	// 0x6060				
 34 			}Elmo_GCON_AxisIn;
 35 
 36 			typedef struct _Elmo_GCON_AxisOut
 37 			{
 38 				INT32		PositionActualValue; 		// 0x6064
 39 				INT16		TorqueActualValue; 		// 0x6077
 40 				UINT16		StatusWord; 	 		// 0x6041
 41 				INT8		ModeOfOperationDisplay; 	// 0x6061				
 42 			}Elmo_GCON_AxisOut;
 43 
 44 			typedef struct _Elmo_GCON_Axis
 45 			{
 46 				Elmo_GCON_AxisIn 	InParam;
 47 				Elmo_GCON_AxisOut 	OutParam;
 48 			} Elmo_GCON_Axis;
 49 		
 50 
 51 		public: 
 52 			SystemInterface_EtherCAT_Elmo_GCON() : _systemReady(false), _modeOp(OP_MODE_CYCLIC_SYNC_POSITION)
 53 			{
 54 			} 
 55 			~SystemInterface_EtherCAT_Elmo_GCON()
 56 			{
 57 				deinit();
 58 			}
 59 			
 60 			inline int init(int ModeOfOperation, UINT32 CycleNano)
 61 			{
 62 				if (_master.init(CycleNano) < 0)
 63 					return -1;
 64 
 65 				// TODO: Init Axis parameters
 66 
 67 				for(int i=0; i<NUM_MOTION_AXIS; ++i){
 68 					Axis[i].InParam.TargetTorque=0;
 69 					Axis[i].InParam.MaxTorque=MAX_TORQUE;
 70 				}
 71 
 72 				_modeOp = ModeOfOperation;
 73 				return 0;
 74 			}
 75 
 76 			inline int deinit()
 77 			{
 78 			    _master.deinit();
 79 				return 0;
 80 			}
 81 
 82 			inline int servoOn2(float * const data)
 83 			{
 84 				unsigned int tempready[NUM_MOTION_AXIS];
 85 				UINT16 controlword[NUM_MOTION_AXIS];
 86 				for (int i=0; i<NUM_MOTION_AXIS; ++i){
 87 					tempready[i]=0;
 88 					controlword[i]=0;
 89 				}
 90 				
 91 				startCommDC();
 92 				
 93 				// TODO: Read servos' status to Axes' Status Word
 94 
 95 				for (int i=0; i<NUM_MOTION_AXIS; ++i)
 96 				{
 97 					_master.readDCDomain<INT32>(&(Axis[i].OutParam.PositionActualValue), 0, i, 0x6064, 0x00);
 98 					_master.readDCDomain<UINT16>(&(Axis[i].OutParam.StatusWord), 0, i, 0x6041, 0x00);
 99 					//rt_printf("Servo %d Status Word 0x%x \n", i, Axis[i].OutParam.StatusWord);
100 
101 					data[i] = Axis[i].InParam.TargetPosition = (float) Axis[i].OutParam.PositionActualValue;
102 				}
103 
104 
105 				// TODO: Compute and Write Control Word + Mode of Operation to servos
106 				// Note that you can call _computeCtrlWrd(StatusWord) in this case but you have to take care of the tempready[i] yourself
107 				// tempready[i] indicates that the ith servo is ready for operation
108 
109 				for (int i=0; i<NUM_MOTION_AXIS; ++i){
110 					if (!tempready[i]){
111 						rt_printf("Servo %d Status Word 0x%x \n", i, Axis[i].OutParam.StatusWord);
112 						if (!(Axis[i].OutParam.StatusWord & (1<<STATUSWORD_OPERATION_ENABLE_BIT))) {
113 							if (!(Axis[i].OutParam.StatusWord & (1<<STATUSWORD_SWITCHED_ON_BIT))) {
114 								if (!(Axis[i].OutParam.StatusWord & (1<<STATUSWORD_READY_TO_SWITCH_ON_BIT))) {
115 									if ((Axis[i].OutParam.StatusWord & (1<<STATUSWORD_FAULT_BIT))) {
116 										controlword[i] = 0x80; //fault reset
117 									}
118 									else
119 									{
120 										controlword[i] = 0x06; //shutdown
121 									}
122 								} else {
123 									controlword[i] = 0x07; //switch on
124 									tempready[i]=1;
125 								}
126 							} else {
127 								controlword[i] = 0x0F; //switch on
128 								tempready[i]=1;
129 							}
130 						}
131 						else
132 						{
133 							controlword[i] = 0x0F; //switch on
134 							tempready[i]=1;
135 						}
136 						_master.writeDCDomain<INT32>(Axis[i].InParam.TargetPosition, 0, i, 0x607A, 0x00);
137 						_master.writeDCDomain<INT32>(Axis[i].InParam.TargetTorque, 0, i, 0x607A, 0x00);
138 						_master.writeDCDomain<UINT16>(controlword[i], 0, i, 0x6040, 0x00);
139 						_master.writeDCDomain<UINT8>(_modeOp, 0, i, 0x6060, 0x00);
140 					}
141 				}
142 
143 
144 				endCommDC();
145 
146 
147 				for (int i=0; i<NUM_MOTION_AXIS; ++i)
148 				{
149 					if (!tempready[i])
150 					{
151 						_systemReady = false;
152 						return -1;
153 					}
154 				}
155 				_systemReady = true;
156 
157 
158 				return 0;
159 			}
160 
161 			inline int servoOn(float * const data)
162 			{
163 				unsigned int tempready[NUM_MOTION_AXIS];
164 				UINT16 controlword[NUM_MOTION_AXIS];
165 				for (int i=0; i<NUM_MOTION_AXIS; ++i){
166 					tempready[i]=0;
167 					controlword[i]=0;
168 				}
169 
170 				startCommDC();
171 
172 				// TODO: Read servos' status to Axes' Status Word
173 
174 				for (int i=0; i<NUM_MOTION_AXIS; ++i)
175 				{
176 					_master.readDCDomain<INT32>(&(Axis[i].OutParam.PositionActualValue), 0, i, 0x6064, 0x00);
177 					_master.readDCDomain<UINT16>(&(Axis[i].OutParam.StatusWord), 0, i, 0x6041, 0x00);
178 					//rt_printf("Servo %d Status Word 0x%x \n", i, Axis[i].OutParam.StatusWord);
179 
180 					data[i] = Axis[i].InParam.TargetPosition = (float) Axis[i].OutParam.PositionActualValue;
181 				}
182 
183 
184 				// TODO: Compute and Write Control Word + Mode of Operation to servos
185 				// Note that you can call _computeCtrlWrd(StatusWord) in this case but you have to take care of the tempready[i] yourself
186 				// tempready[i] indicates that the ith servo is ready for operation
187 
188 				for (int i=0; i<NUM_MOTION_AXIS; ++i){
189 					if (!tempready[i]){
190 
191 						switch (Axis[i].OutParam.StatusWord & 0x6F)
192 						{
193 						case 0:	// No Initialization -> Request Initialization
194 							Axis[i].InParam.ControlWord = (Axis[i].InParam.ControlWord & 0xFF70) | 0;
195 							break;
196 
197 						case 0x40:
198 						case 0x60: // Initialization Completion -> Request Main Circuit Power OFF
199 							Axis[i].InParam.ControlWord = (Axis[i].InParam.ControlWord & 0xFF70) | 0x06;
200 							break;
201 
202 						case 0x21: // Main Circuit Power OFF -> Request Servo Ready
203 							Axis[i].InParam.ControlWord = (Axis[i].InParam.ControlWord & 0xFF70) | 0x07;
204 							tempready[i]=1;
205 							break;
206 
207 						case 0x23: //Servo Ready -> Request Servo ON
208 							Axis[i].InParam.ControlWord = (Axis[i].InParam.ControlWord & 0xFF70) | 0x0F;
209 							tempready[i]=1;
210 							break;
211 
212 						case 0x27: // Servo ON State (Ready for operation)
213 							tempready[i]=1;
214 							break;
215 						case 0x2F: // In Abnormal Process
216 							break;
217 
218 						case 0x28: // Abnormal State -> Request Initialization
219 							Axis[i].InParam.ControlWord = (Axis[i].InParam.ControlWord & 0xFF70) | 0x80;
220 							break;
221 						}
222 
223 						//rt_printf("Servo %d Status 0x%x Control 0x%x\n", i, Axis[i].OutParam.StatusWord /*& 0x6F*/);
224 
225 						_master.writeDCDomain<INT32>(Axis[i].InParam.TargetPosition, 0, i, 0x607A, 0x00);
226 						_master.writeDCDomain<INT32>(Axis[i].InParam.TargetTorque, 0, i, 0x607A, 0x00);
227 						_master.writeDCDomain<UINT16>(Axis[i].InParam.ControlWord, 0, i, 0x6040, 0x00);
228 						_master.writeDCDomain<UINT8>(_modeOp, 0, i, 0x6060, 0x00);
229 					}
230 				}
231 
232 
233 				endCommDC();
234 
235 
236 				for (int i=0; i<NUM_MOTION_AXIS; ++i)
237 				{
238 					if (!tempready[i])
239 					{
240 						_systemReady = false;
241 						return -1;
242 					}
243 				}
244 				_systemReady = true;
245 
246 
247 				return 0;
248 			}
249 
250 			inline int startCommDC()
251 			{
252 				int res = -1;
253 				res = _master.startDCDomain();
254 				return res;
255 			}
256 			virtual void readData(float * const data)
257 			{
258 				// TODO: Read feedback data from servos to axes via master
259 
260 				for(int i=0; i<NUM_MOTION_AXIS; ++i){
261 					_master.readDCDomain<UINT16>(&(Axis[i].OutParam.StatusWord), 0, i, 0x6041, 0x00);
262 					_master.readDCDomain<INT8>(&(Axis[i].OutParam.ModeOfOperationDisplay), 0, i, 0x6061, 0x00);
263 					_master.readDCDomain<INT32>(&(Axis[i].OutParam.PositionActualValue), 0, i, 0x6064, 0x00);
264 					_master.readDCDomain<INT16>(&(Axis[i].OutParam.TorqueActualValue), 0, i, 0x6077, 0x00);
265 
266 					data[i] = (float) Axis[i].OutParam.PositionActualValue;
267 				}
268 
269 			}
270 			virtual void writeData(float const * const data)
271 			{
272 				// TODO: Write control data from axes to servos via master
273 
274 				for (int i=0; i<NUM_MOTION_AXIS; ++i)
275 				{
276 					if (_modeOp == OP_MODE_CYCLIC_SYNC_TORQUE)
277 						Axis[i].InParam.TargetTorque = (INT16) data[i];
278 					else
279 						Axis[i].InParam.TargetPosition = (INT32) data[i];
280 
281 					_master.writeDCDomain<INT32>(Axis[i].InParam.TargetPosition, 0, i, 0x607A, 0x00);
282 					_master.writeDCDomain<INT32>(Axis[i].InParam.TargetVelocity, 0, i, 0x60FF, 0x00);
283 					_master.writeDCDomain<INT16>(Axis[i].InParam.TargetTorque, 0, i, 0x6071, 0x00);
284 					_master.writeDCDomain<UINT16>(Axis[i].InParam.MaxTorque, 0, i, 0x6072, 0x00);
285 					_master.writeDCDomain<UINT16>(_computeCtrlWrd(Axis[i].OutParam.StatusWord), 0, i, 0x6040, 0x00);
286 					_master.writeDCDomain<UINT8>(_modeOp, 0, i, 0x6060, 0x00);
287 				}
288 
289 			}
290 			inline int endCommDC()
291 			{
292 				int res = -1;
293 				res = _master.endDCDomain();
294 				return res;
295 			}
296 
297 			inline UINT16 getAxisStatus(int AxisIdx)
298 			{
299 				UINT16 Status;
300 				
301 				// TODO: Return appropriate servo's status
302 				Status = Axis[AxisIdx].OutParam.StatusWord;
303 				return Status;
304 			}
305 			inline bool isSystemReady()
306 			{
307 				return _systemReady;
308 			}
309 
310 		private:
311 			inline UINT16 _computeCtrlWrd(UINT16 StatWrd)
312 			{
313 				if (!(StatWrd & (1<<STATUSWORD_OPERATION_ENABLE_BIT)))
314 				{
315 					if (!(StatWrd & (1<<STATUSWORD_SWITCHED_ON_BIT))) {
316 						if (!(StatWrd & (1<<STATUSWORD_READY_TO_SWITCH_ON_BIT))) {
317 							if ((StatWrd & (1<<STATUSWORD_FAULT_BIT))) {
318 								return 0x80; //fault reset
319 							}
320 							else
321 							{
322 								return 0x06; //shutdown
323 							}
324 						}
325 						else
326 						{
327 							return 0x07; //switch on
328 						}
329 					}
330 					else
331 					{
332 						return 0x0F; //switch on
333 					}
334 				}
335 				else
336 				{
337 					return 0x0F; //switch on
338 				}
339 
340 				return 0;
341 			}
342 		private: 
343 			EcatMaster_Elmo_GCON _master;
344 			
345 			Elmo_GCON_Axis Axis[NUM_ELMO_GCON_AXES];		//Elmo motor
346 						
347 
348 			UINT8 _modeOp;
349 			bool _systemReady;
350 	}; 
351 }


Real-time Control Application with NRMKEtherCAT System Interface

  1 #ifndef __XENO__
  2 #define __XENO__
  3 #endif
  4 
  5 #include <stdio.h>
  6 #include <stdlib.h>
  7 #include <signal.h>
  8 #include <unistd.h>
  9 #include <sys/mman.h>
 10 #include <string.h>		// string function definitions
 11 #include <fcntl.h>		// File control definitions
 12 #include <errno.h>		// Error number definitions
 13 #include <termios.h>	// POSIX terminal control definitions
 14 #include <time.h>		// time calls
 15 #include <sys/ioctl.h>
 16 #include <math.h>
 17 
 18 //-xenomai-///////////////////////////////////////////////////////////////
 19 #include <native/task.h>
 20 #include <native/timer.h>
 21 #include <native/mutex.h>
 22 #include <rtdk.h>		//The rdtk real-time printing library
 23 
 24 
 25 //EtherCAT Master ******************************************************************
 26 #include "ecrt.h"
 27 
 28 #include "SystemInterface_EtherCAT_Elmo_GCON.h"
 29 
 30 #define NUM_ELMO_GCON_AXIS	(4)		//Modify this number to indicate the actual number of motor on the network
 31 #define Max_Torque	2000
 32 
 33 #ifndef PI
 34 #define PI	(3.14159265359)
 35 #define PI2	(6.28318530718)
 36 #endif
 37 
 38 #define WAKEUP_TIME		(10)	// wake up timeout before really run, in second
 39 #define NSEC_PER_SEC 			1000000000
 40 unsigned int cycle_ns = 1000000; /* 1 ms */
 41 
 42 typedef unsigned int UINT32;
 43 typedef int32_t INT32;
 44 typedef int16_t INT16;
 45 typedef uint16_t UINT16;
 46 typedef uint8_t UINT8;
 47 typedef int8_t INT8;
 48 
 49 NRMKHelper::SystemInterface_EtherCAT_Elmo_GCON _systemInterface_EtherCAT_Elmo_GCON;
 50 
 51 int system_ready = 0;
 52 
 53 int des_torque = 100;
 54 double sine_amp=50000, gt=0, f=0.5, period;
 55 
 56 int zeropos[NUM_ELMO_GCON_AXIS]={0,0,0,0}, despos[NUM_ELMO_GCON_AXIS]={0,0,0,0};
 57 float q[NUM_ELMO_GCON_AXIS] = {0,0,0,0};
 58 float tau[NUM_ELMO_GCON_AXIS] = {0,0,0,0};
 59 
 60 
 61 
 62 /****************************************************************************/
 63 
 64 RT_TASK Elmo_GCON_task;
 65 RT_TASK print_task;
 66 
 67 static int run = 1;
 68 unsigned long fault_count=0;
 69 long ethercat_time=0, worst_time=0;
 70 #define min_time	0
 71 #define max_time	100000
 72 #define hist_step	(100)
 73 unsigned int histdata[hist_step+1];
 74 unsigned int interval_size=350;
 75 
 76 void signal_handler(int signum);
 77 
 78 void compute()
 79 {
 80 //	int i;
 81 //	for (i=0; i<NUM_ELMO_GCON_AXIS; ++i)
 82 //	{
 83 //		if (system_ready)
 84 //			tau[i]=des_torque;
 85 //		else
 86 //			tau[i]=0;
 87 //	}
 88 
 89 
 90 	int i;
 91 	for (i=0; i<NUM_ELMO_GCON_AXIS; ++i)
 92 	{
 93 		if (system_ready)
 94 		{
 95 			tau[i]=(int) (sine_amp*(sin(PI2*f*gt))) + zeropos[i];
 96 		}
 97 		else
 98 			tau[i]=q[i];
 99 	}
100 }
101 
102 // Elmo_GCON_task	
103 void Elmo_GCON_run(void *arg)
104 {
105 	unsigned int cycle_counter = 0, res=0, runcount=0;
106 	RTIME now, previous;
107 	rt_task_set_periodic(NULL, TM_NOW, cycle_ns);
108 
109 	while (run)
110 	{
111 		rt_task_wait_period(NULL); 	//wait for next cycle
112 
113 		cycle_counter++;
114 		runcount++;
115 
116 		if (!run)
117 		{
118 			break;
119 		}
120 
121 		previous = rt_timer_read();
122 
123 		if (!_systemInterface_EtherCAT_Elmo_GCON.isSystemReady())
124 		{
125 			_systemInterface_EtherCAT_Elmo_GCON.servoOn(q);
126 			for(int i=0; i<NUM_ELMO_GCON_AXIS; ++i)
127 				zeropos[i] = q[i];
128 		}
129 		else
130 		{
131 			_systemInterface_EtherCAT_Elmo_GCON.startCommDC();
132 
133 			_systemInterface_EtherCAT_Elmo_GCON.readData(q);
134 			compute();
135 			_systemInterface_EtherCAT_Elmo_GCON.writeData(tau);
136 
137 			_systemInterface_EtherCAT_Elmo_GCON.endCommDC();
138 		}
139 
140 		now = rt_timer_read();
141 		ethercat_time = (long) now - previous;
142 
143 		if (_systemInterface_EtherCAT_Elmo_GCON.isSystemReady() && (runcount>WAKEUP_TIME*(NSEC_PER_SEC/cycle_ns)) )
144 		{
145 			system_ready=1;	//all drives have been done
146 		}
147 		if (system_ready){
148 			gt+= period;
149 			if (worst_time<ethercat_time) worst_time=ethercat_time;
150 		}
151 	}
152 }
153 
154 void print_run(void *arg)
155 {
156 	RTIME now, previous=0;
157 	int i;
158 	unsigned long itime=0, step;
159 	long stick=0;
160 	int count=0;
161 	rt_printf("\e[31;1m \nPlease WAIT at least %i (s) until the system getting ready...\e[0m\n", WAKEUP_TIME);
162 	rt_task_set_periodic(NULL, TM_NOW, 1e8);
163 	while (1)
164 	{
165 		if (++count==10)
166 		{
167 			++stick;
168 			count=0;
169 		}
170 		if (system_ready)
171 		{
172 			now = rt_timer_read();
173 			step=(unsigned long)(now - previous) / 1000000;
174 			itime+=step;
175 			previous=now;
176 			rt_printf("Time=%d.%d s, ", itime/1000, itime % 1000);
177 			rt_printf("dt= %li, worst= %li\n", ethercat_time, worst_time);
178 			for(i=0; i<NUM_ELMO_GCON_AXIS; ++i){
179 				rt_printf("\e[32;1mstatus: 0x%x,  \e[0m", 	_systemInterface_EtherCAT_Elmo_GCON.getAxisStatus(i));
180 				rt_printf("\e[32;1mpos: %i\e[0m\n", 	 	q[i]);
181 			}
182 			rt_printf("\n");
183 		}
184 		else
185 		{
186 			if (count==0){
187 				rt_printf("%i", stick);
188 				for(i=0; i<stick; ++i)
189 					rt_printf(".");
190 				rt_printf("\n");
191 			}
192 		}
193 
194 		rt_task_wait_period(NULL); //wait for next cycle
195 	}
196 }
197 
198 /****************************************************************************/
199 void signal_handler(int signum = 0)
200 {
201 	rt_task_delete(&Elmo_GCON_task);
202 	rt_task_delete(&print_task);
203     printf("Elmo drives Stopped!\n");
204 
205     _systemInterface_EtherCAT_Elmo_GCON.deinit();
206     exit(1);
207 }
208 
209 /****************************************************************************/
210 int main(int argc, char **argv)
211 {
212     rt_print_auto_init(1);
213 
214 	signal(SIGINT, signal_handler);
215 	signal(SIGTERM, signal_handler);
216 
217 	int i;
218 
219 	/* Avoids memory swapping for this program */
220 	mlockall(MCL_CURRENT|MCL_FUTURE);
221 
222 	//user can specify the cycle period (cycle_ns) here, or use default value (1ms)
223 	cycle_ns=500000; //500us: 2kHz
224 	des_torque = 300;
225 	f = 0.5;
226 	period=((double) cycle_ns)/((double) NSEC_PER_SEC);	//period in second unit
227 
228 	//_systemInterface_EtherCAT_Elmo_GCON.init(OP_MODE_CYCLIC_SYNC_TORQUE, cycle_ns);
229 	_systemInterface_EtherCAT_Elmo_GCON.init(OP_MODE_CYCLIC_SYNC_POSITION, cycle_ns);
230 
231 
232 	// Elmo_GCON_task: create and start
233 	printf("Now running rt task ...\n");
234 
235 	rt_task_create(&Elmo_GCON_task, "Elmo_GCON_task", 0, 99, 0);
236 	rt_task_start(&Elmo_GCON_task, &Elmo_GCON_run, NULL);
237 
238 	// printing: create and start
239 	rt_task_create(&print_task, "printing", 0, 75, 0);
240 	rt_task_start(&print_task, &print_run, NULL);
241 
242 	pause();
243 
244 	signal_handler();
245 
246     return 0;
247 }