public void FixedUpdate()
{
Vector4 input = new Vector4();
if (typeOfInput == TypeOfInput.GAMEPAD)
{
var gamepad = Gamepad.current;
if (gamepad != null)
{
input.x = gamepad.rightStick.y.ReadValue();
input.y = gamepad.rightStick.x.ReadValue();
input.z = gamepad.leftStick.x.ReadValue();
input.w = gamepad.leftStick.y.ReadValue();
}
}
else
{
input = externalInput;
}
float currentVerticalVelocity = rb.velocity.y;
var angularVelocity = transform.InverseTransformDirection(rb.angularVelocity);
float currentRollVelocity = -angularVelocity.z;
float currentYawVelocity = angularVelocity.y;
float currentPitchVelocity = -angularVelocity.x;
float currentPitchAngle = NormalizeAngle(-rb.rotation.eulerAngles.x, -180, 180);
float currentRollAngle = NormalizeAngle(-rb.rotation.eulerAngles.z, -180, 180);
var velocity = transform.InverseTransformDirection(rb.velocity);
float currentforwardVelocity = -velocity.z;
float currentrightVelocity = velocity.x;
output = currentYawVelocity;
float targetVerticalVelocity = input.x * 5f;
float targetYawVelocity = input.y * Mathf.Deg2Rad * 200;
float targetForwardVelocity = -input.w * 5;
float targetRightVelocity = input.z * 5;
float targetPitchAngle = forwardVelocityPID.CalculateCurrentAnswer(currentforwardVelocity, targetForwardVelocity);
float targetRollAngle = rightVelocityPID.CalculateCurrentAnswer(currentrightVelocity, targetRightVelocity);
float targetRollVelocity = rollAnglePID.CalculateCurrentAnswer(currentRollAngle, targetRollAngle);
float targetPitchVelocity = pitchAnglePID.CalculateCurrentAnswer(currentPitchAngle, targetPitchAngle);
target = targetYawVelocity;
float thrust = thurstPID.CalculateCurrentAnswer(currentVerticalVelocity, targetVerticalVelocity);
if (ownYawPid)
{
yaw = yawPID.CalculateCurrentAnswer(currentYawVelocity, targetYawVelocity);
}
float pitch = pitchPID.CalculateCurrentAnswer(currentPitchVelocity, targetPitchVelocity);
float roll = rollPID.CalculateCurrentAnswer(currentRollVelocity, targetRollVelocity);
//Debug.Log("++++++++++++++++++++++++++++++++++++++++++++++");
//Debug.Log(currentRollVelocity);
//Debug.Log(currentYawVelocity);
//Debug.Log(currentPitchVelocity);
//Debug.Log("Thurst: " + thrust);
//Debug.Log("VerticalVelocity: " + currentVerticalVelocity);
//Debug.Log("++++++++++++++++++++++++++++++++++++++++++++++");
MotorMixing(thrust, yaw, pitch, roll);
Debug.Log("Inertia Tensor: " + rb.inertiaTensor);
Debug.Log("Inertia Tensor Rotation: " + rb.inertiaTensorRotation.eulerAngles);
}
