Revision adf38895

View differences:

examples/yarp_icub/src/icub_data_receiver.cpp
39 39 
    }
40 40 

  		




  41
  41
  
    
    //printf("\n%f %f %f TIME\n", get_timestamp_ms(), timestamps[2], positions[2]);


  		




  42
  
  
    
    printf("TIMEDIFF %f\n", get_timestamp_ms() - timestamps[2]);


  		




  
  42
  
    
    //printf("TIMEDIFF %f\n", get_timestamp_ms() - timestamps[2]);


  		




  43
  43
  
    

  		




  44
  44
  
    
    //tell humotion to update lid angle (hack)


  		




  45
  45
  
    
    icub_jointinterface->fetch_position(100, 0.0, get_timestamp_ms());


  		












  

    
      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