problema en codigo de movilidad de un robot

International Español
1

Hola
buenos días, tardes o noches dependiendo la hora en la que leas este post, necesito una ayuda de revisión, yo soy estudiante y estoy aprendiendo a usar arduino, mi proyecto es hacer que un robot compuesto de 2 motores donde es salga de una caja girando a la derecha y luego siga recto, he intentado pero no me sale que se dirija de forma recta, se que es muy básico pero si por favor me podrían revisar el código y decirme si así esta bien estructurado o como podría solucionarlo seria de mucha ayuda,es la ultima corrección que le he hecho, gracias por su atención

#include <NewPing.h>

int m_1_p_1 = 2;
int m_1_p_2 = 3;
int m_2_p_1 = 4;
int m_2_p_2 = 5;

NewPing sensor_1(6,7,3);
NewPing sensor_2(8,9,3);

void setup()
{
  Serial.begin (9600);
  pinMode(m_1_p_1, OUTPUT);
  pinMode(m_1_p_2, OUTPUT);
  pinMode(m_2_p_1, OUTPUT);
  pinMode(m_2_p_2, OUTPUT);
}

void loop()
{
  int distance= sensor_1.ping_cm();
  int distance_2= sensor_2.ping_cm();
  Serial.println(distance);
  Serial.println(distance_2);
  delay(200);

  digitalWrite(m_1_p_1, HIGH);
  digitalWrite(m_1_p_1, LOW);  
  digitalWrite(m_2_p_1, LOW);
  digitalWrite(m_2_p_2, LOW);
  delay(900);
   digitalWrite(m_1_p_1, LOW);
  delay(100);
   digitalWrite(m_1_p_1, HIGH);
   digitalWrite(m_2_p_2, HIGH);
  delay(1000);
  digitalWrite(m_1_p_1, LOW);
  digitalWrite(m_2_p_2, LOW);
  delay(500);

  
}
2

El código no tiene orden, tal vez lo tenga para ti pero no para alguien que lo lee y no entiende nada.

Separa las funciones de modo que sea comprensible para quien pueda brindarte ayuda.

Ejemplo:

avance();
derecha();
izquierda();

o comandos de este tipo.

Supongamos porque no lo se, pero lo intuyo que esto sea detener

void detener() {
  digitalWrite(m_1_p_1, LOW);
  digitalWrite(m_2_p_2, LOW);
}

Es un ejemplo