More code!

Finished SD error reporting mechanisms and all init code. Added servo
code. Plenty of debug stuff.