Made debugging easier in BogieController

Used #ifdefs to enable and disable sections of code.
To be honest, I wasn't expecting this to be neccessary on the final
code, but issues seem to crop up often enough that it is.