Agora que já temos o valor de correção baseado no controle PD, vamos para a última etapa necessária pro nosso robô seguir linha! É chegada a hora de fazer os motores funcionarem. Se lembra de como trabalhamos com motores e ponte H e até mesmo fizemos um projeto com uma ponte H controlando um par de rodas? Vamos utilizar os conhecimentos adquiridos naquela etapa.
Declare as seguintes variáveis, tendo em vista a ponte H que está sendo utilizada neste projeto:
int motorEsq=7, motorDir=8, pwmDir=5, pwmEsq=6; // variáveis para controle de estado na ponte H
int velIn=75,velDir=0,velEsq=0; // variáveis para controle de velocidade dos motores
int velMax=180; // delimita a velocidade máxima
Agora, vamos finalmente utilizar o void loop(). Chame as funções que criamos nos tópicos anteriores:
void loop() {
normaliza();
posiciona();
pid();
}
Para calcularmos a velocidade necessária a cada motor, deveremos utilizar um valor inicial em cada motor e atuar com o sinal de correção à medida em que se fizer necessário. Note que deveremos, neste modelo, adicionar o sinal de correção em um lado e subtrair do outro, de modo a permitir a redução e o aumento da velocidade em cada motor isoladamente.
Em termos práticos, ficamos com:
velEsq = velIn - valPid; // calcula velocidade do motor da esquerda
velDir = velIn + valPid; // calcula velocidade do motor da direita
Como não podemos passar valores negativos e nem valores acima de 255, deveremos fazer essas correções manualmente:
if(velDir>velMax) velDir=velMax;
if(velEsq>velMax) velEsq=velMax;
if(velDir<0) velDir*=-1;
if(velEsq<0) velEsq*=-1;
No nosso caso, a velocidade máxima será 180, por motivos de estabilidade.
Finalmente, podemos passar os valores para os nossos motores e ver como ele irá se comportar!
Vamos utilizar o analogWrite() para passar as velocidades dos motores:
analogWrite(pwmEsq, velEsq);
analogWrite(pwmDir, velDir);
Portanto, o nosso void loop() ficou da seguinte forma:
void loop() {
normaliza();
posiciona();
pid();
velEsq = velIn - valPid;
velDir = velIn + valPid;
if(velDir>velMax) velDir=velMax;
if(velEsq>velMax) velEsq=velMax;
if(velDir<0) velDir*=-1;
if(velEsq<0) velEsq*=-1;
analogWrite(pwmEsq, velEsq);
analogWrite(pwmDir, velDir);
}
Este é o nosso código até então:
#include <Wire.h>
unsigned long tempo;
#define uchar unsigned char
uchar t = 0;
uchar data[16];
int i=0, j=0, aux=0;
float Min[6], Max[6];
int norm[6], pos=0;
int ledIndicador=12;
float somaPesos=0, soma=0;
int setP=2500, erro=0;
int I=0, D=0, P=0;
float valPid=0;
float kp=0.062, ki=0.00001, kd=0.32;
int ultErro=0;
int motorEsq=7, motorDir=8, pwmDir=5, pwmEsq=6;
int velIn=75,velDir=0,velEsq=0;
int velMax=180;
int cor_linha = 1;
void lerDados() {
Wire.requestFrom(9, 16);
while (Wire.available()) {
data[t] = Wire.read();
if (t < 15) t++;
else t = 0;
}
}
void setup() {
pinMode(ledIndicador, OUTPUT);
Wire.begin();
Serial.begin(9600);
digitalWrite(ledIndicador, HIGH);
calibra();
digitalWrite(ledIndicador, LOW);
}
void calibra() {
lerDados();
for(i=1;i<=6;i++){
Min[i-1]=float(data[i*2]);
Max[i-1]=float(data[i*2]);
}
while(tempo < 10000) {
tempo = millis();
lerDados();
for(i=1;i<=6;i++){
if (Min[i-1]>float(data[i*2])) Min[i-1]=float(data[i*2]);
if (Max[i-1]<float(data[i*2])) Max[i-1]=float(data[i*2]);
}
}
}
void normaliza() {
lerDados();
for(i=0;i<6;i++) {
norm[i] = int(((float(data[i]) - Min[i]) / (Max[i] - Min[i]))*1000);
}
for(i=0;i<6;i++){
if(norm[i] < 380) norm[i] = 0;
if(norm[i] > 800) norm[i] = 1000;
}
}
void posiciona() {
somaPesos = float((norm[0]*0)+(norm[1]*1)+(norm[2]*2)+(norm[3]*3)+(norm[4]*4)+(norm[5]*5));
soma = float(norm[0]+norm[1]+norm[2]+norm[3]+norm[4]+norm[5]);
if(somaPesos <= 0 || soma <= 0) pos = 0;
else pos = int((somaPesos/soma)*1000);
}
void pid() {
erro = setP - pos;
valPid = (kp*erro) + (kd*(erro-ultErro)) + (ki*(I += erro));
ultErro = erro;
digitalWrite(motorEsq, LOW);
digitalWrite(motorDir, HIGH);
if((valPid > 110)||(valPid < -110)){
if(valPid > 0){
digitalWrite(motorEsq, LOW);
digitalWrite(motorDir, LOW);
}
else if(valPid < 0){
digitalWrite(motorEsq, HIGH);
digitalWrite(motorDir, HIGH);
}
} else{
digitalWrite(motorEsq, LOW);
digitalWrite(motorDir, HIGH);
}
}
void loop() {
normaliza();
posiciona();
pid();
velEsq = velIn - valPid;
velDir = velIn + valPid;
if(velDir>velMax) velDir=velMax;
if(velEsq>velMax) velEsq=velMax;
if(velDir<0) velDir*=-1;
if(velEsq<0) velEsq*=-1;
analogWrite(pwmEsq, velEsq);
analogWrite(pwmDir, velDir);
}
Veja só os resultados deste código!
Laboratório de Robótica Inclusiva 
2 comentários em “Finalização do robô”