First working version, rewritten code, removed LCD