testing with alex
fixed minor conflits due to testing
seperat functions for updating the servo functions and printing to the serial
merged with alex and changed the PID for better compute order
merged with alex and changed the PID for better compute order
Merge branch 'feature-ovoidance-PIDs' into develop

merged alex's improved PID and sonar stuff with some weeeks in to the seprat
report and serial out put functions to hopefully avoid nasty pwm pulses