Added more functions to Motorcontroller, time for testing but before
program monitoring with PC