ПИД-регулятор (ПИ-регулятор) сервомотора

#define PIN_ENA 6 //ШИМ (0 -255)
#define PIN_IN1 5 //направление вращения мотора 
#define PIN_IN2 4 //направление вращения мотора
#define PIN_POT A7 //чтение данных с энкодера (0 -1023)
#define PIN_ANGLEIN A6 // Получение угла с основного контролера (0 -1023)
#define iMin -0.2
#define iMax 0.2
 
float kp = 1; 
 
float ki = 0.001;
float iSum = 0;
 
float kd = 1;
float old_y = 0;
 
float PIDctl(float error, float y)
{
  float up, ui, ud;
 
  up = kp * error;
 
  iSum = iSum + error;
 
  if (iSum < iMin) iSum = iMin;
  if (iSum > iMax) iSum = iMax;
 
  ui = ki * iSum;
 
  ud = kd * (y - old_y);
  old_y = y;
 
  return up + ui;
}
 
void setup()
{
  Serial.begin(9600);
  Serial.println("PID, Encoder");
  pinMode(PIN_POT, INPUT);
  pinMode(PIN_ENA, OUTPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_ANGLEIN, INPUT);
}
 
void loop()
{
  //int angleInput = map(analogRead(PIN_ANGLEIN), 0, 255, 0, 1023);//заготовка под внешний сигнал
  int angleInput = 512;//ввод в ручную, 512 это 90 градусов в формате 0 -1023
 
  int angleEncoder = analogRead(PIN_POT);//читаю данные с экодера в формате 0 -1023
 
  float PID = PIDctl(angleInput - angleEncoder, angleEncoder);
 
  if (PID < 0) // вращение в одну сторону
  {
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  }
  else if (PID > 0) // вращение в другую сторону
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }
 
  Serial.print(PID);
  Serial.print(", ");
  Serial.println(map(angleEncoder, 0 , 1023, 0, 180));//перевожу значение с энкодера в градусы чтоб читалось удобнее
 
  analogWrite(PIN_ENA, map(abs(PID), 0, 1023, 100, 255));//перевожу значение ПИД в ШИМ и отдаю на драйвер мотора, ШИМ меньше 100 мотор не крутит.
 
  if (angleEncoder == angleInput)
  {
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, LOW);
 
  }
}