Pid em nxc

[Lord iTzClink]

Recentemente eu comecei a trabalhar com NXC que é uma linguagem baseada em C voltada para controlar o NXT (Mindstorms). Eu acabei de fazer um programa aqui aplicando PID em 2 sensores diferentes para seguir a linha, porém em muitos sites que vi, deve-se usar um tal de delay e eu não entendi muito bem isso. Está ai meu programa:

 

int black, white, mid_value;
int value_black1, value_black2;
int indicator;
float error1, error2, i1, d1, p1;
float i2, d2, p2, lte1, lte2;
float  ki, kd, kp;
inline void calibrate ()
{
  white = (Sensor(IN_1) + Sensor(IN_2)) / 2;
  while (Sensor(IN_1) < 50)
  {
    OnFwd(OUT_A, 50);
    OnRev(OUT_B, 50);
  }
  Off(OUT_AB);
  value_black1 = Sensor(IN_1);
  while (Sensor(IN_2) < 50)
  {
    OnFwd(OUT_B, 50);
    OnRev(OUT_A, 50);
  }
  Off(OUT_AB);
  value_black2 = Sensor(IN_2);
  black = (value_black1 + value_black2) / 2;
  mid_value = (white - black) / 2 + black;
}
task PID()
{
  while (true)
  {
    error1 = Sensor(IN_1) - mid_value;
    error2 = Sensor(IN_2) - mid_value;
    i1 = (i1 + error1) * ki;
    i2 = (i2 + error2) * ki;
    d1 = (error1 - lte1) * kd;
    d2 = (error2 - lte2) * kd;
    p1 = error1 * kp;
    p2 = error2 * kp;
    lte1 = error1;
    lte2 = error2;
  }
}
inline void line_follower()
{
  calibrate();
  while (indicator == 0)
  {
      OnFwd(OUT_A, p1 + d1 + i1);
      OnFwd(OUT_B, p2 + d2 + i2);
  }
}
task main()
{
  kp = 0;
  ki = 0;
  kd = 0;
  Precedes(PID);
  SetSensorLight(IN_1);
  SetSensorLight(IN_2);
  indicator = 0;
  calibrate();
  line_follower();
}
___________________________________________

 

Gostaria de saber se tenho q fazer alguma alteração no programa para que o PID funcione de forma mais eficaz.