Create forward_test5

I've been tinkering for ages with this an finally (the other week) I cracked it. I got one file to do everything I want. 

In a couple of cases my solutions are far from perfect:
tacho_position should not be such a complex function, but I can't for the life of me work out how to call a tacho reading with the standard library tools. 
Also sonar is a one value reading, but I've stored it as to so as to make the velocity and acceleration functions work with any of my readings.