DSP 速度曲线规划 电机

请教大家，如何做DSP 电机的速度曲线规划？ eg：做一个梯形的速度曲线规划，有一个先加速，再匀速，再减速的过程。程序上如何实现呢？
已经实现了位置环控制。本人想在位置环内规划速度曲线，使得它到位能够准确。