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.
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.
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.
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
}
}
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.
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!
|
A Eletrogate é uma loja virtual de componentes eletrônicos do Brasil e possui diversos produtos relacionados à Arduino, Automação, Robótica e Eletrônica em geral.
Conheça a Metodologia Eletrogate e Lecione um Curso de Robótica nas Escolas da sua Região!
Rua Rio de Janeiro, 441 - Sala 1301
Centro - Belo Horizonte/MG
CEP 30160-041
*Não temos atendimento físico
ANWAR SLEIMAN HACHOUCHE - ME
CNPJ: 18.917.521/0001-73
