Programación robot de laberinto

Buen día,

Estoy construyendo un robot de laberinto “micromouse”, y tengo problemas con la programación del avance de este y no tengo muchos conocimientos con el arduino. A continuación les explico de que se trata:

El robot es de configuración diferencial, es decir dos ruedas (diametro 65 mm x 27mm)(distancia entre ruedas 85 mm-medida interna) con tracción y una rueda loca, esta compuesto por el Arduino Uno, tres sensores de ultrasonido HC-SR04, un controlador L298, una bateria lipo de 7.2 V 1000 mA, dos motorreductores pololu de 320 rpm y dos encoder magnéticos de 12 ppr del motor.

Encoder: Pololu - Magnetic Encoder Pair Kit for Micro Metal Gearmotors, 12 CPR, 2.7-18V (old version)
Motor: https://www.pololu.com/product/2214

Para el control de la velocidad de los motores lo hago por medio de PWM y el controlador L298, debido a la frecuencia de los pulsos del encoder hago uso de las dos interrupciones del Arduino Uno (Pin 2 y Pin3) pero los encoder son de doble cuadratura o dos canales por lo que solo leo uno de cada encoder.

El problema que tengo es que el robot no me avanza en línea, como puedo crear un control de los motores para que me avancen la misma distancia, a la misma velocidad.

este es el avance del código

//SALIDA PWM ENABLE L298

float periodo = 10;
int Motor1 = 11;
int Motor2 = 10;

// CONTROL GIRO MOTOR 1

int Adelante_Motor1 = 12;
int Atras_Motor1 = 13;

// CONTROL GIRO MOTOR 2

int Adelante_Motor2 = 9;
int Atras_Motor2 = 8;

//  ENCODER

int Pbin_0 = 0;
int encoder_1 = 0;

int Pbin_1 = 1;
int encoder_2 = 0;

// OTRAS VARIABLES

volatile int State = LOW;


void setup() 
{

// CONFIGURACION PINES DE CONTROL MOTORES
  
  pinMode (Motor1, OUTPUT);
  pinMode (Motor2, OUTPUT);
  pinMode (Adelante_Motor1, OUTPUT);
  pinMode (Atras_Motor1, OUTPUT);
  pinMode (Adelante_Motor2, OUTPUT);
  pinMode (Atras_Motor2, OUTPUT);

  //Serial.begin(9600);

// ENCODER CONFIG

  attachInterrupt (Pbin_1, contador_1, RISING); 
  attachInterrupt (Pbin_0, contador_2, RISING);

}

// FUNCION INTERRUPCION 0

void contador_1()
{
    encoder_1 ++;
}

// FUNCION INTERRUPCION 1

void contador_2()
{
    encoder_2 ++;  
}

// FUNCION PWM MOTORES

void ciclo (int value, int Motor)
{
  float duty = value/100.0;
  float timeOn = periodo*duty;
  float timeOff = periodo - timeOn;

  digitalWrite (Motor, HIGH);
  delay (timeOn);
  digitalWrite (Motor, LOW);
  delay (timeOff);
}

void loop() 
{
  if (encoder_1 <= 340 && encoder_2 <= 340)
  {
    ciclo (30, Motor1);
    digitalWrite (Adelante_Motor1, 1);
    
    ciclo (30, Motor2);
    digitalWrite (Adelante_Motor2, 1);
  }
  else
  {
    digitalWrite (Adelante_Motor1, 0);
    digitalWrite (Adelante_Motor2, 0); 
  }
}

Muchas gracias a las personas que me pueda ayudar, cualquier informacion adicional con gusto.

[b]Cuando llegas a un foro nuevo, siempre lee sus normas.
En ningún foro se permite que si abres un hilo al minuto u hora abras otro similar. En caso de error, lo resportas con el moderador/administrador para que el segundo sea borrado.[/b]

Luego edito y respondo tu consulta.