Autor Tópico: Lego NXT Seguidor de linha + Controle PID  (Lida 12214 vezes)

Description:

0 Membros e 1 Visitante estão vendo este tópico.

Offline Tita

  • Novato
  • *
  • Posts: 11
  • Sexo: Feminino
  • GUIA CNC
  • Cidade - UF: Belo Horizonte
  • Nome:: Ana
  • Profissão: Estudante
Lego NXT Seguidor de linha + Controle PID
« Online: 26 de Agosto de 2013, 23:30 »
Olá povo,
gostaria de saber como implementar um controle PID num NXT para ele seguir uma linha sem zigue-zaguear. E se ele perder a referência no final da linha ele deve girar e fazer o caminho inverso. Obrigada.

(Como obter o dt?)

Offline Cássio Alvarenga

  • Moderador
  • CNCMASTER
  • ******
  • Posts: 3429
  • Sexo: Masculino
    • www.vatech.ind.br
  • Cidade - UF: Belo Horizonte - MG
  • Profissão: Empresario / Engenheiro Mecânico
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #1 Online: 27 de Agosto de 2013, 09:48 »
É comandado em C++ ? Bota o código ai que eu te ajudo.

 Creio que um delay na leitura do sensor da linha e/ou uma mudança de direção mais suave corrige o zig-zag.

Offline Tita

  • Novato
  • *
  • Posts: 11
  • Sexo: Feminino
  • GUIA CNC
  • Cidade - UF: Belo Horizonte
  • Nome:: Ana
  • Profissão: Estudante
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #2 Online: 28 de Agosto de 2013, 13:07 »
Não. Em NXT-G mesmo.
Eu queria entender o algoritmo PID para poder tentar aplicar.
Seguindo essa aproximação http://1.bp.blogspot.com/-vSRUzfiSOuc/UTZ1yrM5DgI/AAAAAAAABYU/1qNvS-4t5O8/s640/ScreenHunter_15.bmp
Lego NXT Seguidor de linha + Controle PID

http://3.bp.blogspot.com/-2V4K8y69jh0/UTZ32MtQJII/AAAAAAAABYo/Ak3h-Proz0A/s1600/ScreenHunter_17.bmp
Lego NXT Seguidor de linha + Controle PID
, como eu calculo Kp, Ki e Kd? E esse T, é em rellação a quê?
Obrigada (desculpe a minha ignorância sobre o assunto).

Offline F.Gilii

  • Administrador
  • CNCMASTER
  • *******
  • Posts: 13796
  • Sexo: Masculino
  • Consultoria, Automação e Construção de Máquinas
  • Cidade - UF: Atibaia - SP
  • Nome:: Fabio Gilii
  • Profissão: Tec. Mecanica de Precisão
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #3 Online: 28 de Agosto de 2013, 14:30 »
Não que eu entenda disso, mas do pouco que sei é que um controle PID compara um valor medido num processo com um valor de referencia - a diferença (ou sinal de erro) é usada para calcular um novo valor inserido numa entrada do processo de forma a alterar os valores medidos no processo para trazê-los de volta a certos patamares ou "set points" desejáveis.

O valor proporcional é aquele que lida com o presente - o erro é multiplicado (valor negativo) por uma constante "P" e adicionado (na verdade subtraído por ser negativo) ao valor da quantidade controlada.

Integral é o valor que lida com o passado - o erro é integralizado (somado) sobre um período de tempo, e depois é multiplicado (valor negativo) por uma constante "I" assim fazendo uma média, que então é adicionado (na verdade subtraido por ser negativo) ao valor da quantidade.

O Derivativo é aquele que lida com o futuro - o valor derivativo sobre um periodo de tempo(ou a rampa do erro) é calculado e depois é multiplicado (valor negativo) por uma constante "D" que é igualmente adicionado (na verdade também subtraído por ser negativo) ao valor da quantidade.

Dizem que (tecnicamente) um controle em Loop PID pode ser caracterizado por um sistema complexo de controle de frequencia - muito útil para calcular quando o sistema alcançará estabilidade (ou equilibrio).

Casos os valores sejam imputados incorretamente, a entrada do processo pode oscilar, e a saído do processo poderá nunca estar no mesmo ponto.

Sei que tem umas regrinhas para ajudar na interpretação de alguns sintomas, mais não tenho muita experiencia nisso...

Por exemplo, se sua aplicação pode ser monitorada através de um osciloscópio (como nas servos que eu mexo), uso o seguinte método (que nem sempre pode ser usado - depende do sistema)

- deixo os valores I e D em zero
- aumento o valor P até que o sistema se torne oscilante na saída
- vou aumentando I até que a oscilação pare
- vou aumentando D até que o loop se torne aceitável isto é, que chegue ao valor pré determinado rapidamente - por exemplo em alguns casos deixo o sistema rápido mesmo que haja um pouco de "overshooting" do sinal...

Offline minilathe

  • How to ask questions - The Smart Way...
  • Moderadores
  • CNCMASTER
  • ******
  • Posts: 4295
  • Sexo: Masculino
  • http://science-lakes.com/article43-html.html
  • Cidade - UF: Rio de Janeiro - RJ
  • Nome:: Gil Pinheiro
  • Profissão: Engenheiro e professor universitário
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #4 Online: 28 de Agosto de 2013, 18:08 »
Não. Em NXT-G mesmo.
Eu queria entender o algoritmo PID para poder tentar aplicar.
Seguindo essa aproximação http://1.bp.blogspot.com/-vSRUzfiSOuc/UTZ1yrM5DgI/AAAAAAAABYU/1qNvS-4t5O8/s640/ScreenHunter_15.bmp
Lego NXT Seguidor de linha + Controle PID

http://3.bp.blogspot.com/-2V4K8y69jh0/UTZ32MtQJII/AAAAAAAABYo/Ak3h-Proz0A/s1600/ScreenHunter_17.bmp
Lego NXT Seguidor de linha + Controle PID
, como eu calculo Kp, Ki e Kd? E esse T, é em rellação a quê?
Obrigada (desculpe a minha ignorância sobre o assunto).

O algoritmo PID é um dos tipos de algoritmo para controle em malha fechada, que compara o valor de uma leitura (valor medido=VM, por ex.: uma posição de um eixo de movimento ou a leitura de um sensor óptico - detecção de uma linha) com um valor pré estabelecido que se deseja que essa medida assuma (Set-point=SP). Para corrigir o valor sendo medido, o PID possui uma saída que atua no processo sendo controlado (p.ex: a rotação de um motor, o ângulo de uma roda de direcionamento de um robot). Essa comparação na verdade é feita através do cálculo do erro (=SP-VM), a partir do qual o algoritmo PID pode efetuar uma ou mais das suas três ações de correção do erro, proporcional ao erro, a integral do erro (a totalização do erro ao longo do tempo) e a derivada do erro (a variação do erro ao longo do tempo). Cada ação possui um ganho ajustável, daí o nome controle PID, que requer três ganhos, Kp, Ki, Kd, que multiplicam as três ações de correção citadas.

Geralmente são usados os controles P ou P+I, o P+I+D tende a instabilizar o controle, mas é muito empregado em malhas de respostas lentas (ex.: controle de temperatura).

Há vários métodos para determinar as ações de controle (Kp,Ki e Kd), entre eles, pode-se usar uma simulação matemática do processo, ou a partir de um teste de identificação do processo (teste ao impulso) - que visa determinar a função de transferência ou pelo método de tentativa e erro.  Pelo método de tentativa e erro (mais expedito), ajusta-se Kp inicialmente a vai se aplicando degraus ao SP até que o sistema comece a oscilar, em seguida baixa-se Kp "um pouco" ajusta-se (aumenta-se aos poucos) Ki de modo a reduzir o erro de regime permanente. Finalmente, ajusta-se Kd se o processo for muito lento, mas isso requer cuidados para não tornar o sistema instável.

Já sintonizei várias malhas de controle pelo método de tentativa e erro. Há muitos outros métodos e alguns equipamentos possuem uma sintonia automática (auto-tunning), que às vezes "faz besteira".

O grande problema do PID é que é um algoritmo linear, ou seja, o mesmo é linear e também assume que o sistema em controle é linear. Na grande maioria dos problemas da vida real isso não é verdade, ou seja, o Kp ideal deveria ser adaptativo, dependendo do SP que se quer operar. Na prática, se assume que o processo é linear numa pequena excursão da VM, aí o PID é usado.

Mas como diz a propaganda, use com moderação...

O valor de T é uma constante, sendo o intervalo de execução (em milissegundos, segundos, ...) do seu programa controlador PID. O T serve basicamente para definir uma constante para as ações de integração (Ki) e de derivação (Kd), que dependem do tempo. Pode assumir o valor 1 em seu programa. Eu já escrevi e implementei diversos algoritmos PID.
« Última modificação: 28 de Agosto de 2013, 18:29 por minilathe »

Offline minilathe

  • How to ask questions - The Smart Way...
  • Moderadores
  • CNCMASTER
  • ******
  • Posts: 4295
  • Sexo: Masculino
  • http://science-lakes.com/article43-html.html
  • Cidade - UF: Rio de Janeiro - RJ
  • Nome:: Gil Pinheiro
  • Profissão: Engenheiro e professor universitário
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #5 Online: 28 de Agosto de 2013, 18:39 »
Algumas correções:


Offline Tita

  • Novato
  • *
  • Posts: 11
  • Sexo: Feminino
  • GUIA CNC
  • Cidade - UF: Belo Horizonte
  • Nome:: Ana
  • Profissão: Estudante
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #6 Online: 30 de Agosto de 2013, 22:57 »
Obrigada a todos que me responderam. Foi muito útil.
Todo feedback é bem-vindo!
Meu programa em C ficou assim (não sei se funciona, pois não estou com o NXT em mãos):

float kp=2,ki=0.07,kd=5;
int max=0,min=9999,integral=0,lasterror=0,pp=50,count=1,offset,turn,ls,error,derivate,power,p_A,p_C;
int calibration(int &a, int &b)
{
    int aux;
    OnFwd(OUT_A,25);
    OnRev(OUT_C,25);
    while(MotorRotationCount(OUT_A)<=120)
    {
         aux=Sensor(IN_1);
         if(aux<min)
         {
             min=aux;
         }
         if(aux>max)
         {
             max=aux;
         }
    }
   
}
task main()
{
     SetSensor(IN_1, SENSOR_COLORRED);
     SetSensorMode(IN_1, IN_MODE_PCTFULLSCAL E);
     ResetSensor(IN_1);
     SetSensor(IN_2, SENSOR_ROTATION);
     calibration(max,min);
     offset=(max+min)/2;
     OnFwd(OUT_C,25);
     OnRev(OUT_A,25);
     while(true)
     {
         ls=Sensor(IN_1);
         if((ls>max||ls<min)&&count==1)
         {
             OnFwd(OUT_A,25);
             OnRev(OUT_C,25);
             count=0;
         }
         else if((ls>max||ls<min)&&count==0)
         {
              Off(OUT_AB);
         }
         error=ls-offset;
         integral=integral+error;
         derivate=error-lasterror;
         turn=kp*error+ki*integral+kd*derivate;
         p_A=pp+turn;
         p_C=pp-turn;
         OnFwd(OUT_A,p_A);
         OnRev(OUT_C,p_C);
         lasterror=error;

     }
}

Offline Tita

  • Novato
  • *
  • Posts: 11
  • Sexo: Feminino
  • GUIA CNC
  • Cidade - UF: Belo Horizonte
  • Nome:: Ana
  • Profissão: Estudante
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #7 Online: 30 de Agosto de 2013, 23:03 »
Ah!Cometi um erro. Em vez de int calibration... é void calibration... já que estou passando os parâmetros por referência, acredito...

Offline minilathe

  • How to ask questions - The Smart Way...
  • Moderadores
  • CNCMASTER
  • ******
  • Posts: 4295
  • Sexo: Masculino
  • http://science-lakes.com/article43-html.html
  • Cidade - UF: Rio de Janeiro - RJ
  • Nome:: Gil Pinheiro
  • Profissão: Engenheiro e professor universitário
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #8 Online: 30 de Agosto de 2013, 23:48 »
Apesar de estar em linguagem C, que eu conheço, é difícil opinar sem saber o que fazem várias funções listadas no programa e que são específicas do NXT, tais como:
OnFwd(OUT_A,25);
OnRev(OUT_C,25);
....
Parecem comandos de motores, mas entre tentar imaginar o que é e sugerir algo errado, "a distância" é pequena.

O que posso sugerir é reduzir o ganho derivativo para zero inicialmente (kd=0) e depois, aumentado-se aos poucos, avaliar se é necessário e se influencia instabilizando o controle.

O trecho a seguir parece ser o loop de controle, pois lê o sensor, calcula o PID e atualiza as saídas continuamente:

     while(true)
     {
         ls=Sensor(IN_1);
         if((ls>max||ls<min)&&count==1)
         {
             OnFwd(OUT_A,25);
             OnRev(OUT_C,25);
             count=0;
         }
         else if((ls>max||ls<min)&&count==0)
         {
              Off(OUT_AB);
         }
         error=ls-offset;
         integral=integral+error;

         if( integral > IMAX )
             integral = IMAX;
         if( integral < IMIN )
             integral = IMIN;
 
         derivate=error-lasterror;
         turn=kp*error+ki*integral+kd*derivate;
         p_A=pp+turn;
         p_C=pp-turn;
         OnFwd(OUT_A,p_A);
         OnRev(OUT_C,p_C);
         lasterror=error;

     }


O trecho em vermelho que estou sugerindo é necessário para que a ação integral não sature o controle PID e o mesmo pare de funcionar, chama-se ação anti-reset windup, ver figura anexada e referências (http://en.wikipedia.org/wiki/Integral_windup e http://www.controlguru.com/2008/021008.html). IMAX e MIN são duas constantes, que podem ser iguais a +100.0 e -100.0 , ou então +100 e 0 (mais provável, pois "integral" é inicializada com 0) respectivamente. Vai depender de como o motor é controlado (um sinal de 0 a 100 ou de -100 a 100, .... de 0.00 a +1.00, ....).
« Última modificação: 31 de Agosto de 2013, 00:20 por minilathe »

Offline minilathe

  • How to ask questions - The Smart Way...
  • Moderadores
  • CNCMASTER
  • ******
  • Posts: 4295
  • Sexo: Masculino
  • http://science-lakes.com/article43-html.html
  • Cidade - UF: Rio de Janeiro - RJ
  • Nome:: Gil Pinheiro
  • Profissão: Engenheiro e professor universitário
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #9 Online: 31 de Agosto de 2013, 07:52 »
Ah!Cometi um erro. Em vez de int calibration... é void calibration... já que estou passando os parâmetros por referência, acredito...

Os parâmetros da função "calibration" estão sendo passados por referência (o & antes de cada parâmetro), isso não tem nada a ver com a declaração da função (int, void, float, ...), que define o valor que a função retorna. No exemplo a seguir, c recebe o valor de retorno (comando "return") de "calibration":

void main()
{
    int a, b, c;

    a = calibration (&b, &c);
}

int calibration(int &a, int &b)
{
    int aux;
    OnFwd(OUT_A,25);
    OnRev(OUT_C,25);
    while(MotorRotationCount(OUT_A)<=120)
    {
         aux=Sensor(IN_1);
         if(aux<min)
         {
             min=aux;
         }
         if(aux>max)
         {
             max=aux;
         }
    }
    return( 0 );
}


Apesar de eu ter posto um "return", na sua função "calibration", a mesma pode ser do tipo "void", pois não retorna nada, além disso o tipo "void" economiza memória da pilha e ciclos de processamento. Aliás, você pode ter um erro se declarar a função como "void" e colocar um comando "return" dentro da função. Também está errado declará-la como "int" e não colocar um "return".
« Última modificação: 31 de Agosto de 2013, 07:56 por minilathe »

Offline Tita

  • Novato
  • *
  • Posts: 11
  • Sexo: Feminino
  • GUIA CNC
  • Cidade - UF: Belo Horizonte
  • Nome:: Ana
  • Profissão: Estudante
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #10 Online: 31 de Agosto de 2013, 13:53 »
Prof. Gil Pinheiro, muitíssimo obrigada pelo feedback. (Eu não conseguia entender nada sobre PID...)
Comentei meu programa, se alguém identificar alguma incoerência, por favor me avise!

const float KP=2,KI=0,KD=0;
const int IMAX=100,IMIN=0;
int max=0,min=9999,integral=0,lasterror=0,pp=50,count=1,offset,turn,ls,error,derivate,power,p_A,p_C;
void calibration(int &a, int &b)
{
    int aux;
    OnFwd(OUT_A,25);//manda o motor ir para frente com 25% da potência
    OnRev(OUT_C,25);//manda o motor ir para trás
    //MotorRotationCount conta quantos graus o motor girou
    while(MotorRotationCount(OUT_A)<=120)
    {
         aux=Sensor(IN_1);
         if(aux<min) min=aux;
         if(aux>max) max=aux;
    }
   
}
task main()
{
     SetSensor(IN_1, SENSOR_COLORRED);//inicializa o sensor de cor
     SetSensorMode(IN_1, IN_MODE_PCTFULLSCAL E);
     //coloca em modo de aquisição em porcentagem (0 a 100) em vez de 0 a 1023
     ResetSensor(IN_1);//configura como especificado acima
     SetSensor(IN_2, SENSOR_ROTATION);
     ResetSensor(IN_2);
     ClearSensor(IN_2);//limpa o contador de rotações
     calibration(max,min);//calibra o sensor de cor
     offset=(max+min)/2;//setpoint
     OnFwd(OUT_C,25);
     OnRev(OUT_A,25);
     while(true)
     {
         ls=Sensor(IN_1);//ler sensor de cor
         /*esse if serve para identificar quando sensor captar valores diferentes
         de cor em ralção ao valor váximo e mínimo medido na calibração. O count serve para
         saber se o robô chegou no final da linha e com isso deve fazer o giro e retornar.
         Quando o programa identificar que count é 0, significa que já fez o trajeto
         de ida e volta sobre a linha, então o robô para*/
         if((ls>max+3||ls<min-3)&&count==1)
         {
             OnFwd(OUT_A,25);
             OnRev(OUT_C,25);
             count=0;
         }
         /*esse +-3 é para garantir que o robô não vai girar
         caso ocorra uma variação do valor máximo ao longo do percurso,
         já que a cor branca pode apresentar "diferentes tonalidades"
         de uma região para outra da pista*/
         else if((ls>max||ls<min)&&count==0)
         {
              Off(OUT_AB);
         }
         error=ls-offset;
         integral=integral+error;
         derivate=error-lasterror;
         turn=KP*error+KI*integral+KD*derivate;
         if( integral > IMAX ) integral = IMAX;//agora com ação anti-reset windup
         if( integral < IMIN ) integral = IMIN;
         p_A=pp+turn;
         p_C=pp-turn;
         OnFwd(OUT_A,p_A);
         OnRev(OUT_C,p_C);
         lasterror=error;

     }
}

Offline minilathe

  • How to ask questions - The Smart Way...
  • Moderadores
  • CNCMASTER
  • ******
  • Posts: 4295
  • Sexo: Masculino
  • http://science-lakes.com/article43-html.html
  • Cidade - UF: Rio de Janeiro - RJ
  • Nome:: Gil Pinheiro
  • Profissão: Engenheiro e professor universitário
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #11 Online: 31 de Agosto de 2013, 23:10 »
A ação anti-reset windup deve ser feita logo após o cálculo da ação integral, para não saturar o cálculo da ação de correção do PID, conforme segue:

     while(true)
     {
         ls=Sensor(IN_1);//ler sensor de cor
         /*esse if serve para identificar quando sensor captar valores diferentes
         de cor em ralção ao valor váximo e mínimo medido na calibração. O count serve para
         saber se o robô chegou no final da linha e com isso deve fazer o giro e retornar.
         Quando o programa identificar que count é 0, significa que já fez o trajeto
         de ida e volta sobre a linha, então o robô para*/
         if((ls>max+3||ls<min-3)&&count==1)
         {
             OnFwd(OUT_A,25);
             OnRev(OUT_C,25);
             count=0;
         }
         /*esse +-3 é para garantir que o robô não vai girar
         caso ocorra uma variação do valor máximo ao longo do percurso,
         já que a cor branca pode apresentar "diferentes tonalidades"
         de uma região para outra da pista*/
         else if((ls>max||ls<min)&&count==0)
         {
              Off(OUT_AB);
         }
         error=ls-offset;
         integral=integral+error;
         if( integral > IMAX ) integral = IMAX;//agora com ação anti-reset windup
         if( integral < IMIN ) integral = IMIN;
         derivate=error-lasterror;
         turn=KP*error+KI*integral+KD*derivate;
         p_A=pp+turn;
         p_C=pp-turn;
         OnFwd(OUT_A,p_A);
         OnRev(OUT_C,p_C);
         lasterror=error;

     }


Você usou alguma referência ou outro programa para construir este programa?

Há algumas coisinhas que seriam desejáveis, mas podem ser deixadas para depois, tal como calcular o PID com variáveis do tipo float ao invés de int.  Como é um controle simples (seguidor de linha), o controle não é crítico, poderia ser até ON-OFF. Mas, se você já testou assim ou tem uma referência funcional anterior pode deixar como está ou melhorar depois de testar.

Offline Tita

  • Novato
  • *
  • Posts: 11
  • Sexo: Feminino
  • GUIA CNC
  • Cidade - UF: Belo Horizonte
  • Nome:: Ana
  • Profissão: Estudante
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #12 Online: 01 de Setembro de 2013, 12:59 »
Citar
Você usou alguma referência ou outro programa para construir este programa?

Há algumas coisinhas que seriam desejáveis, mas podem ser deixadas para depois, tal como calcular o PID com variáveis do tipo float ao invés de int.  Como é um controle simples (seguidor de linha), o controle não é crítico, poderia ser até ON-OFF. Mas, se você já testou assim ou tem uma referência funcional anterior pode deixar como está ou melhorar depois de testar.

Como a fórmula estava muito complicada eu fiz uma aproximação e fiz o programa em NXC.
Não testei e também não tenho uma referência funcional.  Estou desprovida do Lego. Não faço ideia se o programa vai funcionar.
Vou fazer uma prova prática que envolve o Lego e programação em NXC, não sei ainda que tarefa vou ter que fazer (isso só saberei na hora do teste, cujo resultado terá uma importância enorme). Mas sei que entender a aplicação do PID num programa tal como esse que venho tentando pode ser crucial.
E quais seriam essas coisinhas desejáveis, além de calcular o PID com variáveis do tipo float ao invés de int?

Offline minilathe

  • How to ask questions - The Smart Way...
  • Moderadores
  • CNCMASTER
  • ******
  • Posts: 4295
  • Sexo: Masculino
  • http://science-lakes.com/article43-html.html
  • Cidade - UF: Rio de Janeiro - RJ
  • Nome:: Gil Pinheiro
  • Profissão: Engenheiro e professor universitário
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #13 Online: 01 de Setembro de 2013, 15:47 »
Uma coisa que ajudaria muito seria um simulador do NXT, onde o software pudesse ser testado e corrigido antes de ser carregado. Para análise, a inspeção as variáveis em tempo de execução. Um aspecto que eu não vi em seu programa é a definição do intervalo de execução, o "famoso" T que aparece nos algoritmos PID da literatura. Isso é essencial para que o PID funcione corretamente. Não se trata de definir esse T em sua rotina, mas efetivamente temporizá-la em T milissegundos.

Offline Tita

  • Novato
  • *
  • Posts: 11
  • Sexo: Feminino
  • GUIA CNC
  • Cidade - UF: Belo Horizonte
  • Nome:: Ana
  • Profissão: Estudante
Re:Lego NXT Seguidor de linha + Controle PID
« Resposta #14 Online: 02 de Setembro de 2013, 22:53 »
Bom, agora só falta testar, acho...

float kp,ki,kd,integral,derivada;
#define imax 100
#define imin 0
#define val 50
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 tf=0,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);
           ti=CurrentTick();
           erro=luz-offset;
           integral+=erro;
           if(integral>imax) integral=imax;
           if(integral<imin) integral=imin;
           derivada=erro-ultimoerro;
           pid=kp*erro+ki*integral*tf+kd*derivada/tf;
           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;
           tf=((CurrentTick()-ti)/1000);
     }
}


 

/** * */