PlatformSDK 라이브러리 API 함수

STEP은 EtherCAT 통신을 이용한 고속 실시간 분산제어를 목적으로 만들어진 마스터 PC입니다. STEP의 이더켓 마스터로서의 사용성을 높이기 위해 뉴로메카는 EtherCAT 슬레이브 인터페이스를 자동으로 구성해주는 GUI 툴을 제공합니다 (NRMKECatService). 본 장에서는 자동 생성 된 예제 코드를 기반으로 PlatformSDK 가 제공하는 라이브러리 API 함수들의 사용법을 설명합니다.

참고

STEP의 EtherCAT 시스템인터페이스 구성 방법 (자동 코드생성)은 EtherCAT 매뉴얼에서 제공하는 단계별 튜토리얼을 참고하십시오.

CORE 제어 예제 코드

이 절에서는 STEP 을 이용하여 CORE100 의 실시간 제어를 구현한 예제 코드를 설명합니다. 먼저 다음의 단계별 자동 코드생성을 수행했다고 가정합니다 (EtherCAT 매뉴얼 참조).

참고

뉴로메카 스마트액츄에이터 CORE에 대한 사용방법은 CORE Manual을 참고하시기 바랍니다.

1. main 함수

다음의 예제코드는 변수 및 함수들을 초기화를 수행하는 main함수의 일부분을 나타냅니다.

• cycle_ns, period: 실시간 제어주기 (nano초/초)

• _systemInterface_EtherCAT_[ProjectName]: 시스템인터페이스의 클래스 객체

• init(OP_MODE_CYCLIC_SYNC_TORQUE, cycle_ns): 토크 제어모드로 초기화

• init(OP_MODE_CYCLIC_SYNC_POSITION, cycle_ns): 위치 제어모드로 초기화

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 함수: 초기화

다음의 예제코드는 TCP/IP 통신포트를 개방하고 실시간 제어를 위한 xenomai 태스크를 생성하는 main함수의 일부분을 나타냅니다.

• datasocket: TCP/IP 데이터 소켓 서버 클래스의 객체. Data Scope 응용프로그램을 통해 호스트PC에서 실시간 데이터 plot을 확인할 수 있음.

• controlsoket: TCP/IP 컨트롤 소켓 서버 클래스의 객체. EtherCAT Configuration Tool과 연동하여 온라인 커맨드를 제공

• rt_task_create, rt_task_start: xenomai 실시간 태스크 생성 및 시작. ECATTEST_CORE_run 함수를 실시간 태스크의 시작점으로 지정.

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 함수: TCP/IP 서버 생성 및 xenomai 태스크 생성

2. 실시간 제어 함수

아래의 예제코드는 실시간 태스크가 실행되는 함수 ECATTEST_CORE_run 을 나타냅니다. 이더켓 시스템 인터페이스 객체 (_systemInterface_EtherCAT_[ProjectName])를 통해 이더켓 데이터를 읽고 쓸 수 있습니다.

• rt_task_set_periodic: 실시간 제어주기 설정 (nano second)

• rt_task_wait_period: 메인 루프에서 설정한 제어주기만큼 대기

• _systemInterface_EtherCAT_[ProjectName].processTxDomain: 이더켓 데이터를 읽고 이를 버퍼에 할당

• _systemInterface_EtherCAT_[ProjectName].readBuffer: 버퍼로부터 데이터 획득

• _systemInterface_EtherCAT_[ProjectName].writeBuffer: 이더켓 데이터를 쓰기 위한 버퍼에 할당

• _systemInterface_EtherCAT_[ProjectName].processRxDomain: 이더켓 데이터 쓰기

• compute: 제어 알고리즘 구현 부분

• _q, _qdeg, _qdot, _qdotdeg: 절대 엔코더 데이터를 radian, degree 값으로 변환한 데이터

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 함수

다음은 compute 함수 내에 작성된 제어알고리즘 구현 예제 입니다. 첫 번째로 아래의 예제코드는 오픈루프 사인파 토크 명령을 인가하는 예제입니다. 계산된 타겟토크를 TargetTor 변수에 할당하면 ECATTEST_CORE_run함수안의 _systemInterface_EtherCAT.writeBuffer 를 통해 CORE로 토크가 인가 됩니다.

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;
}

제어기 구현: 오픈루프 사인파 토크 인가

아래의 예제코드는 폐루프 PID제어를 구현한 예제입니다. 클래스 객체 PIDctr의 PID제어기 함수를 이용하여 출력 토크를 계산합니다.

• PIDctr: 사용자에 의해 정의된 제어기 클래스의 객체 (제어기 코드 참고)

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;
}

제어기 구현: 폐루프 PID 제어

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;
}

제어기 구현: PID 제어기, 저주파 통과 필터, 필터 미분기