first draft of trapezoidal velocity profile (incorrect but runs)
also mention PD control