Revision adf38895 examples/yarp_icub/src/icub_jointinterface.cpp

View differences:

examples/yarp_icub/src/icub_jointinterface.cpp
242 242 
    //ivel->velocityMove(id, speed);
243 243 
    if ((id == ICUB_ID_NECK_PAN)  || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) || (id == ICUB_ID_EYES_BOTH_UD) ||  (id == ICUB_ID_NECK_TILT) ){
244 244 
        //do a pd control for velocity moves that incorporates position errors:
245 
        humotion::Timestamp data_ts = get_ts_position(id).get_last_timestamp();
245 
        humotion::Timestamp data_ts = get_ts_position(e).get_last_timestamp();
246 246 
        //TODO: add interpolation into future!
247 247 
        //humotion::Timestamp data_ts = humotion::Timestamp::now(); and extend get_interpol value with get_future_value
248 
        double position_error = target_angle[id] - get_ts_position(id).get_interpolated_value(data_ts);
248 
        double position_error = target_angle[id] - get_ts_position(e).get_interpolated_value(data_ts);
249 249 
        double error_d = (position_error - last_position_error[id]) / (framerate*1000.0);
250 250 
        last_position_error[id] = position_error;
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
  254
  
    
        printf("%f %f %f %f %f %f PID%d\n",


  		




  255
  
  
    
               get_ts_position(id).get_interpolated_value(data_ts),


  		




  
  255
  
    
               get_ts_position(e).get_interpolated_value(data_ts),


  		




  256
  256
  
    
               target_angle[id],


  		




  257
  
  
    
               get_ts_speed(id).get_interpolated_value(data_ts),


  		




  
  257
  
    
               get_ts_speed(e).get_interpolated_value(data_ts),


  		




  258
  258
  
    
               target_velocity,


  		




  259
  259
  
    
               speed,


  		




  260
  260
  
    
               position_error,


  		












Also available in:  Unified diff