Finished 1 DOF trajectory generation with quintic function