[vesc_servo_controller.cpp] Nonlinear treatment of integral terms at saturation boundaries

[TakaakiNumai]

Abstract

In the following program, the behavior of the I-control appears to be non-linear near saturation.

vesc/vesc_hw_interface/src/vesc_servo_controller.cpp

Lines 93 to 101 in 09f7414

 double error_integ_new = error_integ_ + (error_current + error_previous_) / 2.0 * dt; 
 const double u_pid = u_pd + Ki_ * error_integ_new; 
  
 // not use I control if PID input is saturated 
 // since error integration causes bugs 
 if (isSaturated(u_pid)) 
 { 
   u = u_pd; 
 } 

I think the correct solution is as follows.

    if (isSaturated(u_pid))
    {
      u = saturate(u_pid);
    }