Galera, boa tarde!
Tô com um tcc, um pêndulo invertido.
Tenho uma ponte h L298N, que controla dois servo motores por meio da variação angular obtida por um giroscópio MPU6050. Não consigo fazer o sistema ficar em equilibrío. Podem me ajudar?
Código:
#include<Wire.h>
#include <PID_v1.h>
const int MPU=0x68;
int16_t acelY;
int16_t giroY;
int pinoMotorEsquerdoA = 3;
int pinoMotorEsquerdoB = 5;
int pinoMotorDireitoA = 9;
int pinoMotorDireitoB = 10;
//Pinos END
//Parâmetros START
int quantidadeLeiturasMedia = 3;
int intervaloEntreLeituras = 0;
int tolerancia = 5;
//Parâmetros END
//Variáveis globais START
int zeroConsiderado = 0;
int sentidoVariacao = 1;
double giroYKILL=50000;
int pwmAplicarMotores = 0;
int pwmAnterior = 0;
int sentidoVariacaoAnterior = -1;
//Variáveis globais END
//PID
double Setpoint = 0;
double Input = 0;
double Output;
PID myPID(&Input, &Output, &Setpoint, 0.1314, 0.7152, 0.0063, DIRECT);
///
void setup()
{
Serial.begin(9600); //inicia a comunicação serial
Wire.begin(); //inicia I2C
Wire.beginTransmission(MPU); //Inicia transmissão para o endereço do MPU
Wire.write(0x6B);
pinMode(A5,INPUT);
pinMode(A4,INPUT);
//Inicializa o MPU-6050
Wire.write(0);
Wire.endTransmission(true);
pinMode(pinoMotorEsquerdoA,OUTPUT);
pinMode(pinoMotorEsquerdoB,OUTPUT);
pinMode(pinoMotorDireitoA,OUTPUT);
pinMode(pinoMotorDireitoB,OUTPUT);
//config PID
Setpoint = 0;
myPID.SetMode(AUTOMATIC);
}
int leituraMedia(int zeroConsiderado = 0) {
int valorMedio = 0;
int valorLeiturasAcumulado = 0;
int con;
for(con =0; con < quantidadeLeiturasMedia; con++) {
valorLeiturasAcumulado = valorLeiturasAcumulado + giroY;
delay(intervaloEntreLeituras);
}
valorMedio = (valorLeiturasAcumulado / quantidadeLeiturasMedia);
if (valorMedio >= zeroConsiderado) {
sentidoVariacao = 1;
} else {
sentidoVariacao = -1;
}
valorMedio = abs(valorMedio);
if (valorMedio < tolerancia) {
valorMedio = 0;
}
return valorMedio;
}
void acionaMotorEsquerdo(int sentido = 0, int valor = 0) {
digitalWrite(pinoMotorEsquerdoA, LOW);
digitalWrite(pinoMotorEsquerdoB, LOW);
if (sentido == -1){
digitalWrite(pinoMotorEsquerdoB, valor);
} else {
digitalWrite(pinoMotorEsquerdoA, valor);
}
}
void acionaMotorDireito(int sentido = 0, int valor = 255) {
digitalWrite(pinoMotorDireitoA, LOW);
digitalWrite(pinoMotorDireitoB, LOW);
if (sentido == 1){
analogWrite(pinoMotorDireitoB, valor);
} else {
analogWrite(pinoMotorDireitoA, valor);
}
}
void acionaMotores(int sentido = 0, int valor = 255) {
acionaMotorDireito(sentido, valor);
acionaMotorEsquerdo(sentido, valor);
}
void pararMotores(int sentidoVariacaoAnterior, int pwmAnterior){
int con = 0;
for ( con = pwmAnterior; con > 0; con--) {
acionaMotores(sentidoVariacaoAnterior, con);
}
}
void loop()
{
Wire.beginTransmission(MPU); //transmite
Wire.write(0x3B); // Endereço 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false); //Finaliza transmissão
Wire.requestFrom(MPU,14,true); //requisita bytes
//Armazena o valor dos sensores nas variaveis correspondentes
acelY=Wire.read()8|Wire.read(); //0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
giroY=Wire.read()8|Wire.read(); //0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
//Envia valores lidos do acelerômetro
Serial.print("\tAcel:");
Serial.print("\tY:");Serial.print(acelY/16.4);
//Envia valores lidos do giroscópio
Serial.print("\tGiro:");
Serial.print("\tY:");Serial.println(giroY/16.4);
int con = 0;
//antes de serem atualizados pela nova leitura
sentidoVariacaoAnterior = sentidoVariacao;
pwmAnterior = pwmAplicarMotores;
giroY = leituraMedia(zeroConsiderado);
if (giroY >= giroYKILL) {
pararMotores(sentidoVariacaoAnterior, pwmAnterior);
} else {
Input = giroY*-1;
myPID.Compute();
pwmAplicarMotores = Output;
if (pwmAplicarMotores < 0) {
pwmAplicarMotores = 0;
}
if (pwmAplicarMotores > 255) {
pwmAplicarMotores = 255;
}
acionaMotores(sentidoVariacao, pwmAplicarMotores);
}
}
Tags:
é tipo um robô de pendulo invertido ne?
ele chega a se mexer como que tentando manter o equilibrio mais nao consegue?
Verifique se os servo motores estao corretamente alinhados a 0 graus. os 2 devem estar na mesma posição para o calculo de posicionamento funcionar
Isso mesmo...
A questao do posicionamento dos motores, a ponte h faz..
Não tenho conseguido mantê-lo em equilibrio...
Olá,
Pelo seu relato, falta de equilíbrio, o que falta é ajustar os ganhos do PID.
Já fiz vários ajustes no PID e nada...
O motor tem se comportado de forma agressiva, que piora a situaçãors
Verifique se existe folga entre o eixo e a roda, isso prejudica o controle, no mais é por em prática a teoria...
Hehe...
Ok!
Obrigado pelas dicas!
Bem-vindo a
Laboratorio de Garagem (arduino, eletrônica, robotica, hacking)
© 2024 Criado por Marcelo Rodrigues. Ativado por