Senhores, boa tarde!

Estou criando um código para controle de um robo equilibrista (Pêndulo invertido). Não estou conseguindo mantê-lo em equilibrio. Segue o código que estou utilizando... Conseguem ver algo que possa melhorar meu "equilibrio"?

#include <PID_v1.h>
//Pinos START
int pinoMotorEsquerdoA = 3;
int pinoMotorEsquerdoB = 5;
int pinoMotorDireitoA = 9;
int pinoMotorDireitoB = 10;
int pinoY = A0;
//Pinos END
//Parâmetros START
int quantidadeLeiturasMedia = 3;
int intervaloEntreLeituras = 0;
int tolerancia = 0;
//Parâmetros END
//Variáveis globais START
int zeroConsiderado = 0;
int sentidoVariacao = 1;
double valorY = 0;
double valorYKILL = 360;
int pwmAplicarMotores = 0;
int pwmAnterior = 0;
int sentidoVariacaoAnterior = 1;
//Variáveis globais END
//PID
double Setpoint = 0;
double Input = 0;
double Output;
double kp = 3.15;
double ki = 0.1;
double kd = 0.7;
PID myPID(&Input, &Output, &Setpoint, kp, ki, kd, DIRECT);
///
int leituraMedia(int zeroConsiderado = 0) {
int valorMedio = 0;
int valorLeiturasAcumulado = 0;
int con;
for(con =0; con < quantidadeLeiturasMedia; con++) {
valorLeiturasAcumulado = valorLeiturasAcumulado + analogRead(pinoY);
delay(intervaloEntreLeituras);
}
valorMedio = (valorLeiturasAcumulado / quantidadeLeiturasMedia);
if (valorMedio >= zeroConsiderado) {
valorMedio = valorMedio - zeroConsiderado;
sentidoVariacao = 1;
} else {
valorMedio = zeroConsiderado - valorMedio;
sentidoVariacao = -1;
}
valorMedio = abs(valorMedio);
if (valorMedio < tolerancia) {
valorMedio = 0;
}
return valorMedio;
}
void acionaMotorEsquerdo(int sentido = 0, int valor = 255) {
digitalWrite(pinoMotorEsquerdoA, LOW);
digitalWrite(pinoMotorEsquerdoB, LOW);
if (sentido == 1){
analogWrite(pinoMotorEsquerdoB, valor);
} else {
analogWrite(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);
}
}
//Funções default START
void setup() {
Serial.begin(9600);
pinMode(pinoMotorEsquerdoA,OUTPUT);
pinMode(pinoMotorEsquerdoB,OUTPUT);
pinMode(pinoMotorDireitoA,OUTPUT);
pinMode(pinoMotorDireitoB,OUTPUT);
pinMode(pinoY,INPUT);
//zeroConsiderado = leituraMedia(0);
zeroConsiderado = 347;
//config PID
Setpoint = 0;
myPID.SetMode(AUTOMATIC);
/////////
}
void loop() {
int con = 0;
//antes de serem atualizados pela nova leitura
sentidoVariacaoAnterior = sentidoVariacao;
pwmAnterior = pwmAplicarMotores;
valorY = leituraMedia(zeroConsiderado);
if (valorY >= valorYKILL) {
pararMotores(sentidoVariacaoAnterior, pwmAnterior);
} else {
Input = valorY * -1;
myPID.Compute();
pwmAplicarMotores = Output;
if (pwmAplicarMotores < 0) {
pwmAplicarMotores = 0;
}
if (pwmAplicarMotores > 255) {
pwmAplicarMotores = 255;
}
acionaMotores(sentidoVariacao, pwmAplicarMotores);
}
Serial.print(" --- Y - ");
Serial.print(valorY);
Serial.print(" === saida: ");
Serial.print(Output);
Serial.print("\n");
}
//Funções default END