Revision b6d0fdeb
| examples/yarp_icub/src/icub_jointinterface.cpp | ||
|---|---|---|
| 251 | 251 |
//finally do a PD loop to get the target velocity |
| 252 | 252 |
double target_velocity = PID_P[id] * position_error + PID_D[id]*error_d + speed; |
| 253 | 253 |
|
| 254 |
printf("%f %f %f %f %f %f PID\n",
|
|
| 255 |
get_ts_position(id).get_interpolated_value(data_ts), |
|
| 256 |
target_angle[id], |
|
| 257 |
get_ts_speed(id).get_interpolated_value(data_ts), |
|
| 258 |
target_velocity, |
|
| 259 |
speed, |
|
| 260 |
position_error |
|
| 261 |
); |
|
| 262 |
|
|
| 263 |
|
|
| 254 | 264 |
//if (id == ICUB_ID_NECK_PAN) speed = -speed; |
| 255 | 265 |
ivel->velocityMove(id, target_velocity); |
| 256 | 266 |
printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],target_angle[id],distance,speed);
|
Also available in: Unified diff