Tidied up and added lcd prints for troubleshooting