Revision b6d0fdeb examples/yarp_icub/src/icub_jointinterface.cpp

View differences:

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