C# PID控制器返回正值，即使是动态角度

我的总体目标是编写一个PID控制器，允许刚体根据玩家的位置（瞄准/射击非常基本的东西）使用扭矩旋转到特定的角度

问题就在这里。它向外旋转，只是绕着圈子旋转。代码如下所示：

public class EnemyBehavour : MonoBehaviour {
    //speed of enemy
    public float speed = .5f;
    //base distance enemy will attack player
    public float range = 3f;
    //how far back enemy will stand
    public float stopDistance;
    //what the enemy will pursue
    private GameObject target;
    //targetable Enemies
    private GameObject[] targetable;
    //itself
    private Rigidbody self;
    //distance to check against
    private float saveDistance;
    //get the mass for physics stuff
    float mass;
    //gets drag for physics
    float drag;
    //gets angular drag for physics
    float adrag;
    //checks distance
    float distance; 
    //the player
    GameObject player;
    GameObject[] turrets;
    //the base  
    GameObject playerBase;
    EnemyShoot Shoot;
    //torque holder;
    float torque;
//used for PID
public float maxAccPID = 180f;
public float maxSpeedPID = 90f;
public float pGain = 20f;
public float dGain = 10f;
float Accel;
float anglSpeed;


float error;
float lastError = 0f;
float diff;




void Start () {
    //get own rigidbody
    self = GetComponent<Rigidbody> ();
    //playerbase should be obviouse
    playerBase = GameObject.FindGameObjectWithTag ("Base");
    target = playerBase;

    Shoot = GetComponentInChildren<EnemyShoot> ();

    mass = self.mass;
    drag = self.drag;
    adrag = self.angularDrag;
    PIDRotation ();
}


void FixedUpdate () {
    target = FindFoe ();

    //checks range and 
    if (Vector3.Distance (transform.position, target.transform.position) > range || target == playerBase) {
        target = playerBase;
    }
    torque = PIDRotation ();
    //Debug.Log (torque.ToString ());



    torque *= .1f;
    //look at the pursueing target
    self.AddTorque (Vector3.up * torque);
    //move towards the pursuing target
    self.AddForce (transform.forward * speed);
    Debug.Log (torque.ToString ());
}

float PIDRotation () {

    //this gets the difference between facing forward and facing target
    //this creates the vector facing the target
    Vector3 relativePos = self.position - target.transform.position;
    //this gets the crossproduct or the sin of angle between the facing vector and the vector to face
    Vector3 crossAngle = Vector3.Cross (transform.forward.normalized, relativePos.normalized);
    //this gets the current angle
    float currentAngle = transform.eulerAngles.y;
    //this gets the angle in euler to turn; positive is 
    float deltaAngle = Mathf.Asin (crossAngle.y)/Mathf.PI * 180;
    Debug.Log (crossAngle.ToString () + " " + deltaAngle.ToString());



    error = currentAngle - deltaAngle; //generate error signal
    diff = (lastError - error) / Time.deltaTime; //calculate differentail
    Debug.Log ("diff " + diff.ToString ());
    lastError = error;
    Debug.Log ("error " + error.ToString ());
    //calculate acceleration

    Accel = error * pGain + diff * dGain;

    Debug.Log ("Accel1 " + Accel.ToString ());
    Accel = Mathf.Clamp (Accel, -1f * maxAccPID, maxAccPID);
    Debug.Log ("Accel2 " + Accel.ToString ());
    anglSpeed = Accel * Time.deltaTime;
    anglSpeed = Mathf.Clamp (anglSpeed, -maxSpeedPID, maxSpeedPID);

    return anglSpeed;












}






//checks if turrets or player is within range
GameObject FindFoe() {
    distance = range;
    targetable = GameObject.FindGameObjectsWithTag ("ETarget");

    //sets player base as default target


    foreach (GameObject test in targetable) {
        GameObject testing = test.transform.parent.gameObject;
        print (testing.ToString());
        saveDistance = Vector3.Distance (transform.position, testing.transform.position);
        if (saveDistance < distance) {
            distance = saveDistance;
            print (saveDistance.ToString());
            target = testing;
        }



    }
    return target;


}

公共类EnemyBehavor:单一行为{
//敌人的速度
公共浮动速度=.5f；
//基地距离敌人将攻击玩家
公共浮动范围=3f；
//敌人会退后多远
公共浮动停车距离；
//敌人将追求什么
私有游戏对象目标；
//目标敌人
私有游戏对象[]可作为目标；
//本身
私人刚体自我；
//检查距离
私人浮动储蓄距离；
//获取物理材料的质量
浮体；
//对物理学有兴趣
浮动阻力；
//获取物理的角阻力
漂浮漂浮；
//检查距离
浮动距离；
//球员
游戏对象玩家；
游戏物体[]炮塔；
//基地
游戏对象玩家库；
敌人射击；
//扭矩保持器；
浮动扭矩；
//用于PID控制
公共浮点数maxAccPID=180f；
公共浮点数maxSpeedPID=90f；
公众浮标pGain=20f；
公共浮球dGain=10f；
浮动加速度；
浮动速度；
浮动误差；
浮点lastError=0f；
浮差；
无效开始（）{
//拥有自己的刚体
self=GetComponent（）；
//playerbase应该是显而易见的
playerBase=GameObject.FindGameObjectWithTag（“Base”）；
target=playerBase；
Shoot=getComponentChildren（）；
质量=自身质量；
阻力=自阻力；
adrag=自成角阻力；
PIDRotation（）；
}
无效固定更新（）{
target=FindFoe（）；
//检查射程和距离
if（Vector3.Distance（transform.position，target.transform.position）>range | | target==playerBase）{
target=playerBase；
}
扭矩=旋转（）；
//Log（torque.ToString（））；
扭矩*=0.1f；
//看看追求的目标
self.AddTorque（矢量3.up*扭矩）；
//向着追求的目标前进
self.AddForce（transform.forward*速度）；
Log（torque.ToString（））；
}
浮动旋转（）{
//这将获得面向前方和面向目标之间的差异
//这将创建面向目标的向量
Vector3 relativePos=self.position-target.transform.position；
//这将获得面向量和面向量之间的交叉积或角度sin
Vector3交叉角=Vector3.Cross（transform.forward.normalized，relativePos.normalized）；
//这将获取当前角度
float currentAngle=transform.eulerAngles.y；
//这将获取要旋转的euler中的角度；正值为
浮动三角洲角=Mathf.Asin（交叉角y）/Mathf.PI*180；
Log（crossAngle.ToString（）+“”+deltaAngle.ToString（））；
error=currentAngle-deltaAngle；//生成错误信号
diff=（lastError-error）/Time.deltaTime；//计算差异尾
Log（“diff”+diff.ToString（））；
lastError=错误；
Log（“error”+error.ToString（））；
//计算加速度
加速度=误差*pGain+diff*dGain；
Log（“Accel1”+Accel.ToString（））；
加速度=数学钳位（加速度，-1f*maxAccPID，maxAccPID）；
Log（“Accel2”+Accel.ToString（））；
anglSpeed=加速度*时间增量时间；
anglSpeed=数学钳位（anglSpeed，-maxSpeedPID，maxSpeedPID）；
返回速度；
}
//检查炮塔或玩家是否在射程内
GameObject FindFoe（）{
距离=范围；
targetable=GameObject.FindGameObjectsWithTag（“ETarget”）；
//将玩家基础设置为默认目标
foreach（目标中的游戏对象测试）{
游戏对象测试=test.transform.parent.GameObject；
打印（testing.ToString（））；
saveDistance=Vector3.距离（transform.position，testing.transform.position）；
如果（保存距离<距离）{
距离=保存距离；
打印（saveDistance.ToString（））；
目标=测试；
}
}
回报目标；
}

}

调试过程如下所示：

我知道问题出在哪里了。（有些人说我是如何得到加速度的正值的，即使是负角度。）但我不知道哪里出了问题，也不知道如何修复它

查看这一行：

error = currentAngle - deltaAngle; //generate error signal

直接减去两个角度不太可能是您想要做的。假设你的角度都是从

0

-

360

currentAngle = 5
deltaAngle = 355

从逻辑上说，要从

currentAngle

移动到

deltaAngle

，需要移动

-10

度，因为

355

与

-5

在圆上的位置相同。但是如果你只做

currentAngle-deltaAngle

，你会得到

-350

那么如何获得正确的

10

度差呢？谁在乎