To nigdy nie jest tak, że proste PID załatwia sprawę. Trzeba dodać kilka elementów nieliniowych aby algorytm numeryczny nie "wyszedł poza zakres"
Przykładowa implementacja:

void pid_calculate_output( void )
{
I64 calculation;

/* Calculate the error */
PidProcessing.error_p = PidProcessing.command - PidProcessing.feedback;

/* Apply error limits */
if(PidProcessing.error_p > PidSettings.max_error_p)
{
PidProcessing.error_p = PidSettings.max_error_p;
}
else if (PidProcessing.error_p < -(I32)PidSettings.max_error_p)
{
PidProcessing.error_p = -(I32)PidSettings.max_error_p;
}

/* Apply the deadband */
if (PidProcessing.error_p > PidSettings.deadband )
{
PidProcessing.error_p -= PidSettings.deadband ;
}
else if (PidProcessing.error_p < -(I32)PidSettings.deadband)
{
PidProcessing.error_p += PidSettings.deadband;
}
else
{
PidProcessing.error_p = 0;
}

/* If output is in limit, don't let integrator wind up */
if ( (PidProcessing.control_flags & PID_ANTI_WIND_UP_LOCK) == 0 )
{
/* Compute integral term */
PidProcessing.error_i += PidProcessing.error_p;
}
/* Apply integrator limits */
if (PidProcessing.error_i > PidSettings.max_error_i)
{
PidProcessing.error_i = PidSettings.max_error_i;
}
else if (PidProcessing.error_i< -(I32)PidSettings.max_error_i)
{
PidProcessing.error_i = -(I32)PidSettings.max_error_i;
}

/* Calculate derivative of error */
PidProcessing.error_d = (PidProcessing.error_p - PidProcessing.error_p_prev);
PidProcessing.error_p_prev = PidProcessing.error_p ;

/* Apply derivative limits */
if (PidProcessing.error_d > PidSettings.max_command_d )
{
PidProcessing.error_d = PidSettings.max_command_d ;
}
else if (PidProcessing.error_d< -(I32)PidSettings.max_command_d )
{
PidProcessing.error_d = -(I32)PidSettings.max_command_d ;
}

/* Calculate derivative of command */
PidProcessing.command_d = PidProcessing.command - PidProcessing.command_prev;
PidProcessing.command_prev = PidProcessing.command;

/* Apply derivative commands limits */
if (PidProcessing.command_d > PidSettings.max_error_d)
{
PidProcessing.command_d = PidSettings.max_error_d;
}
else if (PidProcessing.command_d < -(I32)PidSettings.max_error_d)
{
PidProcessing.command_d = -(I32)PidSettings.max_error_d;
}

/* Calculate derivative of feedback */
PidProcessing.feedback_d = PidProcessing.feedback - PidProcessing.feedback_prev;
PidProcessing.feedback_prev = PidProcessing.feedback;

/* Apply derivative feedback limits */
if (PidProcessing.feedback_d > PidSettings.max_feedback_d)
{
PidProcessing.feedback_d = PidSettings.max_feedback_d;
}
else if (PidProcessing.feedback_d < -(I32)PidSettings.max_feedback_d)
{
PidProcessing.feedback_d = -(I32)PidSettings.max_feedback_d;
}



/* Calculate the output value */
calculation = (I64)PidProcessing.error_p * (I64)PidSettings.p_gain +
(I64)PidProcessing.error_i * (I64)PidSettings.i_gain +
(I64)PidProcessing.error_d * (I64)PidSettings.d_gain +
(I64)PidProcessing.command * (I64)PidSettings.ff0_gain +
(I64)PidProcessing.command_d * (I64)PidSettings.ff1_gain +
(I64)PidProcessing.feedback_d * (I64)PidSettings.b_gain;


/* Cut fractional part */
calculation = calculation >> 16;

if (calculation > (I64)PidSettings.max_output )
{
PidProcessing.output = PidSettings.max_output ;
PidProcessing.control_flags |= PID_ANTI_WIND_UP_LOCK;
}
else if (calculation < -(I64)PidSettings.max_output)
{
PidProcessing.output = -(I32)PidSettings.max_output ;
PidProcessing.control_flags |= PID_ANTI_WIND_UP_LOCK;
}
else
{
PidProcessing.output = (I32)calculation;
}
}

Sam sterownik też nie pociągnie - tylko enkoder
Przy częstotliwości pracy pętli PID w zakresie 2-10kHz nie możemy spodziewać się cudów ( zwłaszcza gdy PWM na silniku to u mnie 24kHz).

Myslę że realny zakres pracy LPT (pod dosem) wystarczy na taki sterownik serwa

UHU może nie, ale moja zabawka już tak - dsPIC ma hardware'owy układ enkodera ( pracującą z max częstotliwością 30MHz - efektywnie z filtrami ok 10MHz)