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!