big changes to stepping code - change how thread start stops - add in slow start
and slow end for position changes
Sonar code changed to use 5 readings instead of 3 - still return mediam result
Sonar now return value on all circumstances