Use lcd in stead of Serial
Cleanup + small fixes
Display fixes + cleanup