Search Unity

  1. Welcome to the Unity Forums! Please take the time to read our Code of Conduct to familiarize yourself with the forum rules and how to post constructively.
  2. We have updated the language to the Editor Terms based on feedback from our employees and community. Learn more.
    Dismiss Notice
  3. Join us on November 16th, 2023, between 1 pm and 9 pm CET for Ask the Experts Online on Discord and on Unity Discussions.
    Dismiss Notice
  4. Dismiss Notice

Newtonian Flight Assistant

Discussion in 'Scripting' started by BradMick, Dec 14, 2016.

  1. BradMick

    BradMick

    Joined:
    Apr 2, 2014
    Posts:
    113
    Howdy,

    It's been a while with work and all kinds of fun stuff, time to get back to coding. So, to start I decided I'd work on revamping my space flight system and to support that I've started writing the Flight Assist system. Basically the idea is that this guy kicks in and helps fly the ship. So far I've got it working on two axes so long as you stay on those axes.

    I originally started with the normalized angular velocities on each axis. That didn't exactly work, so I then broke down those velocities into their x, y, and z components and normalized them myself (turns out the default maximum angular velocity is 7...easy enough). This produced the results I wanted. You pitch or yaw and keep the ship on axis and once you let off the thruster switch, the opposite side thrusters fire to slow you down...I've even got it to where it will 0 the velocities below a certain threshold...(it's a bit of a cheat I know).

    Now here's where it gets fun:

    Whenever you maneuver the ship off the x or y axes, the system falls apart. I know it's working, and it does it's best to null the velocities but ultimately fails and freaks out. It actually makes things worse and usually results in complete loss of control of the ship.

    And that's where I need help. Sorry for the state of the code, it's a prototype so it's not as polished as it could be.

    Thoguht...I think maybe the problem lies in an inaccurate velocity read...are the angular velocities using global or local axes? I suspect global, because in my test scene I origiented the ship (see picture) and then turned off the flight assist and thrusted...my Y axis velocity hit the max of 7, but the output is 0.6 and it should be 1....that being said, how would I change that to local? I think if I can solve that this will essentially solve itself...anyway, code and pics!

    Code (CSharp):
    1. using UnityEngine;
    2. using System.Collections;
    3.  
    4. public class FlightTest : MonoBehaviour
    5. {
    6.     private GameObject  ParentObject = null;
    7.     private Rigidbody   RB = null;
    8.  
    9.     private GameObject  KeyboardObject = null;
    10.     private Keyboard    KB = null;
    11.  
    12.     public  GameObject      maneuverJetRootObject;
    13.     private ManeuverJetRoot maneuverJetRoot;
    14.  
    15.     private GameObject[] maneuverJets;
    16.  
    17.     private Thruster[]  thrusters;
    18.  
    19.     private float normalizedAngularVelocityX, normalizedAngularVelocityY, normalizedAngularVelocityZ;
    20.  
    21.     void Start()
    22.     {
    23.         ParentObject = gameObject.transform.root.gameObject;
    24.         RB = ParentObject.GetComponent<Rigidbody>();
    25.  
    26.         KeyboardObject = gameObject.transform.root.gameObject;
    27.         KB = KeyboardObject.GetComponent<Keyboard>();
    28.  
    29.         //Assign the manuevering jet root
    30.         maneuverJetRoot = maneuverJetRootObject.GetComponent<ManeuverJetRoot>();
    31.         //Next, copy all of the maneuvering jets from the root
    32.         maneuverJets = maneuverJetRoot.GetManeuveringJets();
    33.         //Initialize the thruster array
    34.         thrusters = new Thruster[maneuverJets.Length];
    35.         //Copy the thrusters from the root
    36.         for (int i = 0; i < maneuverJets.Length; i++)
    37.         {
    38.             thrusters[i] = maneuverJets[i].GetComponent<Thruster>();
    39.         }
    40.     }
    41.  
    42.     void Update()
    43.     {
    44.         normalizedAngularVelocityX = RB.angularVelocity.x / 7.0f;
    45.         normalizedAngularVelocityY = RB.angularVelocity.y / 7.0f;
    46.         normalizedAngularVelocityZ = RB.angularVelocity.z / 7.0f;
    47.  
    48.         Debug.Log("x: " + normalizedAngularVelocityX + " y: " + normalizedAngularVelocityY + " z: " + normalizedAngularVelocityZ);
    49.  
    50.         //Yaw Control
    51.         YawControl();
    52.         NullYRotation();
    53.         //Pitch Control
    54.         PitchControl();
    55.         NullXRotation();
    56.     }
    57.  
    58.     private void NullXRotation()
    59.     {
    60.         Debug.Log("X Vel: " + normalizedAngularVelocityX);
    61.  
    62.         //Stop Pitch Down by activating the thrusters on the opposite side of the ship...
    63.         if (normalizedAngularVelocityX > 0.0f && KB.WKeyState() != true)
    64.         {
    65.             Debug.Log("Stop Pitching Down!");
    66.             if (normalizedAngularVelocityX >= 0.0f)
    67.             {
    68.                 thrusters[6].ApplyThrust();
    69.                 thrusters[7].ApplyThrust();
    70.  
    71.                 thrusters[8].ApplyThrust();
    72.                 thrusters[9].ApplyThrust();
    73.             }
    74.  
    75.             if (normalizedAngularVelocityX <= 0.0001f)
    76.             {
    77.                 RB.angularVelocity = new Vector3(0.0f, normalizedAngularVelocityY, normalizedAngularVelocityZ);
    78.             }
    79.         }
    80.         //Stop Pitch Up by activating the thrusters on the opposite side of the ship...
    81.         if (normalizedAngularVelocityX < 0.0f && KB.SKeyState() != true)
    82.         {
    83.             Debug.Log("Stop Pitching Up!");
    84.             if (normalizedAngularVelocityX <= 0.0f)
    85.             {
    86.                 thrusters[4].ApplyThrust();
    87.                 thrusters[5].ApplyThrust();
    88.  
    89.                 thrusters[10].ApplyThrust();
    90.                 thrusters[11].ApplyThrust();
    91.             }
    92.  
    93.             if (normalizedAngularVelocityX >= -0.0001f)
    94.             {
    95.                 RB.angularVelocity = new Vector3(0.0f, normalizedAngularVelocityY, normalizedAngularVelocityZ);
    96.             }
    97.         }
    98.     }
    99.  
    100.     private void NullYRotation()
    101.     {
    102.         Debug.Log("Y Vel: " + normalizedAngularVelocityY);
    103.  
    104.         //Stop Left Yaw by activating the thrusters on the opposite side of the ship...
    105.         if (normalizedAngularVelocityY < 0.0f && KB.AKeyState() != true)
    106.         {
    107.             Debug.Log("Stop Yawing Left!");
    108.             if (normalizedAngularVelocityY <= 0.0f)
    109.             {
    110.                 thrusters[1].ApplyThrust();
    111.                 thrusters[2].ApplyThrust();
    112.             }
    113.  
    114.             if (normalizedAngularVelocityY >= -0.0001f)
    115.             {
    116.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, 0.0f, normalizedAngularVelocityZ);
    117.             }
    118.         }
    119.         //Stop Right Yaw by activating the thrusters on the opposite side of the ship...
    120.         if (normalizedAngularVelocityY > 0.0f && KB.DKeyState() != true)
    121.         {
    122.             Debug.Log("Stop Yawing Right!");
    123.             if (normalizedAngularVelocityY >= 0.0f)
    124.             {
    125.                 thrusters[0].ApplyThrust();
    126.                 thrusters[3].ApplyThrust();
    127.             }
    128.  
    129.             if (normalizedAngularVelocityY <= 0.0001f)
    130.             {
    131.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, 0.0f, normalizedAngularVelocityZ);
    132.             }
    133.         }
    134.     }
    135.  
    136.     private void YawControl()
    137.     {
    138.         //Yaw Left
    139.         if (KB.AKeyState() == true)
    140.         {
    141.             thrusters[0].ApplyThrust();
    142.             thrusters[3].ApplyThrust();
    143.         }
    144.         //Yaw Right
    145.         if (KB.DKeyState() == true)
    146.         {
    147.             thrusters[1].ApplyThrust();
    148.             thrusters[2].ApplyThrust();
    149.         }
    150.     }
    151.  
    152.     private void PitchControl()
    153.     {
    154.         //Pitch down
    155.         if (KB.WKeyState() == true)
    156.         {
    157.             thrusters[4].ApplyThrust();
    158.             thrusters[5].ApplyThrust();
    159.  
    160.             thrusters[10].ApplyThrust();
    161.             thrusters[11].ApplyThrust();
    162.         }
    163.         //Pitch Up
    164.         if (KB.SKeyState() == true)
    165.         {
    166.             thrusters[6].ApplyThrust();
    167.             thrusters[7].ApplyThrust();
    168.  
    169.             thrusters[8].ApplyThrust();
    170.             thrusters[9].ApplyThrust();
    171.         }
    172.     }
    173.  
    174.  
    175. }
     

    Attached Files:

  2. MV10

    MV10

    Joined:
    Nov 6, 2015
    Posts:
    1,889
    BradMick and Kiwasi like this.
  3. BradMick

    BradMick

    Joined:
    Apr 2, 2014
    Posts:
    113
    Sweet! Okay...I successfully implemented the PID framework, and it's more or less doing it's job...albeit not well. That's more the fault of my rudimentary understanding of what's going on. So with that I have some questions:

    1. Because my thrusters fire off a fixed amount of thrust how do I limit the output of the error to a range of 0 to 1. The idea being that when the error is significantly large it'll fire off the full thrust of the thruster and then as the error decreases, decrease the thrust to slow the ship as it approaches the desired heading.

    2. Why is it that every time I apply a yaw angle do I develop a pitch error? If you watch the output, apply a yaw force using A or D and you'll see the thrust down/up flags light off...which is odd because there isn't an angle for it to aim for.

    3. Am I even using this PID concept correctly? The big differences I see between your example and mine is that I'm using actual thrusters to move the ship which are being supplied a fire command using the error.

    Here's my cobbled together code...

    And yup. Confirmed there is definitely an interaction between the two...which there shouldn't be, especially not when i'm making an on axis heading adjustment, i.e. pitching down, yawing left/right. I think once I figure out how best to manage the thruster we'll be in good shape.

    Code!

    Code (CSharp):
    1. using UnityEngine;
    2. using System.Collections;
    3.  
    4. public class FlightTest : MonoBehaviour
    5. {
    6.     private GameObject  ParentObject = null;
    7.     private Rigidbody   RB = null;
    8.  
    9.     private GameObject  KeyboardObject = null;
    10.     private Keyboard    KB = null;
    11.  
    12.     public  GameObject      maneuverJetRootObject;
    13.     private ManeuverJetRoot maneuverJetRoot;
    14.  
    15.     private GameObject[] maneuverJets;
    16.  
    17.     private Thruster[]  thrusters;
    18.  
    19.     private float normalizedAngularVelocityX, normalizedAngularVelocityY, normalizedAngularVelocityZ;
    20.  
    21.     //PID Test Stuff...
    22.     float Kp = 1.0f;
    23.     float Ki = 0.0f;
    24.     float Kd = 0.1f;
    25.  
    26.     float P, I, D;
    27.     float prevError;
    28.  
    29.     private float targetAngleX;
    30.     private float targetAngleY;
    31.     private float targetAngleZ;
    32.  
    33.     void Start()
    34.     {
    35.         ParentObject = gameObject.transform.root.gameObject;
    36.         RB = ParentObject.GetComponent<Rigidbody>();
    37.  
    38.         KeyboardObject = gameObject.transform.root.gameObject;
    39.         KB = KeyboardObject.GetComponent<Keyboard>();
    40.  
    41.         //Assign the manuevering jet root
    42.         maneuverJetRoot = maneuverJetRootObject.GetComponent<ManeuverJetRoot>();
    43.         //Next, copy all of the maneuvering jets from the root
    44.         maneuverJets = maneuverJetRoot.GetManeuveringJets();
    45.         //Initialize the thruster array
    46.         thrusters = new Thruster[maneuverJets.Length];
    47.         //Copy the thrusters from the root
    48.         for (int i = 0; i < maneuverJets.Length; i++)
    49.         {
    50.             thrusters[i] = maneuverJets[i].GetComponent<Thruster>();
    51.         }
    52.  
    53.         //PID Stuff...
    54.         targetAngleX = transform.eulerAngles.x;
    55.         targetAngleY = transform.eulerAngles.y;
    56.         targetAngleZ = transform.eulerAngles.z;
    57.     }
    58.  
    59.     void Update()
    60.     {
    61.         //Get the angular velocity in local format
    62.         Vector3 localAngularVelocity = ParentObject.transform.InverseTransformDirection(RB.angularVelocity);
    63.  
    64.         if (KB.AKeyState() == true)
    65.             targetAngleY -= 120.0f * Time.fixedDeltaTime;
    66.  
    67.         if (KB.DKeyState() == true)
    68.             targetAngleY += 120.0f * Time.fixedDeltaTime;
    69.  
    70.         if (KB.WKeyState() == true)
    71.             targetAngleX += 120.0f * Time.fixedDeltaTime;
    72.  
    73.         if (KB.SKeyState() == true)
    74.             targetAngleX -= 120.0f * Time.fixedDeltaTime;
    75.  
    76.         float angleErrorX = Mathf.DeltaAngle(transform.eulerAngles.x, targetAngleX);
    77.         float angleErrorY = Mathf.DeltaAngle(transform.eulerAngles.y, targetAngleY);
    78.  
    79.         float angularErrorCorrectionX = GetOutput(angleErrorX, Time.fixedDeltaTime);
    80.         float angularErrorCorrectionY = GetOutput(angleErrorY, Time.fixedDeltaTime);
    81.  
    82.         if (angularErrorCorrectionY < 0)
    83.         {
    84.             Debug.Log("Thrust Left!");
    85.             thrusters[0].ApplyThrust();
    86.             thrusters[3].ApplyThrust();
    87.         }
    88.  
    89.         if (angularErrorCorrectionY > 0)
    90.         {
    91.             Debug.Log("Thrust right!");
    92.             thrusters[1].ApplyThrust();
    93.             thrusters[2].ApplyThrust();
    94.         }
    95.  
    96.         if (angularErrorCorrectionX > 0)
    97.         {
    98.             Debug.Log("Thrust Down!");
    99.             thrusters[4].ApplyThrust();
    100.             thrusters[5].ApplyThrust();
    101.  
    102.             thrusters[10].ApplyThrust();
    103.             thrusters[11].ApplyThrust();
    104.         }
    105.  
    106.         if (angularErrorCorrectionX < 0)
    107.         {
    108.             Debug.Log("Thrust Up!");
    109.             thrusters[6].ApplyThrust();
    110.             thrusters[7].ApplyThrust();
    111.  
    112.             thrusters[8].ApplyThrust();
    113.             thrusters[9].ApplyThrust();
    114.         }
    115.  
    116.         Debug.Log("Target Angle X: " + targetAngleX + " Angle Error X: " + angleErrorX + " Angular Vel Correction X: " + angularErrorCorrectionX);
    117.         Debug.Log("Target Angle Y: " + targetAngleY + " Angle Error Y: " + angleErrorY + " Angular Vel Correction Y: " + angularErrorCorrectionY);
    118.         Debug.Log(new Vector3(targetAngleX, targetAngleY, targetAngleZ));
    119.         /*
    120.         Vector3 localAngularVelocity = ParentObject.transform.InverseTransformDirection(RB.angularVelocity);
    121.  
    122.         normalizedAngularVelocityX = localAngularVelocity.x / 7.0f;
    123.         normalizedAngularVelocityY = localAngularVelocity.y / 7.0f;
    124.         normalizedAngularVelocityZ = localAngularVelocity.z / 7.0f;
    125.  
    126.         Debug.Log("x: " + normalizedAngularVelocityX + " y: " + normalizedAngularVelocityY + " z: " + normalizedAngularVelocityZ);
    127.  
    128.         //Pitch Control
    129.         PitchControl();
    130.         NullXRotation();
    131.         //Yaw Control
    132.         YawControl();
    133.         NullYRotation();
    134.         //Roll Control
    135.         RollControl();
    136.         NullZRotation();
    137.         */
    138.     }
    139.  
    140.     private float GetOutput(float currentError, float deltaTime)
    141.     {
    142.         P = currentError;
    143.         I += P * deltaTime;
    144.         D = (P - prevError) / deltaTime;
    145.         prevError = currentError;
    146.  
    147.         return P * Kp + I * Ki + D * Kd;
    148.     }
    149.  
    150.     /*private void NullXRotation()
    151.     {
    152.         //Stop Pitch Down by activating the thrusters on the opposite side of the ship...
    153.         if (normalizedAngularVelocityX > 0.0f && KB.WKeyState() != true)
    154.         {
    155.             Debug.Log("Stop Pitching Down!");
    156.             if (normalizedAngularVelocityX > 0.0f)
    157.             {
    158.                 thrusters[6].ApplyThrust();
    159.                 thrusters[7].ApplyThrust();
    160.  
    161.                 thrusters[8].ApplyThrust();
    162.                 thrusters[9].ApplyThrust();
    163.             }
    164.  
    165.             if (normalizedAngularVelocityX < 0.001f)
    166.             {
    167.                 RB.angularVelocity = new Vector3(0.0f, normalizedAngularVelocityY, normalizedAngularVelocityZ);
    168.             }
    169.         }
    170.         //Stop Pitch Up by activating the thrusters on the opposite side of the ship...
    171.         if (normalizedAngularVelocityX < 0.0f && KB.SKeyState() != true)
    172.         {
    173.             Debug.Log("Stop Pitching Up!");
    174.             if (normalizedAngularVelocityX < 0.0f)
    175.             {
    176.                 thrusters[4].ApplyThrust();
    177.                 thrusters[5].ApplyThrust();
    178.  
    179.                 thrusters[10].ApplyThrust();
    180.                 thrusters[11].ApplyThrust();
    181.             }
    182.  
    183.             if (normalizedAngularVelocityX > -0.001f)
    184.             {
    185.                 RB.angularVelocity = new Vector3(0.0f, normalizedAngularVelocityY, normalizedAngularVelocityZ);
    186.             }
    187.         }
    188.     }
    189.  
    190.     private void NullYRotation()
    191.     {
    192.         //Stop Left Yaw by activating the thrusters on the opposite side of the ship...
    193.         if (normalizedAngularVelocityY < 0.0f && KB.AKeyState() != true)
    194.         {
    195.             Debug.Log("Stop Yawing Left!");
    196.             if (normalizedAngularVelocityY < 0.0f)
    197.             {
    198.                 thrusters[1].ApplyThrust();
    199.                 thrusters[2].ApplyThrust();
    200.             }
    201.  
    202.             if (normalizedAngularVelocityY > -0.001f)
    203.             {
    204.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, 0.0f, normalizedAngularVelocityZ);
    205.             }
    206.         }
    207.         //Stop Right Yaw by activating the thrusters on the opposite side of the ship...
    208.         if (normalizedAngularVelocityY > 0.0f && KB.DKeyState() != true)
    209.         {
    210.             Debug.Log("Stop Yawing Right!");
    211.             if (normalizedAngularVelocityY > 0.0f)
    212.             {
    213.                 thrusters[0].ApplyThrust();
    214.                 thrusters[3].ApplyThrust();
    215.             }
    216.  
    217.             if (normalizedAngularVelocityY < 0.001f)
    218.             {
    219.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, 0.0f, normalizedAngularVelocityZ);
    220.             }
    221.         }
    222.     }
    223.  
    224.     private void NullZRotation()
    225.     {
    226.         //Stop Left Roll by activating the thrusters on the opposite side of the ship...
    227.         if (normalizedAngularVelocityZ > 0.0f && KB.QKeyState() != true)
    228.         {
    229.             Debug.Log("Stop Rolling Left!");
    230.             if (normalizedAngularVelocityZ > 0.0f)
    231.             {
    232.                 thrusters[12].ApplyThrust();
    233.                 thrusters[15].ApplyThrust();
    234.             }
    235.  
    236.             if (normalizedAngularVelocityZ < 0.001f)
    237.             {
    238.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, normalizedAngularVelocityY, 0.0f);
    239.             }
    240.         }
    241.         //Stop Right Roll by activating the thrusters on the opposite side of the ship...
    242.         if (normalizedAngularVelocityZ < 0.0f && KB.EKeyState() != true)
    243.         {
    244.             Debug.Log("Stop Rolling Right!");
    245.             if (normalizedAngularVelocityZ < 0.0f)
    246.             {
    247.                 thrusters[13].ApplyThrust();
    248.                 thrusters[14].ApplyThrust();
    249.             }
    250.  
    251.             if (normalizedAngularVelocityZ > -0.001f)
    252.             {
    253.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, normalizedAngularVelocityY, 0.0f);
    254.             }
    255.         }
    256.     }
    257.  
    258.     private void YawControl()
    259.     {
    260.         //Yaw Left
    261.         if (KB.AKeyState() == true)
    262.         {
    263.             thrusters[0].ApplyThrust();
    264.             thrusters[3].ApplyThrust();
    265.         }
    266.         //Yaw Right
    267.         if (KB.DKeyState() == true)
    268.         {
    269.             thrusters[1].ApplyThrust();
    270.             thrusters[2].ApplyThrust();
    271.         }
    272.     }
    273.  
    274.     private void PitchControl()
    275.     {
    276.         //Pitch down
    277.         if (KB.WKeyState() == true)
    278.         {
    279.             thrusters[4].ApplyThrust();
    280.             thrusters[5].ApplyThrust();
    281.  
    282.             thrusters[10].ApplyThrust();
    283.             thrusters[11].ApplyThrust();
    284.         }
    285.         //Pitch Up
    286.         if (KB.SKeyState() == true)
    287.         {
    288.             thrusters[6].ApplyThrust();
    289.             thrusters[7].ApplyThrust();
    290.  
    291.             thrusters[8].ApplyThrust();
    292.             thrusters[9].ApplyThrust();
    293.         }
    294.     }
    295.  
    296.     private void RollControl()
    297.     {
    298.         //Roll Left
    299.         if(KB.QKeyState() == true)
    300.         {
    301.             thrusters[13].ApplyThrust();
    302.             thrusters[14].ApplyThrust();
    303.         }
    304.         //Roll Right
    305.         if (KB.EKeyState() == true)
    306.         {
    307.             thrusters[12].ApplyThrust();
    308.             thrusters[15].ApplyThrust();
    309.         }
    310.     }*/
    311. }
     
  4. GroZZleR

    GroZZleR

    Joined:
    Feb 1, 2015
    Posts:
    3,201
    You'll probably want 2 PID controllers - one that steers towards the target and one that counter-steers to stop the rotations entirely. The error for the second controller would be trying to bring the angular velocity to zero (angularVelocity * -1f). I do this in my game and it works swimmingly (I actually use 6 different controllers per ship).

    The only issue I see is that your thrusters appear to be either full on or full off so you may end up with some wobbling.
     
    BradMick likes this.
  5. Kiwasi

    Kiwasi

    Joined:
    Dec 5, 2013
    Posts:
    16,860
    PID definitely works best when you can modulate the output. Technically it will work with bang-bang control, but the results are never as pretty.

    Multiple PID controllers can be fun. My current project has seven to control a single system, and number eight should be going in in a couple of months. A couple of them are set up to cascade (ie feed their outputs into the set points of other PID controllers).

    @BradMick: I would suggest a few changes to your PID implementation.
    • Set it up as a separate class for easy reuse
    • Clamp the output (0-1 is traditional, but you can use other scales)
    • Clamp the value of I to prevent windup
    • Consider filtering the derivative term. This is not always needed, but can improve stability in high noise situation
     
    BradMick and MV10 like this.
  6. BradMick

    BradMick

    Joined:
    Apr 2, 2014
    Posts:
    113
    I think I'm starting to understand this a little better...But, because I'm trying to understand why I'm getting certain results...Math!

    Alrighty, here is my experimental setup:

    I have 2 thrusters oriented such that when I press the 'W' key the pitches down.

    The thrusters are located at 0m, 0.5m, 2.5m and 0m, -0.5m, -2.5m

    Each thruster puts out 100 newtons of thrust.

    I assume that since a Box Collider drives mass distribution that a rectangular prism is what Unity is using to generate the Moment of Inertia for the Rigid Body and thus use the formula: 1/12 * m * (h^2 + w^2) where m = mass (kg), h = box height and w = box length (for an object rotating about the Z axis).

    My box collider is 1m x 1m x 5m.

    When I run the numbers for the thrusters using the Cross Product I get a net torque of 500 N m.

    Using the formula for the rectangular prism I get a moment of inertia of 21.4583 (if I assume a uniform rod of length 5m I get 20.8333) kg m^2.

    To find the acceleration I start with T = Ia, where T = torque, I = Moment of inertia and a = angular acceleration. Rearranging to solve for acceleration I get: a = T/I.

    My resultant angular acceleration then is 500 N m / 21.4583 kg m^2 = -23.3010 rad/s.

    If I thrust for a period of 0.09999999s (unity won't i, I should have a velocity of 2.330097 rad/s ( w = w0 + a * t).

    I go through ALL of that (please check me...I'm no super math wizard) to get to this:

    Unity gets me to 3...

    ------------

    Okay...scratch all of that. I'm dumb. I realized I had passed the time as an argument which was then adjusting my thrust and thus screwing up my results. I'm within the ball park. My excel math nets me a velocity of 2.330097 using the prism and 2.39999976 using the rod. Unity reports the angular velocity as 2.307692...i'd say that's pretty dog gone close. I think the prism is the closest of the two as well. I'm leaving everything above (I was testing/writing as I went) for anyone else who's struggled with understanding this...and also to be proven wrong, because these are nothing but assumptions on my part.

    So, the real point of all of that was this:

    I think it's going to be important to know the max torque that can be generated by a thruster so the system can say "i need a rotational velocity of 'x' which means I need to generate a torque of 'y', now fire the thrusters at this % of thrust to make that happen and using the PID controller to make that happen.

    I got the idea after reading:

    https://robertsspaceindustries.com/comm-link/engineering/13951-Flight-Model-And-Input-Controls

    I'm not trying to go that insane, just want the system to be able to stop velocities on an axis using the thrusters it has available.

    Anyway, Here's the code I used to test my rig. If you create a box collider with similar scale, and then use apply force at position for the thrusters.

    Code (CSharp):
    1. using UnityEngine;
    2. using System.Collections;
    3.  
    4. public class FlightTest : MonoBehaviour
    5. {
    6.     private GameObject  ParentObject = null;
    7.     private Rigidbody   RB = null;
    8.  
    9.     private GameObject  KeyboardObject = null;
    10.     private Keyboard    KB = null;
    11.  
    12.     public  GameObject      maneuverJetRootObject;
    13.     private ManeuverJetRoot maneuverJetRoot;
    14.  
    15.     private GameObject[] maneuverJets;
    16.  
    17.     private Thruster[]  thrusters;
    18.  
    19.     private float normalizedAngularVelocityX, normalizedAngularVelocityY, normalizedAngularVelocityZ;
    20.  
    21.     //PID Test Stuff...
    22.     float Kp = 1.0f;
    23.     float Ki = 0.0f;
    24.     float Kd = 0.1f;
    25.  
    26.     float P, I, D;
    27.     float prevError;
    28.  
    29.     private float targetAngleX;
    30.     private float targetAngleY;
    31.     private float targetAngleZ;
    32.  
    33.     float timeElapsed;
    34.  
    35.     void Start()
    36.     {
    37.         ParentObject = gameObject.transform.root.gameObject;
    38.         RB = ParentObject.GetComponent<Rigidbody>();
    39.  
    40.         KeyboardObject = gameObject.transform.root.gameObject;
    41.         KB = KeyboardObject.GetComponent<Keyboard>();
    42.  
    43.         //Assign the manuevering jet root
    44.         maneuverJetRoot = maneuverJetRootObject.GetComponent<ManeuverJetRoot>();
    45.         //Next, copy all of the maneuvering jets from the root
    46.         maneuverJets = maneuverJetRoot.GetManeuveringJets();
    47.         //Initialize the thruster array
    48.         thrusters = new Thruster[maneuverJets.Length];
    49.         //Copy the thrusters from the root
    50.         for (int i = 0; i < maneuverJets.Length; i++)
    51.         {
    52.             thrusters[i] = maneuverJets[i].GetComponent<Thruster>();
    53.         }
    54.  
    55.         //PID Stuff...
    56.         targetAngleX = transform.localEulerAngles.x;
    57.         targetAngleY = transform.localEulerAngles.y;
    58.         targetAngleZ = transform.localEulerAngles.z;
    59.     }
    60.  
    61.     void FixedUpdate()
    62.     {
    63.         timeElapsed += Time.fixedDeltaTime;
    64.  
    65.         if (timeElapsed < 0.1f)
    66.         {
    67.             thrusters[0].ApplyThrust();
    68.             thrusters[1].ApplyThrust();
    69.  
    70.             Debug.Log(timeElapsed);
    71.         }
    72.  
    73.         Debug.Log("Angular Velocity: " + RB.angularVelocity.x);
    74.  
    75.         if (Input.GetKeyDown(KeyCode.W))
    76.         {
    77.             thrusters[0].ApplyThrust(timeElapsed);
    78.             thrusters[1].ApplyThrust(timeElapsed);
    79.         }
    80.  
    81.  
    82.  
    83.         //Get the angular velocity in local format
    84.         /*Vector3 localAngularVelocity = ParentObject.transform.InverseTransformDirection(RB.angularVelocity);
    85.  
    86.         if (KB.AKeyState() == true)
    87.             targetAngleY -= 120.0f * Time.fixedDeltaTime;
    88.  
    89.         if (KB.DKeyState() == true)
    90.             targetAngleY += 120.0f * Time.fixedDeltaTime;
    91.  
    92.         if (KB.WKeyState() == true)
    93.             targetAngleX += 120.0f * Time.fixedDeltaTime;
    94.  
    95.         if (KB.SKeyState() == true)
    96.             targetAngleX -= 120.0f * Time.fixedDeltaTime;
    97.  
    98.         float angleErrorX = Mathf.DeltaAngle(transform.eulerAngles.x, targetAngleX);
    99.         float angleErrorY = Mathf.DeltaAngle(transform.eulerAngles.y, targetAngleY);
    100.  
    101.         float angularErrorCorrectionX = GetOutput(angleErrorX, Time.fixedDeltaTime);
    102.         float angularErrorCorrectionY = GetOutput(angleErrorY, Time.fixedDeltaTime);
    103.  
    104.         if (angularErrorCorrectionY < 0)
    105.         {
    106.             float thrustScalar = 0.0f;
    107.  
    108.             if (angularErrorCorrectionY > -5)
    109.                 thrustScalar = 0.1f;
    110.             if (angularErrorCorrectionY >= -5 || angleErrorY < -20)
    111.                 thrustScalar = 0.5f;
    112.             if (angularErrorCorrectionY >= -20)
    113.                 thrustScalar = 1.0f;
    114.  
    115.             Debug.Log("Thrust Left!");
    116.             thrusters[0].ApplyThrust(thrustScalar);
    117.             thrusters[3].ApplyThrust(thrustScalar);
    118.         }
    119.  
    120.         if (angularErrorCorrectionY > 0)
    121.         {
    122.             float thrustScalar = 0.0f;
    123.  
    124.             if (angularErrorCorrectionY < 5)
    125.                 thrustScalar = 0.1f;
    126.             if (angularErrorCorrectionY >= 5 || angleErrorY < 20)
    127.                 thrustScalar = 0.5f;
    128.             if (angularErrorCorrectionY >= 20)
    129.                 thrustScalar = 1.0f;
    130.  
    131.             Debug.Log("Thrust right!");
    132.             thrusters[1].ApplyThrust(thrustScalar);
    133.             thrusters[2].ApplyThrust(thrustScalar);
    134.         }*/
    135.  
    136.         /*if (angularErrorCorrectionX > 0)
    137.         {
    138.             Debug.Log("Thrust Down!");
    139.             thrusters[4].ApplyThrust();
    140.             thrusters[5].ApplyThrust();
    141.  
    142.             thrusters[10].ApplyThrust();
    143.             thrusters[11].ApplyThrust();
    144.         }
    145.  
    146.         if (angularErrorCorrectionX < 0)
    147.         {
    148.             Debug.Log("Thrust Up!");
    149.             thrusters[6].ApplyThrust();
    150.             thrusters[7].ApplyThrust();
    151.  
    152.             thrusters[8].ApplyThrust();
    153.             thrusters[9].ApplyThrust();
    154.         }
    155.  
    156.         Debug.Log("Target Angle X: " + targetAngleX + " Angle Error X: " + angleErrorX + " Angular Vel Correction X: " + angularErrorCorrectionX);
    157.         Debug.Log("Target Angle Y: " + targetAngleY + " Angle Error Y: " + angleErrorY + " Angular Vel Correction Y: " + angularErrorCorrectionY);
    158.         Debug.Log(new Vector3(targetAngleX, targetAngleY, targetAngleZ));
    159.        
    160.         Vector3 localAngularVelocity = ParentObject.transform.InverseTransformDirection(RB.angularVelocity);
    161.  
    162.         normalizedAngularVelocityX = localAngularVelocity.x / 7.0f;
    163.         normalizedAngularVelocityY = localAngularVelocity.y / 7.0f;
    164.         normalizedAngularVelocityZ = localAngularVelocity.z / 7.0f;
    165.  
    166.         Debug.Log("x: " + normalizedAngularVelocityX + " y: " + normalizedAngularVelocityY + " z: " + normalizedAngularVelocityZ);
    167.  
    168.         //Pitch Control
    169.         PitchControl();
    170.         NullXRotation();
    171.         //Yaw Control
    172.         YawControl();
    173.         NullYRotation();
    174.         //Roll Control
    175.         RollControl();
    176.         NullZRotation();
    177.         */
    178.     }
    179.  
    180.     private float GetOutput(float currentError, float deltaTime)
    181.     {
    182.         P = currentError;
    183.         I += P * deltaTime;
    184.         D = (P - prevError) / deltaTime;
    185.         prevError = currentError;
    186.  
    187.         return P * Kp + I * Ki + D * Kd;
    188.     }
    189.  
    190.     /*private void NullXRotation()
    191.     {
    192.         //Stop Pitch Down by activating the thrusters on the opposite side of the ship...
    193.         if (normalizedAngularVelocityX > 0.0f && KB.WKeyState() != true)
    194.         {
    195.             Debug.Log("Stop Pitching Down!");
    196.             if (normalizedAngularVelocityX > 0.0f)
    197.             {
    198.                 thrusters[6].ApplyThrust();
    199.                 thrusters[7].ApplyThrust();
    200.  
    201.                 thrusters[8].ApplyThrust();
    202.                 thrusters[9].ApplyThrust();
    203.             }
    204.  
    205.             if (normalizedAngularVelocityX < 0.001f)
    206.             {
    207.                 RB.angularVelocity = new Vector3(0.0f, normalizedAngularVelocityY, normalizedAngularVelocityZ);
    208.             }
    209.         }
    210.         //Stop Pitch Up by activating the thrusters on the opposite side of the ship...
    211.         if (normalizedAngularVelocityX < 0.0f && KB.SKeyState() != true)
    212.         {
    213.             Debug.Log("Stop Pitching Up!");
    214.             if (normalizedAngularVelocityX < 0.0f)
    215.             {
    216.                 thrusters[4].ApplyThrust();
    217.                 thrusters[5].ApplyThrust();
    218.  
    219.                 thrusters[10].ApplyThrust();
    220.                 thrusters[11].ApplyThrust();
    221.             }
    222.  
    223.             if (normalizedAngularVelocityX > -0.001f)
    224.             {
    225.                 RB.angularVelocity = new Vector3(0.0f, normalizedAngularVelocityY, normalizedAngularVelocityZ);
    226.             }
    227.         }
    228.     }
    229.  
    230.     private void NullYRotation()
    231.     {
    232.         //Stop Left Yaw by activating the thrusters on the opposite side of the ship...
    233.         if (normalizedAngularVelocityY < 0.0f && KB.AKeyState() != true)
    234.         {
    235.             Debug.Log("Stop Yawing Left!");
    236.             if (normalizedAngularVelocityY < 0.0f)
    237.             {
    238.                 thrusters[1].ApplyThrust();
    239.                 thrusters[2].ApplyThrust();
    240.             }
    241.  
    242.             if (normalizedAngularVelocityY > -0.001f)
    243.             {
    244.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, 0.0f, normalizedAngularVelocityZ);
    245.             }
    246.         }
    247.         //Stop Right Yaw by activating the thrusters on the opposite side of the ship...
    248.         if (normalizedAngularVelocityY > 0.0f && KB.DKeyState() != true)
    249.         {
    250.             Debug.Log("Stop Yawing Right!");
    251.             if (normalizedAngularVelocityY > 0.0f)
    252.             {
    253.                 thrusters[0].ApplyThrust();
    254.                 thrusters[3].ApplyThrust();
    255.             }
    256.  
    257.             if (normalizedAngularVelocityY < 0.001f)
    258.             {
    259.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, 0.0f, normalizedAngularVelocityZ);
    260.             }
    261.         }
    262.     }
    263.  
    264.     private void NullZRotation()
    265.     {
    266.         //Stop Left Roll by activating the thrusters on the opposite side of the ship...
    267.         if (normalizedAngularVelocityZ > 0.0f && KB.QKeyState() != true)
    268.         {
    269.             Debug.Log("Stop Rolling Left!");
    270.             if (normalizedAngularVelocityZ > 0.0f)
    271.             {
    272.                 thrusters[12].ApplyThrust();
    273.                 thrusters[15].ApplyThrust();
    274.             }
    275.  
    276.             if (normalizedAngularVelocityZ < 0.001f)
    277.             {
    278.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, normalizedAngularVelocityY, 0.0f);
    279.             }
    280.         }
    281.         //Stop Right Roll by activating the thrusters on the opposite side of the ship...
    282.         if (normalizedAngularVelocityZ < 0.0f && KB.EKeyState() != true)
    283.         {
    284.             Debug.Log("Stop Rolling Right!");
    285.             if (normalizedAngularVelocityZ < 0.0f)
    286.             {
    287.                 thrusters[13].ApplyThrust();
    288.                 thrusters[14].ApplyThrust();
    289.             }
    290.  
    291.             if (normalizedAngularVelocityZ > -0.001f)
    292.             {
    293.                 RB.angularVelocity = new Vector3(normalizedAngularVelocityX, normalizedAngularVelocityY, 0.0f);
    294.             }
    295.         }
    296.     }
    297.  
    298.     private void YawControl()
    299.     {
    300.         //Yaw Left
    301.         if (KB.AKeyState() == true)
    302.         {
    303.             thrusters[0].ApplyThrust();
    304.             thrusters[3].ApplyThrust();
    305.         }
    306.         //Yaw Right
    307.         if (KB.DKeyState() == true)
    308.         {
    309.             thrusters[1].ApplyThrust();
    310.             thrusters[2].ApplyThrust();
    311.         }
    312.     }
    313.  
    314.     private void PitchControl()
    315.     {
    316.         //Pitch down
    317.         if (KB.WKeyState() == true)
    318.         {
    319.             thrusters[4].ApplyThrust();
    320.             thrusters[5].ApplyThrust();
    321.  
    322.             thrusters[10].ApplyThrust();
    323.             thrusters[11].ApplyThrust();
    324.         }
    325.         //Pitch Up
    326.         if (KB.SKeyState() == true)
    327.         {
    328.             thrusters[6].ApplyThrust();
    329.             thrusters[7].ApplyThrust();
    330.  
    331.             thrusters[8].ApplyThrust();
    332.             thrusters[9].ApplyThrust();
    333.         }
    334.     }
    335.  
    336.     private void RollControl()
    337.     {
    338.         //Roll Left
    339.         if(KB.QKeyState() == true)
    340.         {
    341.             thrusters[13].ApplyThrust();
    342.             thrusters[14].ApplyThrust();
    343.         }
    344.         //Roll Right
    345.         if (KB.EKeyState() == true)
    346.         {
    347.             thrusters[12].ApplyThrust();
    348.             thrusters[15].ApplyThrust();
    349.         }
    350.     }*/
    351. }
     
  7. Kiwasi

    Kiwasi

    Joined:
    Dec 5, 2013
    Posts:
    16,860
    PID controllers are designed to work with unknown models. So you don't actually need to know the relationship between velocity, tourtière and thrust. I mention this mainly because it will save you recalculating every time you adjust paramatwes on your vehicle.
     
    BradMick likes this.
  8. BradMick

    BradMick

    Joined:
    Apr 2, 2014
    Posts:
    113
    Hmm...then I'm definitely not grasping the concept. I have thrusters with a set thrust power that can be throttled. I have a rough correction setup working at the moment, it just wobbles back and forth never stopping itself. I know the poor responsiveness is because it's waiting for a conditional to be true before lighting off the opposite thrusters.

    What I think I understand is that the PID is dimensionless based on your most recent post..and from re-reading the other posts a bunch of times. The trick is to map it into a range that's useable.

    So, what I ultimately have to do is fire both sets of thrusters at the same time. The sequence is something like:

    The pilot holds the stick left for 1 second. The flight control computer has been given an arbitrary limit of 50 degrees per second (mainly to keep the rotation within what the thrusters are capable of comfortably handing) which means I need an acceleration of 0.7854 rad/s. Thruster 0 and 1 (right front and left rear) fire to start the rotation, at the same time thrusters 2 and 3 fire to start arresting the rate (albeit at a very low level). As we near our desired heading of 50 degrees thrusters 0 and 1 reduce thrust and thrusters 2 and 3 increase thrust ultimately cancelling the velocity out at a head of 050 degrees...all of this happens within 1 second.

    Logically I know one set of thrusters (0/1) are going to fire for 0.5 seconds and then the other set (2/3) is going to fire for 0.5 seconds cancelling out the initial acceleration/velocity to bring us to a stop.

    How do I make the PID work within the above?
     
  9. MV10

    MV10

    Joined:
    Nov 6, 2015
    Posts:
    1,889
    PID just applies corrections based on the amount a measurement differs from a target value. That difference is called the "error" term.

    The Proportional term is an immediate-feedback correction. If the error is large, the correction generated by P will be large.

    The Integral term is an accumulated correction based on errors over time. It is meant to sort of counteract the instantaneous reaction of the Proportional term. The "windup" mentioned by BoredMormon is when this term has been accumulating error data for longer than is useful. In some scenarios it can quickly spiral out of control if you don't limit the corrections it applies. Typically the I term is not that large in all the systems I've ever seen (primarily controlling drones and robotics).

    The Derivative term is meant to dampen the corrections as the errors fall off (the measurement is getting closer to the target value). In effect it's trying to predict future errors. It should prevent corrections from overshooting the target value.

    Each of those is combined into a single correction. You feed that into your control system, measure again, calculate the error, and do it all over again.

    This is what BoredMormon meant about PIDs working with unknown models. It really doesn't matter what the PID is controlling, they all work this way. You feed it a measurement and an error, and it spits out a correction. You "tune" the PID by assigning reasonable starting values, tweaking how aggressively each term reacts, and how you handle clamping of the values (most especially Integral windup).

    If you search the Googles, there's quite a bit out there about tuning PIDs -- you typically do one thing if you're seeing oscillations, other things if everything seems sluggish, and so on. In Unity I find it really helps to build little test projects with tons of visual cues with Debug lines and such.
     
    Last edited: Dec 15, 2016
    BradMick, Braineeee and Kiwasi like this.
  10. Kiwasi

    Kiwasi

    Joined:
    Dec 5, 2013
    Posts:
    16,860
    P is for Proportional, just in case anyone is googling it.
     
    BradMick and MV10 like this.
  11. MV10

    MV10

    Joined:
    Nov 6, 2015
    Posts:
    1,889
    Oops.
     
  12. GroZZleR

    GroZZleR

    Joined:
    Feb 1, 2015
    Posts:
    3,201
    Have two PIDs, as I suggested earlier. When you initially want to turn, the error on the rotation will be larger than the counter-rotation. As you near your target, the counter-rotation error will be larger and will begin to overtake the rotational PID. If you tune them properly, you shouldn't have much if any overshoot.
     
    BradMick likes this.
  13. BradMick

    BradMick

    Joined:
    Apr 2, 2014
    Posts:
    113
    Finally! After re-reading all of ya'lls posts which were chock full of useful information I was to tired/frustrated/dense to pick up on initially I figured out what ya'll were getting at. So now I have a working PID implementation.

    Here's what I did:

    To simplify my life, I decided I'd just specify a maximum angular velocity the ship could attain in the form of a degree's per second value after using info from:

    https://forum.unity3d.com/threads/spaceship-control-using-pid-controllers.191755/

    What that did was greatly simplify everything. i don't have to calculate torques, forces, nothin'. All I have to do now is tell the correct sequence of thrusters to fire based on the output of the PID. To that end I used 3 PID's total and clamped the values between -1 and 1, which is the throttle range of the thrusters. I also had to work through some issues with the error values. For my X and Z axis values I actually had to do the pitch rate + angular velocity to avoid having to flip controller axes.

    The end result is that the system now attempts to get to the specified angular velocity as is defined by the ship designer.

    Here's the test code, I'm going to start cutting it apart and modularizing things but I wanted to share the completed and working solution as I have it now. Thanks MV10, GroZZler and BoreMormon! Ya'll were super helpful! Also, if you guys have any further insights into how I can better optimize/tweak the PID please let me know! One of the things I look forward to playing with in the future is seeing how damage effects the system...and also the difference between flight assist on and flight assist off...I mean, I know more or less, but it'll be interesting. Oh! yeah, that's what I was wondering...How might I get this to be less crisp as right now it's super crisp...almost to crisp. Although, I guess that's okay, the f-16/f-18 et. al. are pretty crisp on their rolls and the like...so maybe that's okay. *shrugs*

    Code!

    Code (CSharp):
    1. using UnityEngine;
    2. using System.Collections;
    3.  
    4. public class PIDTesting : MonoBehaviour
    5. {
    6.     private GameObject      root;
    7.  
    8.     public  GameObject      CoMObject;
    9.     private Rigidbody       RB;
    10.  
    11.     private Keyboard        KB;
    12.     private XBoxController  XB;
    13.  
    14.     private GameObject[]    thrusterObjects;
    15.     private Thruster[]      thrusters;
    16.  
    17.     public float pitchRate  = 45.0f; //deg/sec
    18.     public float yawRate    = 37.5f; //deg/sec
    19.     public float rollRate   = 37.5f; //deg/sec
    20.  
    21.     private ForceResolver   forceResolver;
    22.  
    23.     private Vector2         leftStickAxisInput;
    24.     private Vector2         rightStickAxisInput;
    25.  
    26.     //Testing Stuff
    27.     private float           timeElapsed;
    28.     Vector3                 localAngularVelocity;
    29.  
    30.     void Start()
    31.     {
    32.         root = transform.root.gameObject;
    33.         RB = root.GetComponent<Rigidbody>();
    34.         KB = root.GetComponent<Keyboard>();
    35.         XB = root.GetComponent<XBoxController>();
    36.  
    37.         thrusters = root.GetComponentsInChildren<Thruster>();
    38.         thrusterObjects = new GameObject[thrusters.Length];
    39.  
    40.         for (int i = 0; i < thrusters.Length; i++)
    41.         {
    42.             thrusterObjects[i] = thrusters[i].gameObject;
    43.         }
    44.  
    45.         //forceResolver = root.GetComponent<ForceResolver>();
    46.  
    47.         /*for (int i = 0; i < thrusterObjects.Length; i++)
    48.         {
    49.             Debug.Log(thrusterObjects[i].name + " Axial Thrust: " + thrusters[i].GetAxialThrust());
    50.             Debug.Log(thrusterObjects[i].name + " Torque: " + forceResolver.CalculateTorque(thrusterObjects[i]));
    51.         }*/
    52.     }
    53.  
    54.     void Update()
    55.     {
    56.         leftStickAxisInput = new Vector2(Input.GetAxis("LeftStickXAxis"), Input.GetAxis("LeftStickYAxis"));
    57.         rightStickAxisInput = new Vector2(Input.GetAxis("RightStickXAxis"), Input.GetAxis("RightStickYAxis"));
    58.  
    59.         localAngularVelocity = root.transform.InverseTransformDirection(RB.angularVelocity);
    60.  
    61.         //Debug.Log(rightStickAxisInput.x);
    62.  
    63.         Debug.Log(localAngularVelocity);
    64.     }
    65.  
    66.     private float GetOutput(float currentError, float deltaTime)
    67.     {
    68.         float Kp = 1.0f;
    69.         float Ki = 0.0f;
    70.         float Kd = 0.1f;
    71.  
    72.         float P = 0.0f; float I = 0.0f; float D = 0.0f; float prevError = 0.0f;
    73.  
    74.         P = currentError;
    75.         I += P * deltaTime;
    76.         D = (P - prevError) / deltaTime;
    77.         prevError = currentError;
    78.  
    79.         return P * Kp + I * Ki + D * Kd;
    80.     }
    81.  
    82.     void FixedUpdate()
    83.     {
    84.         float time = Time.fixedDeltaTime;
    85.  
    86.         float pitchError = ((Mathf.Deg2Rad * pitchRate) * leftStickAxisInput.y) + localAngularVelocity.x;
    87.         float yawError   = ((Mathf.Deg2Rad * yawRate) * leftStickAxisInput.x) - localAngularVelocity.y;
    88.         float rollError  = ((Mathf.Deg2Rad * rollRate) * rightStickAxisInput.x) + localAngularVelocity.z;
    89.  
    90.         float outputX = Mathf.Clamp(GetOutput(pitchError, time), -1.0f, 1.0f);
    91.         float outputY = Mathf.Clamp(GetOutput(yawError, time), -1.0f, 1.0f);
    92.         float outputZ = Mathf.Clamp(GetOutput(rollError, time), -1.0f, 1.0f);
    93.  
    94.         Debug.Log("Pitch Error: " + outputX);
    95.         Debug.Log("Yaw Error: " + outputY);
    96.         Debug.Log("Roll Error: " + outputZ);
    97.  
    98.         //Yaw Left
    99.         if (outputY < 0.0f)
    100.         {
    101.             thrusters[2].ApplyThrust(Mathf.Abs(outputY));
    102.             thrusters[3].ApplyThrust(Mathf.Abs(outputY));
    103.         }
    104.         //Yaw Right
    105.         if (outputY > 0.0f)
    106.         {
    107.             thrusters[4].ApplyThrust(Mathf.Abs(outputY));
    108.             thrusters[5].ApplyThrust(Mathf.Abs(outputY));
    109.         }
    110.         //Pitch Down
    111.         if (outputX < 0.0f)
    112.         {
    113.             thrusters[6].ApplyThrust(Mathf.Abs(outputX));
    114.             thrusters[8].ApplyThrust(Mathf.Abs(outputX));
    115.  
    116.             thrusters[7].ApplyThrust(Mathf.Abs(outputX));
    117.             thrusters[9].ApplyThrust(Mathf.Abs(outputX));
    118.         }
    119.         //Pitch Up
    120.         if (outputX > 0.0f)
    121.         {
    122.             thrusters[10].ApplyThrust(Mathf.Abs(outputX));
    123.             thrusters[12].ApplyThrust(Mathf.Abs(outputX));
    124.  
    125.             thrusters[11].ApplyThrust(Mathf.Abs(outputX));
    126.             thrusters[13].ApplyThrust(Mathf.Abs(outputX));
    127.         }
    128.         //Roll Left
    129.         if (outputZ < 0.0f)
    130.         {
    131.             thrusters[9].ApplyThrust(Mathf.Abs(outputZ));
    132.             thrusters[11].ApplyThrust(Mathf.Abs(outputZ));
    133.         }
    134.         //Roll Right
    135.         if (outputZ > 0.0f)
    136.         {
    137.             thrusters[7].ApplyThrust(Mathf.Abs(outputZ));
    138.             thrusters[13].ApplyThrust(Mathf.Abs(outputZ));
    139.         }
    140.     }
    141. }
     
    MV10 and Kiwasi like this.
  14. Kiwasi

    Kiwasi

    Joined:
    Dec 5, 2013
    Posts:
    16,860
    Nice!

    For the crispness play with the values for K, I and D. It's possible to detune a controller.

    Increasing K generally makes a controller respond faster and more dramatically. Reducing K makes it slower.

    Increasing I makes the controller respond faster, while at the same time introducing overshoot. Increase I enough and your system will 'hunt' bouncing around the set point.

    D is similar, increasing D speeds up the response time by allowing the model to be predictive. At the cost of introducing instability.

    You can tune a PID controller to have any sort of behaviour you like. I've got one slow loop that takes 20 minutes to respond to a change, but it's super stable despite a lot of noise in its set point. And I've got fast loops that respond in seconds.
     
    BradMick likes this.
  15. MV10

    MV10

    Joined:
    Nov 6, 2015
    Posts:
    1,889
    A lot of drone stabilization "helper" routines and features like auto-hover works that way. The fast loops are just motor control in response to user input, and slower loops are sort of like "effects" you can turn on or off. For awhile one of the cleanflight profiles had a PID auto-tune mode that was fascinating to watch. You'd get your PIDs to some partly-stable value, fly the thing about 20 feet away and 10 feet up, and flip a switch. It would start twitching on all its axes for 10 or 15 seconds, then just like magic, it flew better. Very cool. (Unfortunately, the better flight algorithms didn't support it.)

    Generically speaking, I've noticed it's also common to only use P by itself, or PI or PD. I've never done it so I'm not sure of the correct use-case, maybe BoredMormon can give us some insight?
     
    BradMick likes this.
  16. Kiwasi

    Kiwasi

    Joined:
    Dec 5, 2013
    Posts:
    16,860
    Now you are testing my knowledge. It's been a while since I did control system theory in school.

    It's probably worth starting with gap control. Gap control (or bang-bang control) is the simplest to implement. You simply turn on if the measured value is slower the. The set point, and off if the value is higher. It's how a thermostat typically works.

    In general P and PI controllers are all special cases of PID controllers.

    P control works by measuring the absolute error and controlling proportional to that. The advantage of a P controller is they are super simple to understand and operate. The disadvantage is it will never hit the actual set point, as it requires an error from the set point in order to activate. The float in a cistern is a good example of P only control.

    I control considers the past. How long has the error existed? It's slower then P control, but will always find the set point. Finding the right temperature in the shower is a good example. You check the temperature, if it's still cold for a few minutes you turn it up. Just like the shower I control can cause overshoot, making too much of it undesirable in some circumstances.

    D control looks at the rate of change in order to predict the future. D anticipates the direction that the error is going, and tries to compensate for it. Its fixing an error before it becomes a problem. Driving is a good example, you don't wait until you are off the road before you make a correction, rather you correct your direction as soon as you start to drift. D tends to make control response faster. It also tends to act as a damper, reducing overshoot. On the downside the D term tends to amplify noise in the signal.

    So a P only controller is used when you don't care that much about accuracy. I want this tank to always have some water in it, but I don't give two hoots about how much water there actually is.

    PI control is used on high noise signals. Say I have a tank with an agitator and a couple of infeeds. There is a lot of splashing in the tank, so I never see a stable level signal. Thus measuring the rate of change in the system is fairly meaningless.

    PD control is most often used in applications where overshoot is expensive or undesirable. Or where the primary goal is to control occupations. pH control comes to mind. Car suspension is another example of PD control.
     
    BradMick, Braineeee and MV10 like this.