Bug fixes

The Microcontroller was running too fast so a delay was needed. Some
debug code was also added. This will be erased once the code is fully
tested