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

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

• rt_task_create, rt_task_start: Create and start xenomai real-time tasks. Specify the ECATTEST_CORE_run function as the starting point for real-time tasks.

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].readBuffer: read the data from the 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.

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)  …
    }
}

real-time task: read and write EtherCAT data

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.

int compute()
{
// CST
for (int i=0; i<NUM_AXIS; ++i)
{
    if (system_ready)
        TargetTor[i]=100*sin(PI2*f*gt);
    else
        TargetTor[i]=0;
}
return 0;
}

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.

• PIDctr: Object of controller class defined by user (see Controller Code)

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<NUM_AXIS; ++i)
    {
        if (system_ready)           
            TargetTor[i] = PIDctr.PIDcontroller(k_pid,_q,qd);
        else
            TargetTor[i]=0;
    }
return 0;
}

controller implementation: closed-loop PID control

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