Spektrum vs Mega let Micro be the judge

Removed serial printing on mega
reduced delay to 5ms
testing with alex
fixed minor conflits due to testing
seperat functions for updating the servo functions and printing to the serial