Working on the calculation for PIDController