First commit of working code

This code has been working for at least 2 days on the raspberry pi.