Algumas correções relativas ao intervalo de execução do programa.
Usei um comando sleep que eu não sei se funciona no NXT. A idéia desse comando é fazer o programa ser executado a cada Ts milissegundos, o sleep ou outro comando de retardo (delay, wait, ...) não é o ideal, pois o ciclo de execução equivale ao sleep(Ts)+Ti, onde Ti é o tempo de execução das instruções. O Ti varia de acordo com as instruções e cálculos executados, fazendo com que Ts+Ti possua um certo jitter, se Ts>>Ti é aceitável. O ideal seria se o programa tivesse um comando de escalonamento temporal com baixíssimo jitter.
float kp,ki,kd,integral,derivada;
#define imax 100
#define imin 0
#define val 50
const float Ts=100;
int offset,luz,erro,pa,pc,max=0,min=9999,contador,ultimoerro=0;
void calibracao(int &ma, int &min)
{
int aux;
OnRev(OUT_C, 25);
OnFwd(OUT_A, 25);
do
{
aux=Sensor(IN_1);
if(aux<min) min=aux;
if(aux>max) max=aux;
}
while(MotorRotationCount(OUT_A)<=120);
}
task main()
{
float ti,pid;
SetSensor(IN_1, SENSOR_COLORRED);
SetSensorMode(IN_1, IN_MODE_PCTFULLSCAL E);
calibracao(max, min);
offset=(max+min)/2;
OnRev(OUT_C, 25);
OnFwd(OUT_A, 25);
while(true)
{
luz=Sensor(IN_1);
if((luz>max+5||luz<min-5)&&contador==1)
{
OnFwd(OUT_C, 70);
OnRev(OUT_A, 30);
contador=0;
}
else if((luz>max+5||luz<min-5)&&contador==0) Off(OUT_AC);
erro=luz-offset;
integral+=erro;
if(integral>imax) integral=imax;
if(integral<imin) integral=imin;
derivada=erro-ultimoerro;
pid=kp*erro+ki*integral*Ts+kd*derivada/Ts;
pa=val-pid;
pc=val+pid;
OnFwd(OUT_A, pa);
if(pc>0) OnFwd(OUT_C, pc);
else
{
pc*=(-1);
OnRev(OUT_C, pc);
}
ultimoerro=erro;
sleep( Ts );
}
}