Arduino - Pasar delay() a millis()

////////////// DECLARACION PIN MOTORES ////////////////////////

////////////////////////////////////////////

//MOTOR B

int IN3 = 5;

int IN4 = 4;

//MOTOR A

int IN1 = 2;

int IN2 = 3;

//////////////////////////////////////

//////////////////////////////////////

//////////////// BUZZER /////////////////////////

/////////////////////////////////////////////////

const int pinBuzzer = 6;

/////////////////////////////////////////

////////////////////////////////////////

////////////// HCSR04/////////////////////////////

/////////////////////////////////////////////////

long tiempo;

int disparador = 9;   // triger

int entrada = 8;      // echo

float distancia;

const int ledObstaculo = 26;

////////////////////////////////////

void setup()

  {

//////////////motor/////////////////////

////////////////////////////////////////

  //MOTOR B

  pinMode (IN3, OUTPUT);

  pinMode (IN4, OUTPUT);

  //MOTOR A

  pinMode (IN1, OUTPUT);

  pinMode (IN2, OUTPUT);

////////////////////////////////////////

///////////////////////////////////////

////////////hcsr04////////////////////

/////////////////////////////////////

  pinMode(disparador, OUTPUT);

  pinMode(entrada, INPUT);

  Serial.begin(9600);

///////////////////////////////////////

/////////////////////////////////////

}

void Derecha()            //El robot gira en sentido derecho.

{

  // Motor B

  digitalWrite(IN4, LOW);

  analogWrite(IN3, 200);

  //MOTOR A

  digitalWrite(IN1, LOW);

  digitalWrite(IN2, LOW);

  }

void Izquierda()         //El robot gira en sentido izquierdo.

{

  // Motor B

  digitalWrite(IN3, LOW);

  digitalWrite(IN4, LOW);

  //MOTOR A

  digitalWrite(IN2, LOW);

  analogWrite(IN1, 200);

}

void Adelante()         //El robot va hacia adelante.

{

  // Motor B

  digitalWrite(IN4, LOW);

  analogWrite(IN3, 255);

  //MOTOR A

  digitalWrite(IN2, LOW);

  analogWrite(IN1, 255);

}

void Atras()          //El robot va hacia atrás.

{

  // Motor B

  digitalWrite(IN3, LOW);

  analogWrite(IN4, 255);

  //MOTOR A

  digitalWrite(IN1, LOW);

  analogWrite(IN2, 255);

}

void Parar()          //Robot para.

{

  // Motor B

  digitalWrite(IN3, LOW);

  digitalWrite(IN4, LOW);

  //MOTOR A

  digitalWrite(IN1, LOW);

  digitalWrite(IN2, LOW);

}

////////////////////////////////// RUTA DE PRUEBA ////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////

void loop()

{

MedidaDistancia();

if(distancia <=30)

{

  SonidoLedObstaculo();

  delay(1000);

}

  else

{

  RutaMotor1();

}

delay(2000);

}

//////////////Void Ruta Motor 1 motores/////////////////////

//////////////////////////////////////////////

void RutaMotor1()

{

  Adelante();

  delay(5000);           //tiempo que esta funcionando Adelante

  Parar();

  delay(2000);          // tiempo que esta parado hasta la siguiente funcion

  Derecha();

  delay(1000);          //tiempo que esta funcionando Derecha

  Parar();

  delay(2000);         // tiempo que esta parado hasta la siguiente funcion

  Adelante();

  delay(5000);

  Parar();

  delay(2000);

  Derecha();

  delay(2000);

  Parar();

  delay(2000);

  Adelante();

  delay(5000);

  Parar();

  delay(2000);

  Izquierda();

  delay(1000);

  Parar();

  delay(2000);

  Adelante();

  delay(5000);

  Parar();

  delay(2000);

  Parar();

  delay(1000);

}

////////////////// VOID MEDID DISTANCIA hcsr04 //////////////////////

////////////////////////////////////////////////////////////////////

void SonidoLedObstaculo()

{

  if(distancia <= 30){

 {

   digitalWrite(ledObstaculo,HIGH); //Si el sensor mide una distancia más pequeña o igual a 30cm LED ON

   ObstaculoBuzzer();

   delay(1000);

  }

  delay(1000);

  }

  else{

    digitalWrite(ledObstaculo,LOW); //Sinó, apaga el LED

  }

}

void MedidaDistancia()

{

    // lanzamos un pequeño pulso para activar el sensor

  digitalWrite(disparador, HIGH);

  delayMicroseconds(10);

  digitalWrite(disparador, LOW);

  // medimos el pulso de respuesta

  tiempo = (pulseIn(entrada, HIGH)/2); // dividido por 2 por que es el 

                                       // tiempo que el sonido tarda

                                       // en ir y en volver

  // ahora calcularemos la distancia en cm

  // sabiendo que el espacio es igual a la velocidad por el tiempo

  // y que la velocidad del sonido es de 343m/s y que el tiempo lo 

  // tenemos en millonesimas de segundo

  distancia = float(tiempo * 0.0343);

  // y lo mostramos por el puerto serie una vez por segundo

  Serial.println(distancia);

  delay(1000);

}

void ObstaculoBuzzer()

{

  //generar tono de 440Hz durante 1000 ms

  tone(pinBuzzer, 150);

  delay(550);

  tone(pinBuzzer, 50);

  delay(700);

  tone(pinBuzzer, 325);

  delay(250);

  tone(pinBuzzer, 50);

  delay(600);

  tone(pinBuzzer, 670);

  delay(200);

  tone(pinBuzzer, 1125);

  delay(200);

  tone(pinBuzzer, 2500);

  delay(800);

  noTone(pinBuzzer);

}

///////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////

[/quote]

Esto creo que se debe a la programacion realizada con los delay().

La solución sería realizar el mismo sketch pero con millis(), cosa que soy incapaz de realizar.

A continuacion pongo el sketch unicamente de los motores, realizada la programacion con delay(), que es lo que hay que pasar a millis().

Pero que soy incapaz...

[quote]

///////////////    DECLARACION PIN MOTORES         ////////////////

/////////////   PARA EL CONTROL DEL MOTOR CON L298N ///////////

//MOTOR B

int IN3 = 5;

int IN4 = 4;

//MOTOR A

int IN1 = 2;

int IN2 = 3;

//////////////////////////////////////////////

/////////////////////////////////////////////

void setup()

  {

  //MOTOR B

  pinMode (IN3, OUTPUT);

  pinMode (IN4, OUTPUT);

  //MOTOR A

  pinMode (IN1, OUTPUT);

  pinMode (IN2, OUTPUT);

}

void Derecha()

{

  // Motor B

  digitalWrite(IN4, LOW);

  analogWrite(IN3, 200);

  //MOTOR A

  digitalWrite(IN1, LOW);

  digitalWrite(IN2, LOW);

  }

void Izquierda()

{

  // Motor B

  digitalWrite(IN3, LOW);

  digitalWrite(IN4, LOW);

  //MOTOR A

  digitalWrite(IN2, LOW);

  analogWrite(IN1, 200);

}

void Adelante()

{

  // Motor B

  digitalWrite(IN4, LOW);

  analogWrite(IN3, 255);

  //MOTOR A

  digitalWrite(IN2, LOW);

  analogWrite(IN1, 255);

}

void Atras()

{

  // Motor B

  digitalWrite(IN3, LOW);

  analogWrite(IN4, 255);

  //MOTOR A

  digitalWrite(IN1, LOW);

  analogWrite(IN2, 255);

}

void Parar()

{

  // Motor B

  digitalWrite(IN3, LOW);

  digitalWrite(IN4, LOW);

  //MOTOR A

  digitalWrite(IN1, LOW);

  digitalWrite(IN2, LOW);

}

////////////////////////////////// RUTA DE MOTOR ////////////////////////////////////

////////////////////////////////  RUTA DE PRUEBA  ////////////////////////////////

void loop() {

  Adelante();

  delay(5000);        //tiempo que esta funcionando Adelante

  Parar();

  delay(2000);        // tiempo que esta parado hasta la siguiente funcion

  Atras();

  delay(5000);        //tiempo que esta funcionando Atras

  Parar();

  delay(2000);

  Derecha();

  delay(5000);        //tiempo que esta funcionando Dereccha

  Parar();

  delay(2000);

  Izquierda();        //tiempo que esta funcionando Izquierda

  delay(5000);

  Parar();

  delay(2000);

  Parar();

  delay(1000);

}

///////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////

/*************

 * SIN DELAY *

 *************/

/* Este programa realiza dos trabajos al mismo tiempo: el primero,

hacer parpadear un intermitente; el segundo, ejecutar una lista de

tareas. En este caso, se considera cada tarea terminada una vez se

acaba el tiempo que se supone lleva realizarla.

   La secuencia de una ejecucion es como sigue: no hay accion has-

ta que no se activa el pulsador de marcha. Una vez dado, se ejecu-

ta la lista de tareas. Mientras no esten terminadas, el piloto in-

termitente parpadeara con T=0,5s; y el pulsador de marcha se igno-

rara. Una vez se haya concluido, todo se apagara de nuevo. Por ca-

da tarea hay una luz indicadora de realizacion que permanecera en-

cendida el tiempo que dure dicha actividad. La secuencia se repite

tantas veces como se desee. */

/* ENTRADAS

   ======== */

#define E_MARCHA 2

/* SALIDAS

   ======= */

#define S_INTERMITENTE 3

#define S_ADELANTE 4

#define S_ATRAS 5

#define S_SONREIR 6

/* INFORMACION DEL ESTADO DEL SISTEMA

   ================================== */

// Informacion generica

boolean enMarcha = false; // Saber si hay tareas pendientes

long tActual = 0; // Llevar cuenta del tiempo transcurrido

// Informacion sobre el intermitente

#define T_INTERMITENCIA 625 // Tiempo de cambio: 625ms

long tUltimoCambio = 0; // Tiempo del ultimo cambio

boolean itmttOn = false; // ¿Esta el intermitente encendido?

// Lista de tareas: con sus identificadores de tareas, ...

#define TAREA_ADELANTE 0

#define TAREA_ATRAS 1

#define TAREA_SONREIR 2

// ... la lista propiamente dicha, y ...

#define NO_TAREAS 7 // Numero de tareas en la lista

byte lTareas[NO_TAREAS] = {

  TAREA_ADELANTE,

  TAREA_ATRAS,

  TAREA_SONREIR,

  TAREA_ATRAS,

  TAREA_ADELANTE,

  TAREA_SONREIR,

  TAREA_ADELANTE

};

// ... y llevar la cuenta sobre las tareas

long tInicioTarea = 0; // Tiempo de inicio de la tarea actual

byte noTareaActual = 0; // Numero de la tarea actual en la lista

/* FUNCIONES

   ========= */

void setup() { // Inicializar el programa

  pinMode(E_MARCHA,INPUT);

  pinMode(S_INTERMITENTE,OUTPUT);

  pinMode(S_ATRAS,OUTPUT);

  pinMode(S_SONREIR,OUTPUT);

}

/* Todo loop (ciclo) empieza leyendo las entradas (informacion del

entorno y recepcion de ordenes), para inmediatamente hacer aquello

que se desee teniendo en cuenta lo que se recuerda que sucede y lo

que las  entradas han contado. */

void leerEntradas() { // Poner al dia la informacion del estado

   if(enMarcha) tActual = millis(); // Toca cronometrar

   else // En reposo, pero... ¿toca trabajar?

      if(digitalRead(E_MARCHA) == HIGH) {

        enMarcha = true; // A trabajar, ...

        noTareaActual = 0; // ..., empezando por la primera tarea, y ...

        tInicioTarea = millis(); // ... y ya. ¡Crono andando!

      }

}

void trabajoParpadeo() { // Hacer que parpadee la salida

   // Comprobacion de si toca parpadear

   if(tActual - tUltimoCambio >= T_INTERMITENCIA) { // ¿ya toca?

     itmttOn = ! itmttOn; // Cambio de iluminacion

     tUltimoCambio = tActual; // Reiniciamos la cuenta

   }

   // Mostrar lo que sea que toque

   if(itmttOn) digitalWrite(S_INTERMITENTE,HIGH);

   else digitalWrite(S_INTERMITENTE,LOW);

}

void trabajoLTareas() { // Haciendo tareas

   // Seleccionar la tarea actual correspondiente

   byte tareaActual = lTareas[noTareaActual];

   if(tareaActual == TAREA_ADELANTE) irAdelante();

   else if(tareaActual == TAREA_ATRAS) irAtras();

   else if(tareaActual == TAREA_SONREIR) sonreir();

   else enMarcha = false; // Tipo de tarea desconocida. ¡Alto!

}

void irAdelante() {

   // Codigo de avance

   digitalWrite(S_ADELANTE,HIGH);

   // Tras avanzar, ¿se ha terminado la tarea?

   if(tActual - tInicioTarea > 2200) {

      digitalWrite(S_ADELANTE,LOW);

      siguienteTarea();

   }

}

void irAtras() {

   // Codigo de retroceso

   digitalWrite(S_ATRAS,HIGH);

   // Tras retroceder, ¿se ha terminado la tarea?

   if(tActual - tInicioTarea > 2200) {

      digitalWrite(S_ATRAS,LOW);

      siguienteTarea();

   }

}

void sonreir() {

   // Codigo de hacer sonar la sirena

   digitalWrite(S_SONREIR,HIGH);

   // Tras sonar la sirena, ¿se ha terminado la tarea?

   if(tActual - tInicioTarea > 500) {

      digitalWrite(S_SONREIR,LOW);

      siguienteTarea();

   }

}

void siguienteTarea() {

   // Pasar a la siguiente tarea

   ++noTareaActual;

   tInicioTarea = tActual;

   // Porque... ¿hay tarea, verdad?

   enMarcha = noTareaActual != NO_TAREAS;

}

void loop() {

  leerEntradas();

  if(enMarcha) {

     trabajoParpadeo();

     trabajoLTareas();

  }

}