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] |
-
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.
-
In the current version, it is not linked with Operation Speed [%].
Return
|
Value |
Description |
|
0 |
Error |
|
1 |
Success |
Example
// 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
}