C# Unity Drone Script Programing

Need Expert advise for Lines 53 through 70(code below)- need to modify this area?

My Drone only turns(left and right), and goes up and down using keys W,S,A,D.

I need to add line to ROLL AND PITCH?

using UnityEngine;

public class UAVController : MonoBehaviour { public GameObject drone; public GameObject medicineBox; public PIDController altitudePID; public static int numberofMedicineBoxesDropped;

public float Kp; public float Ki; public float Kd;

private Rigidbody droneRigidBody; private Animator UAVAnimator;

public static float _thrust; //direct motor thrust public static Vector3 torque; //torque around the x y and z axes

private float _maxThrust = 1.0f; private float _maxRollThrust = 0.5f; private float _maxPitchRollTorque = 0.75f;

private float hoverForce = 20.0f; public float hoverHeight = 0.0f;

private Vector3 zeroPosition = new Vector3(); private Quaternion zeroRotation = new Quaternion();

// Use this for initialization void Start() { droneRigidBody = drone.GetComponent(); UAVAnimator = drone.GetComponent();

UAVAnimator.enabled = true;

zeroPosition = drone.transform.position; zeroRotation = drone.transform.rotation;

altitudePID = new PIDController(Kp, Ki, Kd);

}

// Update is called once per frame void Update() { _thrust = 0; torque = Vector3.zero;

/////////////////////////// Flight Control with Buttons ////////////////////////////////////// /// //Alitiude if (Input.GetAxis("VerticalThrust") != 0.0f) _thrust = Input.GetAxis("VerticalThrust") * _maxThrust;

//Yaw if (Input.GetAxis("YawThrust") != 0.0f) torque.y = Input.GetAxis("YawThrust") * _maxRollThrust;

//student Activity //pitch if (Input.GetAxis("PitchThrust") != 0.0f) _thrust = Input.GetAxis("PitchThrust") * _maxThrust;

//roll

if (Input.GetAxis("RollThrust") != 0.0f) torque.y = Input.GetAxis("RollThrust") * _maxRollThrust;

/////////////////////////// Flight Control with Joystick ////////////////////////////////////// //Insert Code here for extra credit ////Alitiude

//////yaw

//////pitch

////////roll

//Drop a medicine box if (Input.GetKeyDown(KeyCode.B) || Input.GetKeyDown(KeyCode.Joystick1Button1)) { Instantiate(medicineBox, drone.transform.position, Quaternion.identity); numberofMedicineBoxesDropped++; }

//////////////////////////// Other Actions /////////////////////////////////////////////////// if (_thrust != 0.0f) hoverHeight = drone.transform.position.y; //change hoverheight based on current altitude with thrust applied

PowerConsumption(Mathf.Abs(_thrust)/_maxThrust); //call to method to compute remain battery life

if (Input.GetKeyDown(KeyCode.R)) ReturnToStartingPosition(); //reset postion to starting position }

private void PowerConsumption(float throttleSetting) { //fill in your code here for the remaining battery life calculation

}

/////////// no need to alter anything below this line /////////////////////////////////// private void FixedUpdate() { Vector3 droneAngles = drone.transform.localEulerAngles;

if (_thrust != 0 || torque.magnitude != 0) {

if (_thrust != 0.0f || torque.y != 0.0f) { float verticalForce = 0.0f; verticalForce = (float)droneRigidBody.mass * (float)Physics.gravity.y * (-1) * (1.0f + _thrust); droneRigidBody.AddRelativeForce(Vector3.up * verticalForce, ForceMode.Force);

droneRigidBody.AddRelativeTorque(Vector3.up * torque.y, ForceMode.Force); }

float _throttlePositionTorquePitch = 0.0f; float _throttlePositionTorqueRoll = 0.0f; float dampingRoll = .5f; float dampingPitch = 0.1f;

if (torque.x != 0.0f || torque.z != 0.0f) { Quaternion droneQuat = drone.transform.rotation;

//////////////////////////// this part controls the pitch angle ////////////////////////////////////////////// if (droneQuat.x > -.25f && droneQuat.x < .25f) _throttlePositionTorquePitch = (Mathf.Clamp((torque.x), -0.5f, 0.5f)); //between 0 and 1 with max hand angle at 30 degrees // _throttlePositionTorquePitch = (Mathf.Clamp(-rightHandPalm.Pitch / (Mathf.PI / 6.0f), -1.0f, 1.0f)); //between 0 and 1 with max hand angle at 30 degrees

else if (droneQuat.x >= 0.24f) _throttlePositionTorquePitch = -0.325f * (droneQuat.x - .24f); //DRONE angle at 30 degrees

else if (droneQuat.x <= -.24f) _throttlePositionTorquePitch = -0.325f * (droneQuat.x + .24f); //max DRONE angle at 30 degrees

///////////////////////////// this part controls the roll angle /////////////////////////////////////////// if (droneQuat.z > -.25f && droneQuat.z < 0.250f) _throttlePositionTorqueRoll = (Mathf.Clamp(torque.z, -0.5f, 0.5f)); //between 0 and 1 with max hand angle at xx degrees

if (droneQuat.z >= 0.24f) _throttlePositionTorqueRoll = -0.325f * (droneQuat.z - .24f); //max hand angle at 30 degrees

if (droneQuat.z <= -.24f) _throttlePositionTorqueRoll = -0.325f * (droneQuat.z + 0.24f); //max hand angle at 30 degrees

//Apply the final torque vector for pitch and roll Vector3 totalTorqueVector = (Vector3.forward * _throttlePositionTorqueRoll * dampingRoll) + (Vector3.right * _throttlePositionTorquePitch * dampingPitch); droneRigidBody.AddRelativeTorque(totalTorqueVector, ForceMode.Force);

//this subset of code determines the unit vector in the y direction to compute the force such that the drone stays at a constant altitude when translating in the vertical plane only Vector3 translation = new Vector3(0, 0, 0); Vector3 eulerAngles = drone.transform.eulerAngles; Vector3 scale = new Vector3(1, 1, 1); Vector3 forceVector = new Vector3(0, 1, 0); Quaternion rotation = Quaternion.Euler(eulerAngles);

Matrix4x4 transformationMatrix = Matrix4x4.TRS(translation, rotation, scale); Matrix4x4 inverseTransformationMatrix = transformationMatrix.inverse; Vector3 newForceVector = inverseTransformationMatrix.MultiplyVector(forceVector); // Debug.Log(newForceVector.y);

float _thrust = (float)droneRigidBody.mass * (float)Physics.gravity.y * (-1) / (newForceVector.y); droneRigidBody.AddRelativeForce(Vector3.up * _thrust, ForceMode.Force); // Debug.Log(_thrust); } } // go into hover mode if no control inputs given if (_thrust == 0.0f && torque.magnitude == 0.0f) HoverPID(); } private void HoverPID() { //return to hover position uses simple proportional control scheme Vector3 droneAngles = drone.transform.localEulerAngles; float slope = 1.0e-3f; float deltaAngleZ = Mathf.DeltaAngle(0, droneAngles.z); float deltaAngleX = Mathf.DeltaAngle(0, droneAngles.x); float torqueZ = slope * deltaAngleZ; float torqueX = slope * deltaAngleX; droneRigidBody.AddRelativeTorque(-torqueX, 0.0f, -torqueZ, ForceMode.Force);

//Altitude Control for Hover - Uses PID Controller RaycastHit hit; Ray downRay = new Ray(drone.transform.position, -Vector3.up); Physics.Raycast(downRay, out hit); float currentAltitude = drone.transform.position.y; float error = hoverHeight - currentAltitude;

float forceMultiplier = Mathf.Clamp01(altitudePID.Update(error));

Vector3 appliedHoverForce = Vector3.up * (forceMultiplier * hoverForce); droneRigidBody.AddRelativeForce(appliedHoverForce, ForceMode.Force);

} private void ReturnToStartingPosition() { drone.transform.position = zeroPosition; drone.transform.rotation = zeroRotation; hoverHeight = 0.0f; }

}