# Real-time control using STEP

STEP is a master PC designed for high speed real time distributed control using EtherCAT communication. To increase the usability of STEP's EtherCAT master capability, Neuromeca provides a GUI tool that automatically configures the EtherCAT slave interface (NRMKECatService). This chapter describes how to use the library API functions provided by PlatformSDK based on an automatically generated example code.

Note

Please refer to the step-by-step tutorial provided in the EtherCAT Manual for how to configure EtherCAT system interface using STEP (automatic code generation)

## Real-time control of Neuromeka Smart Actuator (CORE)

This section describes example code that implements real-time control of CORE100 using STEP. This section assumes that you have performed the following step-by-step automatic code generation (see the EtherCAT Manual).

Note

Please refer to CORE Manual for how to use Neuromeka's Smart Actuator CORE.

### 1. main function

The following example code shows the part of main function that initializes some variables and functions.

• cycle_ns, period: realtime control rate (nanosec, sec)

• _systemInterface_EtherCAT_[ProjectName]: instance of system interface class

• init(OP_MODE_CYCLIC_SYNC_TORQUE, cycle_ns): initilizing torque conrtol mode

• init(OP_MODE_CYCLIC_SYNC_POSITION, cycle_ns): initilizing position conrtol mode

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 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 cycle_ns=1000000; // nanosecond period=((double) cycle_ns)/((double) NSEC_PER_SEC); //period in second unit // Set the operation mode for EtherCAT servo drives // For CST (cyclic synchronous torque) control _systemInterface_EtherCAT_ECATTEST_CORE.init(OP_MODE_CYCLIC_SYNC_TORQUE, cycle_ns); // For CSP (cyclic synchronous position) control //_systemInterface_EtherCAT_ECATTEST_CORE.init(OP_MODE_CYCLIC_SYNC_POSITION, cycle_ns); // For trajectory interpolation initAxes(); … (Omittied) … } 

main function: initialization

The following example code represents part of the main function that opens a TCP/IP communication port and creates a xenomai task for real-time control.

• datasocket: Instance of the TCP/IP data socket server class. Data plots can be checked on the host PC through the Data Scope application.

• controlsoket: Instance of the TCP/IP control socket server class. Provide online command in connection with EtherCAT Configuration Tool

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 int main(int argc, char **argv) { … (Omittied) … // 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); // ECATTEST_CORE_task: create and start printf("Now running rt task ...\n"); rt_task_create(&ECATTEST_CORE_task, "ECATTEST_CORE_task", 0, 99, 0); rt_task_start(&ECATTEST_CORE_task, &ECATTEST_CORE_run, NULL); … (Omittied) … } 

main function: create TCP/IP server and xenomai task

### 2. Real-time control function

The following example code represents the function ECATTEST_CORE_run on which a real-time task is executed. The EtherCAT system interface instance (_systemInterface_EtherCAT_ [ProjectName]) enable to read and write Ethernet data.

• rt_task_set_periodic: set the real-time control rate (nano second)

• rt_task_wait_period: delay the task until the next periodic release point in the loop

• _systemInterface_EtherCAT_[ProjectName].processTxDomain: read EtherCAT data and assign it to a buffer

• _systemInterface_EtherCAT_[ProjectName].writeBuffer: assign data to the buffer to write EtherCAT data

• _systemInterface_EtherCAT_[ProjectName].processRxDomain: write EtherCAT data

• compute: implement control algorithm

• _q, _qdeg, _qdot, _qdotdeg: Data obtained by converting absolute encoder data to radian and degree values.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 void ECATTEST_CORE_run(void *arg) { unsigned int runcount=0; RTIME now, previous; // Synchronize EtherCAT Master (for Distributed Clock Mode) //_systemInterface_EtherCAT_NRMKCORETest.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_NRMKCORETest.processTxDomain(); _systemInterface_EtherCAT_NRMKCORETest.readBuffer(COE_POSITION, ActualPos); _systemInterface_EtherCAT_NRMKCORETest.readBuffer(COE_VELOCITY, ActualVel); _systemInterface_EtherCAT_NRMKCORETest.readBuffer(COE_TORQUE, ActualTor); _systemInterface_EtherCAT_NRMKCORETest.readBuffer(DATA_IN, DataIn); _systemInterface_EtherCAT_NRMKCORETest.readBuffer(0x60610, Modesofoperationdisplay); _q = (double) (ActualPos - ZeroPos)/6619136*PI2; // Radian pos _qdeg = (double) (ActualPos - ZeroPos)/6619136*360; // Deg pos _qdot = (double) ActualVel/6619136*PI2; // Radian vel _qodtdeg = (double) ActualVel/6619136*360; // Deg vel /// TO DO: Main computation routine... compute(); /// TO DO: write data to actuators in EtherCAT system interface _systemInterface_EtherCAT_NRMKCORETest.writeBuffer(COE_POSITION, TargetPos); _systemInterface_EtherCAT_NRMKCORETest.writeBuffer(COE_VELOCITY, TargetVel); _systemInterface_EtherCAT_NRMKCORETest.writeBuffer(COE_TORQUE, TargetTor); _systemInterface_EtherCAT_NRMKCORETest.writeBuffer(DATA_OUT, DataOut); _systemInterface_EtherCAT_NRMKCORETest.processRxDomain(); // For EtherCAT performance statistics now = rt_timer_read(); ethercat_time = (long) now - previous; … (Omittied) … } } 

### 3. compute function

The following is an example implementation of the control algorithm written in the compute function. First, the following [example code] (#control_algorithm_function1) is an example of applying open-loop sinusoidal wave torque command. When the calculated target torque is assigned to the TargetTor variable, the torque is applied to CORE through _systemInterface_EtherCAT.writeBuffer in the ECATTEST_CORE_run function.

  1 2 3 4 5 6 7 8 9 10 11 12 int compute() { // CST for (int i=0; i

controller implementation: applying open-loop sinusoidal wave torque

Example code below is an example of implementing closed-loop PID control. Calculate the output torque using the PID controller function of the class object PIDctr.

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 int compute() { // CST // PID control if (system_ready){ if (flag_init_pos == 0){ q_init = q; flag_init_pos = 1; } } controllers PIDctr; qd = q_init + 1*sin(PI2*0.2*gt); // Desired position: sine 1Hz double k_pid[3] = {45.0,85.0,60.0}; // PID control gain for (int i=0; i

controller implementation: closed-loop PID control

  1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 controllers::controllers() : m_dT(0.001) , eint_dr(NULL) , e_dr_prev(NULL) , edot_dr_prev(NULL) { } controllers::~controllers() { } double controllers::PIDcontroller(double k[3], double y, double y_des){ double kp = k[0], ki = k[1], kd = k[2]; double e_dr = y_des - y; double edot_dr = FilteredDerivative(e_dr_prev, e_dr, edot_dr_prev, 20); eint_dr += (e_dr + e_dr_prev)*m_dT; e_dr_prev = e_dr; edot_dr_prev = edot_dr; return (kp*e_dr + ki*eint_dr + kd*edot_dr); } double controllers::LowPassFilter(double input_prev, double input_present, double output_prev, double cutoff) { return ((1 - cutoff*m_dT/2)/(1 + cutoff*m_dT/2)*output_prev + (cutoff*m_dT/2)/(1 + cutoff*m_dT/2)*(input_present + input_prev)); } double controllers::FilteredDerivative(double input_prev, double input_present, double output_prev, double cutoff) { double ALPHA = ( (2 * cutoff * m_dT) / (2 + cutoff*m_dT) ); double output_present = ALPHA * ( (input_present - input_prev)/m_dT) + (1 - ALPHA) * output_prev; return output_present; } 

controller class: PID controller, low-pass filter, filter derivative