Desviar de objeto utilizando PID

Bom Dia galera! Sou iniciante na área e estou fazendo um robô seguidor de linha, a parte de seguir a linha está funcionando normalmente. Meu problema é em desviar de um objeto. O robô possui 3 sensores ultrasonicos, um na frente e dois nas laterais. Com os sensores da laterais ele tem que contornar um objeto sempre ficando a 30cm do mesmo. Estou tentando implementar com PID, porém sem sucesso. Podem me ajudar?

#include <PID_v1.h>

double Setpoint, Input, Output;
float distancia = 0;
unsigned long serialTime; //this will help us know when to talk with processing

PID myPID(&Input, &Output, &Setpoint, 0.30, 0, 0.0, DIRECT);

void setup() {
  Serial.begin(9600);
  //Motores
  pinMode(M1_IN1, OUTPUT);
  pinMode(M1_IN2, OUTPUT);
  pinMode(M2_IN3, OUTPUT);
  pinMode(M2_IN4, OUTPUT);

  //Inicializa sensores ultrasonicos
  pinMode(ultraEchoCenter, INPUT);
  pinMode(ultraTrigCenter, OUTPUT);

  pinMode(ultraEchoEsq, INPUT);
  pinMode(ultraTrigEsq, OUTPUT);

  pinMode(ultraEchoDir, INPUT);
  pinMode(ultraTrigDir, OUTPUT);

  digitalWrite(ultraTrigDir, LOW);
  delayMicroseconds(10);
  
  //PID
  Input = distancia;
  Setpoint = 30;
  myPID.SetMode(AUTOMATIC);

}

void loop() {
  digitalWrite(ultraTrigDir, LOW);
  // mantem a porta em LOW por 2 microssegundos
  delayMicroseconds(2);
  //Alterar trig para alto
  digitalWrite(ultraTrigDir, HIGH);
  //Mantem aberto por 10 ms
  delayMicroseconds(10);
  digitalWrite(ultraTrigDir, LOW);

  float tempo = pulseIn(ultraEchoDir, HIGH);
  //Coversao de tempo para cm
  distancia = tempo / 29.4 / 2;
  
  Input = distancia;
  myPID.Compute();

  int M1velocidade = 200+Output;
  int M2velocidade = 200-Output;

  //Verificacoes de velocidade dos motores.
    if (M1velocidade > 255)
      M1velocidade = 255;
    if (M2velocidade > 255)
      M2velocidade = 255;
    if (M2velocidade > 0) {
      digitalWrite(M2_IN3, LOW); //Para frente
    } else {
      digitalWrite(M2_IN3, HIGH); //Para frente
    }

  if (M1velocidade > 0) {
    digitalWrite(M1_IN1, LOW);
  } else {
    M1velocidade = 0;
  }

  //Abaixo fazemos o robo andar com base no cálculo PID
  analogWrite(M1_IN2, M1velocidade);
  analogWrite(M2_IN4, M2velocidade);
}