Trapezoidal Acceleration code for the 2014 robot