优化位置控制

修改struct PID和struct err