CDRFLEx.speedj_rt

Features

This is a function that controls the joint velocity from an external controller.

Parameter

Parameter Name	Data Type	Default Value	Description
fTargetVel	float[6]	-	Target Joint Velocity. [deg/s]
fTargetAcc	float[6]	-	Target Joint Acceleration [deg/s2]. If None Values(-10000) entered, it is automatically calculated based on the target joint velocity you entered.
fTargetTime	float	-	Target Time [s]

Note

Asnyc Command.
The inner profile is interpolated to reach (fTargetVel, fTargetAcc ) at fTargetTime .
If fTargetTime <= controller’s control period (=1ms), control is performed at the corresponding speed without interpolation.
If the next command is not received until arriving at fTargetVel, the last input velocity is maintained.
However, for safety, if the next command is not received for 0.1[s], an error is generated as a time-out and stops.
If the acceleration limit set globally during motion is exceeded, the motion is not stopped and an Info message is generated.

Caution

In the current version, it is not linked with Operation Speed [%].

Return

Value	Description
0	Error
1	Success

Example

CPP

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