Revision 196b8635
| 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",
|
|
| 254 |
printf("%f %f %f %f %f %f PID%d\n",
|
|
| 255 | 255 |
get_ts_position(id).get_interpolated_value(data_ts), |
| 256 | 256 |
target_angle[id], |
| 257 | 257 |
get_ts_speed(id).get_interpolated_value(data_ts), |
| 258 | 258 |
target_velocity, |
| 259 | 259 |
speed, |
| 260 |
position_error |
|
| 260 |
position_error, |
|
| 261 |
id |
|
| 261 | 262 |
); |
| 262 | 263 |
|
| 263 | 264 |
|
Also available in: Unified diff