Ciao
(so che ormai la maturità è vicina quindi non avrete molto tempo da dedicare al progetto

) il tuo sw si blocca perché la funzione finecorsa continua ad essere eseguita occupando tutte le risorse disponibili... vedo che sono stati usati molti "salti" (goto), mentre la "buona" programmazione, anche in Arduino, suggerisce di sfruttare il loop() principale utilizzando delle variabili di stato, qualcosa tipo:
- premo il pulsante piano 1, imposto una variabile di stato che dice "sto andando al piano 1"
- all'interno del loop principale verifico: sono nello stato "sto andando al piano 1"? allora controllo se ci sono arrivato
spero di essermi spiegato...