PID Aplicado no Controle da Posição de um Robô

Introdução

Há diversas aplicações nas quais o método PID de controle pode ser aplicado. Aqui no blog, por exemplo, temos já dois posts que abordam o assunto: Drone MDF Maker com Arduino – Configuração e Fonte regulável com Arduino e Transistor. Nesta publicação, usaremos tal algoritmo para controlar, em uma dimensão, a posição de um robô montado no chassi 4WD e controlado por um Arduino Nano.

Entrada e Saída

Há diversas formas de se verificar a posição de um objeto. Para este projeto, utilizaremos um sensor ultrassônico HC-SR04, sobre o qual o post Sensor Ultrassônico HC-SR04 com Arduino fala. Assim, a medição feita pelo sensor de nosso sistema é de tempo. No entanto, nosso setpoint será definido em metros, uma medida de distância. A entrada para o algoritmo PID, então, deve ser em distância também. Para isso, será feita a conversão comum de quando se utiliza o citado sensor, multiplicando o tempo pela velocidade do som no meio e dividindo por dois, fazendo com que o método controlador receba um valor de distancia, que será subtraído do setpoint para alimentar o PID. A posição do robô é controlada por seus motores que, por sua vez, têm sua velocidade determinada pela tensão média neles aplicada. A saída do algoritmo, então, será em um valor digital que, convertido para tensão por meio do duty-cycle aplicado nas saídas PWM do Arduino, controlará a ponte H dupla que alimenta os motores que, por sua vez, definem a saída do sistema em uma distância.

Componentes e Montagem

Materiais necessários para o projeto PID Aplicado no Controle da Posição de um Robô

• Kit Chassi 4WD Robô para Arduino
• Módulo Regulador de Tensão Step Down Buck DC-DC Mini 360 3A
• Ponte H Dupla L298N
• Módulo Sensor de Distância Ultrassônico HC-SR04
• Nano v3.0 + Cabo USB para Arduino
• Protoboard 400 Pontos
• Bateria

A montagem do kit 4WD é simples e já foi abordada no post Guia de montagem da Plataforma robótica 4WD. As conexões eletrônicas devem seguir o diagrama abaixo:

O robô de nosso projeto, montado, pode ser visto na imagem a seguir.

O Código

Utilizaremos, em nosso programa, a biblioteca PID_v2.h, criada por Brett Beauregard e mantida por Max Ignatenko. Ela pode ser baixada diretamente pela Arduino IDE e sua documentação pode ser encontrada aqui.

Inicialmente, é adicionada a biblioteca e são definidas algumas macros:

#include <PID_v2.h>
#define TRIGGER   2 //pino de trigger do sensor
#define ECHO      3 //pino de echo do sensor
#define VEL_SOMu  0.000340 //velocidade do som para calculos
#define M1F  5 //pino de tensão direta no motor 1
#define M2F  6 //pino de tensão direta no motor 2
#define M1R  9 //pino de tensão reversa no motor 1
#define M2R  10 //pino de tensão reversa no motor 2
#define TAM_FILTRO  10 //tamanho do vetor de filtro

A seguir, são declaradas as variáveis e instanciados os objetos:

//     tensão aplicada nos  tensão aplicada nos  distancia  distancia
//     motores para avanço  motores para ré      calculada  desejada
double tensao_saidaf = 100, tensao_saidar = 100, distancia, setpoint = 0.1,

//coeficientes: proporcional integral   derivativo  vetor de medições   soma p/ calculo  
                Kp = 3,      Ki = 0.75, Kd = 3,     filtro[TAM_FILTRO], soma; //do filtro

char parametro;         //auxiliar para
long cont = 0;          //comunicação serial

//objeto que guardará as informações do PID de avanço
PID pid(&distancia, &tensao_saidaf, &setpoint, Kp, Ki, Kd, DIRECT);

//objeto que guardará as informações do PID de ré
PID pidr(&distancia, &tensao_saidar, &setpoint, Kp, Ki, Kd, REVERSE);

A função de setup define os pinos como entrada ou saída e inicializa os PID’s:

void setup() {
  pinMode(TRIGGER, OUTPUT);
  digitalWrite(TRIGGER, LOW);
  pinMode(ECHO, INPUT);
  pinMode(M1F, OUTPUT);  
  pinMode(M2F, OUTPUT);
  pinMode(M1R, OUTPUT);  
  pinMode(M2R, OUTPUT);
  Serial.begin(115200);
  
  pid.SetMode(AUTOMATIC);   //inicia
  pidr.SetMode(AUTOMATIC);  //os PID
}

Iniciamos o loop com a verificação da serial e, caso os critérios sejam atendidos, a atualização das variáveis:

if(Serial.available() > 0) parametro = Serial.read();       //guarda o primeiro byte da serial
                                                            //em "parametro".
if(parametro == 'p')        Kp = Serial.parseFloat();       //de acordo com o caracter recebido,
else  if(parametro == 'i')  Ki = Serial.parseFloat();       //atualiza as variáveis dos
else  if(parametro == 'd')  Kd = Serial.parseFloat();       //coeficientes ou do setpoint
else  if(parametro == 's')  setpoint = Serial.parseFloat(); //do sistema

Então, são atualizados os coeficientes dos algoritmos:

pid.SetTunings(Kp, Ki, Kd);   //atualiza os
pidr.SetTunings(Kp, Ki, Kd);  //coeficientes

Em seguida, o programa passa a leitura do sensor por um filtro digital e, então, calcula a distancia a ser registrada na variável:

for(int i = TAM_FILTRO - 2; i >= 0 ; i --)                    //passa cada item do filtro para a
  filtro[i] = filtro[i + 1];                                  //posição anterior, apagando o mais antigo
                                                              //
digitalWrite(TRIGGER, HIGH);                                  //envia uma onda ultrassônica
delayMicroseconds(10);                                        //e, quando recebe o pulso de ECHO
digitalWrite(TRIGGER, LOW);                                   //armazena a distancia calculada na ultima
filtro[TAM_FILTRO - 1] = pulseIn(ECHO, HIGH) * VEL_SOMu / 2;  //posição do vetor de filtro
                                                              //
soma = 0;                                                     //então, a média do vetor é
for(int i = 0; i < TAM_FILTRO; i ++)                          //calculada e enviada para a
  soma += filtro[i];                                          //variável distancia
                                                              //
distancia = soma / TAM_FILTRO;                                //

Com as informações atualizadas, os PID’s calculam a nova saída:

pid.Compute();  //calcula os novos valores de tensao_saida, com base nos coeficientes e na
pidr.Compute(); //distancia calculada. saídas, entradas e setpoint estão ligados ao algoritmo
                //pelos ponteiros para seus endereços, passados na inicialização da instancia.
                //já os coeficientes foram atualizados no método SetTunings

Daí, a cada mil iterações, as informações do sistema são enviadas via serial:

if(cont > 1000) {                                                     //
  Serial.println("Kp    Ki    Kd    set   distancia   tensao_saida"); //
  Serial.print(Kp); Serial.print("  ");                               //a cada 1000 ciclos da void loop
  Serial.print(Ki); Serial.print("  ");                               //as informações sobre o sistema
  Serial.print(Kd); Serial.print("  ");                               //são enviadas ao monitor serial.
  Serial.print(setpoint); Serial.print("  ");                         //aqui, o objetivo é somente
  Serial.print(distancia); Serial.print("        ");                  //acompanhar os dados do sistema
  Serial.println(tensao_saidaf);                                      //
  cont = 0;                                                           //
} cont ++;

Por ultimo, com base na posição do robô, é aplicada a tensão calculada por um dos PID’s nos motores:

if(distancia < setpoint - 0.02) {   //se a distancia for menor do que dois
  analogWrite(M1F, tensao_saidaf);  //centimetros a menos do que o setpoint,
  analogWrite(M2F, tensao_saidaf);  //os motores de avanço são acionados
  analogWrite(M1R, 0);              //com a tensão calculada pelo PID
  analogWrite(M2R, 0);              //
}
else if(distancia < setpoint + 0.02 &&  //se a distancia estiver
        distancia > setpoint - 0.02) {  //a menos de 2 centimetros
  analogWrite(M1R, 0);                  //do setpoint, os motores
  analogWrite(M2R, 0);                  //freiam
  analogWrite(M1F, 0);                  //
  analogWrite(M2F, 0);                  //
}
else {
  analogWrite(M1R, tensao_saidar);  //se a distancia for maior do que dois
  analogWrite(M2R, tensao_saidar);  //centimetros a mais do que o setpoint,
  analogWrite(M1F, 0);              //os motores de ré são acionados
  analogWrite(M2F, 0);              //com a tensão calculada pelo PID
}

A seguir, o código completo:

#include <PID_v2.h>
#define TRIGGER   2 //pino de trigger do sensor
#define ECHO      3 //pino de echo do sensor
#define VEL_SOMu  0.000340 //velocidade do som para calculos
#define M1F  5 //pino de tensão direta no motor 1
#define M2F  6 //pino de tensão direta no motor 2
#define M1R  9 //pino de tensão reversa no motor 1
#define M2R  10 //pino de tensão reversa no motor 2
#define TAM_FILTRO  10 //tamanho do vetor de filtro

//     tensão aplicada nos  tensão aplicada nos  distancia  distancia
//     motores para avanço  motores para ré      calculada  desejada
double tensao_saidaf = 100, tensao_saidar = 100, distancia, setpoint = 0.1,

//coeficientes: proporcional integral   derivativo  vetor de medições   soma p/ calculo  
                Kp = 3,      Ki = 0.75, Kd = 3,     filtro[TAM_FILTRO], soma; //do filtro

char parametro;         //auxiliar para
long cont = 0;          //comunicação serial

//objeto que guardará as informações do PID de avanço
PID pid(&distancia, &tensao_saidaf, &setpoint, Kp, Ki, Kd, DIRECT);

//objeto que guardará as informações do PID de ré
PID pidr(&distancia, &tensao_saidar, &setpoint, Kp, Ki, Kd, REVERSE);

void setup() {
  pinMode(TRIGGER, OUTPUT);
  digitalWrite(TRIGGER, LOW);
  pinMode(ECHO, INPUT);
  pinMode(M1F, OUTPUT);  
  pinMode(M2F, OUTPUT);
  pinMode(M1R, OUTPUT);  
  pinMode(M2R, OUTPUT);
  Serial.begin(115200);
  
  pid.SetMode(AUTOMATIC);   //inicia
  pidr.SetMode(AUTOMATIC);  //os PID
}

void loop() {

  if(Serial.available() > 0) parametro = Serial.read();       //guarda o primeiro byte da serial
                                                              //em "parametro".
  if(parametro == 'p')        Kp = Serial.parseFloat();       //de acordo com o caracter recebido,
  else  if(parametro == 'i')  Ki = Serial.parseFloat();       //atualiza as variáveis dos
  else  if(parametro == 'd')  Kd = Serial.parseFloat();       //coeficientes ou do setpoint
  else  if(parametro == 's')  setpoint = Serial.parseFloat(); //do sistema
  
  pid.SetTunings(Kp, Ki, Kd);   //atualiza os
  pidr.SetTunings(Kp, Ki, Kd);  //coeficientes


  for(int i = TAM_FILTRO - 2; i >= 0 ; i --)                    //passa cada item do filtro para a
    filtro[i] = filtro[i + 1];                                  //posição anterior, apagando o mais antigo
                                                                //
  digitalWrite(TRIGGER, HIGH);                                  //envia uma onda ultrassônica
  delayMicroseconds(10);                                        //e, quando recebe o pulso de ECHO
  digitalWrite(TRIGGER, LOW);                                   //armazena a distancia calculada na ultima
  filtro[TAM_FILTRO - 1] = pulseIn(ECHO, HIGH) * VEL_SOMu / 2;  //posição do vetor de filtro
                                                                //
  soma = 0;                                                     //então, a média do vetor é
  for(int i = 0; i < TAM_FILTRO; i ++)                          //calculada e enviada para a
    soma += filtro[i];                                          //variável distancia
                                                                //
  distancia = soma / TAM_FILTRO;                                //
  
  
  pid.Compute();  //calcula os novos valores de tensao_saida, com base nos coeficientes e na
  pidr.Compute(); //distancia calculada. saídas, entradas e setpoint estão ligados ao algoritmo
                  //pelos ponteiros para seus endereços, passados na inicialização da instancia.
                  //já os coeficientes foram atualizados no método SetTunings

  if(cont > 1000) {                                                     //
    Serial.println("Kp    Ki    Kd    set   distancia   tensao_saida"); //
    Serial.print(Kp); Serial.print("  ");                               //a cada 1000 ciclos da void loop
    Serial.print(Ki); Serial.print("  ");                               //as informações sobre o sistema
    Serial.print(Kd); Serial.print("  ");                               //são enviadas ao monitor serial.
    Serial.print(setpoint); Serial.print("  ");                         //aqui, o objetivo é somente
    Serial.print(distancia); Serial.print("        ");                  //acompanhar os dados do sistema
    Serial.println(tensao_saidaf);                                      //
    cont = 0;                                                           //
  } cont ++;                                                            //

  if(distancia < setpoint - 0.02) {   //se a distancia for menor do que dois
    analogWrite(M1F, tensao_saidaf);  //centimetros a menos do que o setpoint,
    analogWrite(M2F, tensao_saidaf);  //os motores de avanço são acionados
    analogWrite(M1R, 0);              //com a tensão calculada pelo PID
    analogWrite(M2R, 0);              //
  }
  else if(distancia < setpoint + 0.02 &&  //se a distancia estiver
          distancia > setpoint - 0.02) {  //a menos de 2 centimetros
    analogWrite(M1R, 0);                  //do setpoint, os motores
    analogWrite(M2R, 0);                  //freiam
    analogWrite(M1F, 0);                  //
    analogWrite(M2F, 0);                  //
  }
  else {
    analogWrite(M1R, tensao_saidar);  //se a distancia for maior do que dois
    analogWrite(M2R, tensao_saidar);  //centimetros a mais do que o setpoint,
    analogWrite(M1F, 0);              //os motores de ré são acionados
    analogWrite(M2F, 0);              //com a tensão calculada pelo PID
  }
}

Funcionamento

A seguir, um vídeo que mostra o funcionamento do protótipo:

É importante reparar como o PID garante um funcionamento suave mas com respostas ágeis às mudanças da entrada.

Por Fim

Para este projeto, com um robô leve e motores lentos, o PID é de fácil ajuste e uma grande faixa de valores atende o funcionamento desejado. Para aplicações mais complexas, com mais variáveis, mais resistências a serem vencidas ou atuadores mais potentes, é preciso dar uma maior atenção ao processo de escolha dos coeficientes. Ainda assim, todo projeto mais complexo precisa passar por modelos mais simples. Portanto, espero que o post seja de utilidade para dar mais um passo na compreensão e na aplicação deste modelo de controle. Obrigado pela leitura!

Conheça a Metodologia Eletrogate e ofereça aulas de robótica em sua escola!

Sobre o Autor

Eduardo Henrique Formado técnico em mecatrônica no CEFET-MG, atualmente estuda Engenharia de Controle e Automação na UFMG.