CDRFLEx.speedj_rt | V2-manual

기능

외부 제어기에서 조인트 속도 제어를 하는 함수입니다.

인수

인수명	자료형	기본값	설명
fTargetVel	float[6]	-	타겟 조인트 속도. [deg/s]
fTargetAcc	float[6]	-	타겟 조인트 가속도 [deg/s²]. null값을 입력하면, 입력했던 타겟 조인트 위치 기반으로 자동 계산됩니다.
fTargetTime	float	-	타겟 시간 [s]

알아두기

Asnyc 명령어입니다. 호출된 후 다음 명령줄로 넘어갑니다.
내부 프로파일은 fTargetTime 에 (fTargetVel, fTargetAcc ) 에 도달하도록 보간됩니다.
fTargetTime <= controller’s control period(=1ms) 인 경우 보간없이 바로 해당 속도로 제어합니다.
fTargetVel에 도달할 때까지 다음 명령이 들어오지 않으면, 마지막 들어온 속도를 유지합니다.
단, 안전을 위해서 1[s] 동안 다음 명령이 들어오지 않으면 Time-Out으로 에러를 발생시키고 정지합니다.
모션 중 전역으로 설정된 가속도 제한값을 초과할 경우, 모션을 정지하지는 않고 Info 메시지를 발생시킵니다.

주의

현재 버전에서는 Operation Speed [%] 와 연동되지 않습니다.

리턴

값	설명
0	오류
1	성공

예제

C++

// main.cpp
CDRFLEx drfl;
drfl.connect_rt_control();
string version = “v1.0”;
float period = 0.001; // 1 msec
int losscount = 4;
Drfl.set_on_rt_monitoring_data(OnRTMonitoringData);
drfl.set_rt_control_output(version, period, losscount);
drfl.start_rt_control();

float time = 0.0;
int count = 0;
float q[NUMBER_OF_JOINT] = {0.0, };
float q_dot[NUMBER_OF_JOINT] = {0.0, };
float q_d[NUMBER_OF_JOINT] = {0.0, };
float q_dot_d[NUMBER_OF_JOINT] = {0.0, };
float vel_d[NUMBER_OF_JOINT] = {0.0, };
float acc_d[NUMBER_OF_JOINT] = {0.0, };
float kv[NUMBER_OF_JOINT] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; // have to tune
float kp[NUMBER_OF_JOINT] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; // have to tune
float ki[NUMBER_OF_JOINT] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; // have to tune
float integral_v_error[NUMBER_OF_JOINT] = {0.0, };
while (1)
{
    time=(++count)*period;
    // get current state
    memcpy(q, drfl.read_data_rt()->actual_joint_position, sizeof(float)*6);
    memcpy(q_dot, drfl.read_data_rt()->actual_joint_velocity, sizeof(float)*6);
    
    // make trajectory
    TrajectoryGenerator(q_d, q_dot_d); // Custom Trajectory Generation Function

    // velocity feedforward + pi controller
    for (int i=0; i<6; i++) {
        q_dot_v[i] = kv[i] * (q_d[i] - q[i]);
        integral_v_error[i] += q_dot_v[i] - q_dot[i];
        vel_d[i] = q_dot_d[i] + kp[i]*(q_dot_v[i] – q_dot[i]) + ki[i]*integral_v_error[i];
        acc_d[i] = -10000;
    }
    drfl.speedj_rt(vel_d, acc_d[i], period);

    if(time > plan1.time)
    {
        time=0;
        Drfl.stop(STOP_TYPE_SLOW);
        break;
    }

    rt_task_wait_period(NULL); // RTOS function
}