Got the Trapezoidal Motion Control to Sort of Work!!!!