Also die ISR ist sicher zu voll, du rufst ja auch die LCD-Routine aus der ISR auf und hast am Ende noch ein Wait drinnen. Bist du sicher das du da keinen Stack-Überlauf produzierst?