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