ПИД - регулятор оборотов двигателя

Здравствуйте, пытаюсь реализовать ПИД - регулятор двигателя постоянного тока. Управление осуществляется с помощью ШИМ сигнала, коэффициент заполнения изменяется от 0 до 10000. Скорость измеряю с помощью двух датчиков Холла, прерывание одного из них настроены по нарастающему и падающем фронту. Получается 2 инкремента или декремента на один оборот. Никак не получается настроить регулятор. Подозреваю, что главная проблема, когда управляющее воздействие становится меньше 0. Я пытался его умножать на -1, но это не помогло. Подскажите, в чем может быть проблема.

CВыделить код

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

int16_t PID_Reg(struct regulator parm, int zadanie, int measure)
{
  int16_t input, e=0, I_Temp, D_Temp;
  e=zadanie-measure;
  if(parm.I(parm.Summ_Error+e)<=parm.Max_Summ_Error)
 {
    I_Temp=parm.Summ_Error+e;
    parm.Summ_Error=I_Temp;
 }
  else
     I_Temp=parm.Summ_Error;
  D_Temp=parm.Last_Process_Value-measure;
  parm.Last_Process_Value=measure;
  input=parm.Pe+parm.II_Temp+parm.DD_Temp;
  if(input>MAX_VALUE)
    input=MAX_VALUE;
  if(input<0)
    input=0;
  return input;
}
 
void Init_PID_Reg(void)
{
  Reg1.P=72;
  Reg1.D=20 ;
  Reg1.I=210;
  Reg1.Summ_Error=0;
  Reg1.Max_Summ_Error=10000;
  Reg1.Last_Process_Value=0;
}
void Holl(void args)
{
    // Ïðåîáðàçîâàíèå àðãóìåíòà ê òðåáóåìîìó òèïó, â äàííîìó ñëó÷àå ê uint8_t
    uint8_t arg = ((uint8_t) args);
    // Ïðîâåðêà, ÷òî ïåðåäà÷à àðãóìåíòà ðàáîòàåò
      if(palReadPad(GPIOB,8u)==1)
      {
        if(palReadPad(GPIOB,9u)==1)
        {
          holl_speed++;
        }
        if(palReadPad(GPIOB,9u)==0)
        {
          holl_speed--;
        }
      }
      if(palReadPad(GPIOB,8u)==0)
      {
        if(palReadPad(GPIOB,9u)==0)
        {
          holl_speed++;
        }
        if(palReadPad(GPIOB,9u)==1)
        {
          holl_speed--;
        }
      }
}