电机转角控制算法

时间周期是1-10ms可调。
橙色和绿色代表期望和真实速度，
红色代表期望角度，
紫色代表电机转角，蓝色是减速机输出角度。
在9700时刻角度已到达期望角度，速度由-200还未到0；所以算法上需要在要到达目标前某值就需要提前减速。这个值和当前真实速度有关。
第一次修改后效果不好，真实速度和期望有延迟且同步不太好。
超调很多，加一个偏移值，让期望速度更提前减速，才能让真实角度跟得上

这里的红色是真实角度，紫色是期望角度。
修改了pid的i积分比例和积分总量随动，效果还不是太好，虽然能降低震荡，但和目标值差距有10/32768 * 360°的差距。