Search Unity

  1. Unity support for visionOS is now available. Learn more in our blog post.
    Dismiss Notice

Question Adding force on point of rigid body to hold an expected angle

Discussion in 'Physics' started by ObalonyPivvot, Oct 17, 2023.

  1. ObalonyPivvot

    ObalonyPivvot

    Joined:
    Oct 17, 2023
    Posts:
    5
    Hi. Im trying to do physics based drone simulator and now im on the point of steering. I was trying to manipulate propellers speed which generate force to turn the drone, but it is very wobbly and inefficient.

    Code (CSharp):
    1.  
    2. using System;
    3. using System.Collections;
    4. using System.Collections.Generic;
    5. using UnityEngine;
    6. using UnityEngine.UIElements;
    7.  
    8. public class SimpleDroneController : MonoBehaviour
    9. {
    10.     Rigidbody droneBody;
    11.  
    12.     public int propellersCount = 4;
    13.  
    14.     public float rotationSpeed = 0f;
    15.     public List<float> rotationSpeedList = new List<float>();
    16.  
    17.     public float maxSpeed = 500f;
    18.     public float maxAngle = 30f;
    19.  
    20.     public float speedChangeAmount = 8f;
    21.     public float rotationDelay = 2f;
    22.     public float agleChangeAmount = 0.1f;
    23.  
    24.     public float expectedAngleLR = 0f;
    25.     public float expectedAngleFB = 0f;
    26.  
    27.     public float pitchAngle = 0f;
    28.     public float rollAngle = 0f;
    29.  
    30.     public float wingspan;
    31.     public float wingArea;
    32.     public float areodynamics = 1;
    33.     private float force;
    34.  
    35.     [SerializeField]
    36.     Animator animator;
    37.     bool isOnGround = true;
    38.  
    39.     private void Start()
    40.     {
    41.         droneBody = GetComponent<Rigidbody>();
    42.         droneBody.maxAngularVelocity = maxSpeed;
    43.  
    44.         for(int i = 0; i<propellersCount; i++)
    45.         {
    46.             rotationSpeedList.Add(0f);
    47.         }
    48.  
    49.         animator = GetComponent<Animator>();
    50.     }
    51.  
    52.     private void Update()
    53.     {
    54.         Controlls();
    55.         CalculateForce();
    56.         //transform.localEulerAngles = Vector3.back * right_left_angle + Vector3.right * forward_backward_angle;
    57.     }
    58.  
    59.     private void FixedUpdate()
    60.     {
    61.         //droneBody.AddRelativeForce(right_left_axis, up_down_axis, forward_backward_axis);
    62.     }
    63.  
    64.     void Controlls()
    65.     {
    66.         if (Input.GetKey(KeyCode.Q))
    67.         {
    68.             rotationSpeed += speedChangeAmount;
    69.             rotationSpeed = Math.Min(rotationSpeed, maxSpeed);
    70.             for (int i = 0; i < propellersCount; i++)
    71.             {
    72.                 rotationSpeedList[i] += speedChangeAmount;
    73.                 rotationSpeedList[i] = Math.Min(rotationSpeedList[i], maxSpeed);
    74.             }
    75.  
    76.             animator.SetBool("Fly", true);
    77.             isOnGround = false;
    78.  
    79.         }
    80.         else if (Input.GetKey(KeyCode.E))
    81.         {
    82.             rotationSpeed -= speedChangeAmount;
    83.             rotationSpeed = Math.Max(rotationSpeed, 0);
    84.             for (int i = 0; i < propellersCount; i++)
    85.             {
    86.                 rotationSpeedList[i] -= speedChangeAmount;
    87.                 rotationSpeedList[i] = Math.Max(rotationSpeedList[i], 0);
    88.             }
    89.         }
    90.  
    91.         if (Input.GetKey(KeyCode.W))
    92.         {
    93.             expectedAngleFB += agleChangeAmount;
    94.             expectedAngleFB = (float)Math.Round(expectedAngleFB, 1);
    95.             expectedAngleFB = Math.Min(expectedAngleFB, maxAngle);
    96.         }
    97.         else if (Input.GetKey(KeyCode.S))
    98.         {
    99.             expectedAngleFB -= agleChangeAmount;
    100.             expectedAngleFB = (float)Math.Round(expectedAngleFB, 1);
    101.             expectedAngleFB = Math.Max(expectedAngleFB, -maxAngle);
    102.         }
    103.         /*else
    104.         {
    105.             GoBackToMainSpeed();
    106.         }*/
    107.  
    108.         if (Input.GetKey(KeyCode.A))
    109.         {
    110.             expectedAngleLR -= agleChangeAmount;
    111.             expectedAngleLR = (float)Math.Round(expectedAngleLR, 1);
    112.             expectedAngleLR = Math.Max(expectedAngleLR, -maxAngle);
    113.         }
    114.         else if (Input.GetKey(KeyCode.D))
    115.         {
    116.             expectedAngleLR += agleChangeAmount;
    117.             expectedAngleLR = (float)Math.Round(expectedAngleLR, 1);
    118.             expectedAngleLR = Math.Min(expectedAngleLR, maxAngle);
    119.         }
    120.         /*else
    121.         {
    122.             GoBackToMainSpeed();
    123.         }*/
    124.  
    125.  
    126.         if(Input.GetKey(KeyCode.W) && Input.GetKey(KeyCode.A))
    127.         {
    128.             expectedAngleFB = Math.Min(expectedAngleFB + agleChangeAmount, maxAngle);
    129.             expectedAngleLR = Math.Max(expectedAngleLR - agleChangeAmount, -maxAngle);
    130.         }
    131.          if (Input.GetKey(KeyCode.W) && Input.GetKey(KeyCode.D))
    132.          {
    133.             expectedAngleFB = Math.Min(expectedAngleFB + agleChangeAmount, maxAngle);
    134.             expectedAngleLR = Math.Min(expectedAngleLR + agleChangeAmount, maxAngle);
    135.         }
    136.          if (Input.GetKey(KeyCode.S) && Input.GetKey(KeyCode.A))
    137.          {
    138.             expectedAngleFB = Math.Max(expectedAngleFB - agleChangeAmount, -maxAngle);
    139.             expectedAngleLR = Math.Max(expectedAngleLR - agleChangeAmount, -maxAngle);
    140.         }
    141.          if (Input.GetKey(KeyCode.S) && Input.GetKey(KeyCode.D))
    142.          {
    143.             expectedAngleFB = Math.Max(expectedAngleFB - agleChangeAmount, -maxAngle);
    144.             expectedAngleLR = Math.Min(expectedAngleLR + agleChangeAmount, maxAngle);
    145.         }
    146.  
    147.         AdjustPropellersSpeed();
    148.     }
    149.     private void CalculateForce()
    150.     {
    151.         Quaternion rotationQuaternion = droneBody.rotation;
    152.         for (int i = 0; i < propellersCount; i++)
    153.         {
    154.             int j = i + 1;
    155.             string cubeName = "Cube.00" + j;
    156.             string animatorSpeedName = "Speed" + j;
    157.             Transform cube = droneBody.transform.Find(cubeName);
    158.  
    159.             animator.SetFloat(animatorSpeedName, rotationSpeedList[i]);
    160.             float speed = (float)(2 * Math.PI * wingspan/2 * rotationSpeedList[i]);
    161.  
    162.             force = 0.001225f * wingArea * speed * speed * areodynamics;
    163.             Vector3 localForce = new Vector3(0, force, 0);
    164.             Vector3 globalForce = rotationQuaternion * localForce;
    165.  
    166.             droneBody.AddForceAtPosition(globalForce, cube.position);
    167.         }
    168.        
    169.         //droneBody.useGravity = true;
    170.     }
    171.     public float maxDelayLR = 0;
    172.     public float maxDelayFB = 0;
    173.     private void AdjustPropellersSpeed()
    174.     {
    175.         pitchAngle = (int)Math.Round(droneBody.transform.eulerAngles.x);
    176.         pitchAngle = pitchAngle > 180 ? pitchAngle - 360 : pitchAngle;
    177.         rollAngle = (int)Math.Round(droneBody.transform.eulerAngles.z);
    178.         rollAngle = -1 * (rollAngle > 180 ? rollAngle - 360 : rollAngle);
    179.  
    180.  
    181.         if (isOnGround) return;
    182.         if ((int)Math.Round(expectedAngleFB) > pitchAngle)
    183.         {
    184.            SlowDownRotation(0);
    185.             SlowDownRotation(1);
    186.             SpeedUpRotation(2);
    187.             SpeedUpRotation(3);
    188.         }
    189.         else if ((int)Math.Round(expectedAngleFB) < pitchAngle)
    190.         {
    191.             SlowDownRotation(2);
    192.             SlowDownRotation(3);
    193.             SpeedUpRotation(0);
    194.             SpeedUpRotation(1);
    195.         }
    196.  
    197.         if ((int)Math.Round(expectedAngleLR) > rollAngle)
    198.         {
    199.             SlowDownRotation(0);
    200.             SlowDownRotation(3);
    201.             SpeedUpRotationt(1);
    202.             SpeedUpRotationt(2);
    203.         }
    204.  
    205.         else if ((int)Math.Round(expectedAngleLR) < rollAngle)
    206.         {
    207.             SlowDownRotation(1);
    208.             SlowDownRotation(2);
    209.             SpeedUpRotationt(0);
    210.             SpeedUpRotationt(3);
    211.         }
    212.     }
    213.     private void SlowDownRotation(int propellerNr)
    214.     {
    215.         int i = propellerNr;
    216.         rotationSpeedList[i] -= rotationDelay / 100;
    217.         rotationSpeedList[i] = Math.Max(rotationSpeedList[i], rotationSpeed - maxDelayFB < 0 ? 0 : rotationSpeed - maxDelayFB);      
    218.     }
    219.  
    220.     private void SpeedUpRotation(int propellerNr)
    221.     {
    222.         int i = propellerNr;
    223.         rotationSpeedList[i] += rotationDelay;
    224.         rotationSpeedList[i] = Math.Min(rotationSpeedList[i], rotationSpeed+ maxDelayFB > maxSpeed?maxSpeed:rotationSpeed+ maxDelayFB);
    225.     }
    226.  
    227.     private void GoBackToMainSpeed()
    228.     {
    229.         if (expectedAngleFB > 0)
    230.             expectedAngleFB -= agleChangeAmount;
    231.         else if (expectedAngleFB < 0)
    232.             expectedAngleFB += agleChangeAmount;
    233.  
    234.         if (expectedAngleLR > 0)
    235.             expectedAngleLR -= agleChangeAmount;
    236.         else if (expectedAngleLR < 0)
    237.             expectedAngleLR += agleChangeAmount;
    238.     }
    239.  
    240.     private void OnCollisionEnter(Collision collision)
    241.     {
    242.         if (collision.gameObject.tag == "ground")
    243.             isOnGround = true;
    244.     }
    245. }
    246.  
    In this code i have 4 propellers wchich are cube.001 2 3 and 4. Each propeller has its own speed and gereted force based on speed. I dont know if i should use it in fixed update to add force to the rigidbody or what to help it stabilize and keep an angle. Can someone take a look at the code and let me know how can i improve it? Thanks ;)

    upload_2023-10-17_16-48-44.png

    its a drone im using
     
  2. halley

    halley

    Joined:
    Aug 26, 2013
    Posts:
    2,282
    Your code seems to be using what is called "bang bang" regulation: if an angle is outside the expected fixed range, you suddenly trigger a change in motor speed by a fixed amount. This is how oven thermostats work: if it's too hot, turn off the power; if it's too cold, turn on the power.

    What you want is a PID (Proportional-Integral-Derivative) controller: this takes the desired angle and the current motor speed as continuous values, and the amount of change calculated by the amount of errant angle detected. When the angle is really small, the adjustments to speed needs to be really small. When the errant angle is larger, smoothly ramp the speed to a greater extent to manage the angle. Real drones use PIDs to take orientation data and smoothly control the motors to minimize errant angle.
     
    ObalonyPivvot likes this.
  3. ObalonyPivvot

    ObalonyPivvot

    Joined:
    Oct 17, 2023
    Posts:
    5
    Okey, i will start some research in that way. Do you know anything about coding something like that in unity?
     
  4. halley

    halley

    Joined:
    Aug 26, 2013
    Posts:
    2,282
    There are PID tutorials out there. That said, I think it would be overkill to try to simulate a drone physically through motor speeds, unless you're trying to study drone control, like really study, academically.

    Games are about verisimilitude: compute the appearance of reality as cheaply as possible, do not get sucked into the idea of actually computing everything realistically. I would control a drone's position and orientation according to the game's needs, and then decide afterwards a motor speed that gave the look I wanted. If the drone is a Rigidbody, add a single torque perpendicular to the errant angle, not computing four separate thrusts which make four separate torques for the engine to mix.
     
  5. ObalonyPivvot

    ObalonyPivvot

    Joined:
    Oct 17, 2023
    Posts:
    5
    I already did something like that. One force, rotation forced for rigidbody but my academical promotore said its not enough. He want something more physic based so im trying to achieve this. I will try adding this PID concept to my game, dont know if it will work but whatever. Wish me luck. Thanks for help
     
  6. halley

    halley

    Joined:
    Aug 26, 2013
    Posts:
    2,282
    Then by all means, simulate using PIDs.