Got the firmware working and tested. Removed the prints which sped up the execution a lot. Cleaned up the python example code.