Software debugging complete.

The code compiles without bugs. Currently working on optimising size of
code and ram usage.