add trapezoidal motion profiling

[ctgoyette]

Explanation of motion profiling

Also take a look at this motion profiler: https://github.com/Cooper7196/Real-Time-Motion-Profiling