Funzione con fasi temporizzate

Salve.
Sto programmando un Atmega 328p per fungere da scheda elettronica per una lavatrice.
Ora mi trovo davanti ad un problema. Dovrei programmare la fase di lavaggio con una logica di questo tipo:

Carico Acqua -> Lavaggio a Freddo per 1 minuto -> Riscaldamento Acqua --> A riscaldamento effettuato lava per altri 5 minuti --> Scarica Acqua

Il problema sorge dal fatto che la fase di riscaldamento dipende dalla temperatura impostata e quindi non è di lunghezza costante. Inoltre, nella fase precedente al riscaldamento, se dovesse abbassarsi il livello dell'acqua, deve ricaricarla e azzerare il timer, così che la fase di 1 minuto inizi dopo aver raggiunto il livello desiderato.

Il codice che ho scritto è il seguente:

bool lavaggioExpress(short int temperatura)
{
  static unsigned long tempoLavaggio;
  static short int stage = 0;

  if (caricaLav() == 0)//vasca vuota
  {
    caricaLav();
    tempoLavaggio = millis();
  }
  else//vasca piena
  {
    lavaggioNormale();
    if (millis() - tempoLavaggio > 60000 && stage == 0) //1 minuto in acqua fredda
    {
      riscaldamento(temperatura);
      if (riscaldamento(temperatureVett[temperatura]) == 1)
      {
        stage = 1;
        tempoLavaggio = 60000;
      }
    }
    if (stage == 1) //post riscaldamento
    {
      if (millis() - tempoLavaggio > 36000)
      {
        scarico(15000);//scarico fino a pressostato vuoto più 15 sec
        return (scarico(15000));
      }
    }
  }
}

Ma non mi convince. Le funzioni carica(), scarico() e riscaldamento() sono di tipo bool e restituiscono 1 quando sono completate. Vi chiedo come affrontereste un problema di questo tipo. C'è qualcosa di errato nel codice? Sono sicuro che si possa migliorare, il dubbio è come. Grazie

Thread in violazione del REGOLAMENTO, punto 15 e suoi sottopunti.

Mi spiace, devo chiudere. :confused:

Guglielmo