========H==========
class Tpid_reg
{
    private:
    float integr;
    float diff;
    float error_prev;
    float error_f;
    float integr_prev;
    public:
    float dt;
    float coef_p;
    float coef_i;
    float coef_d;
    float calculate( s16 error );
    void clear( void );
};
========C==========
#include "pid_reg.h"
float Tpid_reg::calculate( s16 error)
{
  error_f = error; // приведение к float
  diff = ( error_f - error_prev ) / dt;
  integr = integr_prev + error_f * dt;
  error_prev = error_f;
  integr_prev = integr;
  return( coef_p * error_f + coef_i * integr + coef_d * diff );
}
void Tpid_reg::clear( void )
{
  error_prev = 0;
  integr_prev = 0;
}
========Использование==========
Ошибка есть разница между фазами
pid_reg.dt = WorkParametrs.period_grip_last; // период губок
// WorkParametrs.period_diff_last - разница между фазами пленки и губок
// WorkParametrs.period_grip_last - период губок
// WorkParametrs.period_tape_last - период пленки
    if (WorkParametrs.period_diff_last < (WorkParametrs.period_grip_last / 2))
        WorkParametrs.error = -WorkParametrs.period_diff_last;
    else
        WorkParametrs.error = WorkParametrs.period_grip_last - WorkParametrs.period_diff_last;
            WorkParametrs.adjust_value = pid_reg.calculate( WorkParametrs.error );
            if ( WorkParametrs.adjust_value < 0)
            {
                WorkParametrs.variator_dir = WorkParametrs.adjust_value;
                variator_change_on_time( VARIATOR_SET_FAST, abs( WorkParametrs.adjust_value )); // включить двигатель для замедления вариатора на N миллисекунд
            }
            else
            {
                WorkParametrs.variator_dir = WorkParametrs.adjust_value;
                variator_change_on_time( VARIATOR_SET_SLOW, WorkParametrs.adjust_value );
            }