DFOC_M0_set_Velocity_Angle 代码bug

[Tony607]

DFOC_M0_set_Velocity_Angle函数中DFOC_M0_VEL_PID函数的输入应该是DFOC_M0_ANGLE_PID的输出（即期望的速度）减去当前的速度，这样才是正确的双环控制策略。
因此，DFOC_M0_set_Velocity_Angle函数可以修改为如下形式：

void DFOC_M0_set_Velocity_Angle(float Target)
{
  float sensor_angle = DFOC_M0_Angle();
  float sensor_vel = DFOC_M0_Velocity();
  float target_vel = DFOC_M0_ANGLE_PID((Target - sensor_angle)*180/PI);
  setTorque(DFOC_M0_VEL_PID(target_vel - sensor_vel), _electricalAngle());
}

函数首先计算出当前的角度和速度，然后将角度误差输入到角度环PID控制器，得到期望的速度。再将期望的速度和当前的速度做差，得到速度误差，再输入到速度环PID控制器，得到最终的输出。