Added debug & Diagnostics

* Added file 'diagnostics.ino' which contains all debug functions
* Added file 'motorController.ino' which contains all functions for
controlling motors and applying PWM.
Merge changes by Serjh