PID controller returning a positive value even for nagetive angles

Question

My over all goal was to write a PID controller that allowed a rigid body to use torque to rotate to a specific angle based on player location (Aim/shoot pretty basic stuff).

Here is the problem. It derps out and just spins around in circles. the code looks like this:

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;


}

}

and the debug looks like this: http://puu.sh/j9Df2/52a2b2d07b.jpg

I see where the problem is occurring. (Some how I'm getting a positive value for accelleration even for negative angles.) But I have no idea what went wrong or how to fix it.

Answer 1

Check out this line:

error = currentAngle - deltaAngle; //generate error signal

Subtracting two angles directly is unlikely to be what you want to do. Let's say your angles are both from 0-360.

currentAngle = 5
deltaAngle = 355

Logically, you'd say that to go from currentAngle to deltaAngle you need to move -10 degrees, because 355 is the same place on a circle as -5. But if you just do currentAngle - deltaAngle, you get -350!

So how do you get the correct 10 degrees difference? Who cares, Unity can do it for you with Mathf.DeltaAngle(a, b)!