void FixedUpdate()
{
if (_abortFixedUpdate)
{
return;
}
try
{
_headingController.Update();
_bankController.Update();
_altitudeController.Update();
_vertSpeedController.Update();
_speedByPitchController.Update();
_pitchController.Update();
if (_singleStep)
{
FlightDriver.SetPause(true, postScreenMessage: false);
}
}
catch
{
Debug.Log("WAP: Exception occurred in FixedUpdate; stopping.");
_abortFixedUpdate = true;
throw;
}
}
private void FixedUpdate()
{
headAngle = Vector3.SignedAngle((head.transform.position - body.transform.position), Vector3.up, Vector3.forward); //Find angle of head compared to the body position
headForceVector = -Vector2.Perpendicular(new Vector2(head.transform.position.x, head.transform.position.y) - new Vector2(body.transform.position.x, body.transform.position.y)); //Janky line of code that finds a vector perpendicular to the body offset
//Calculate the speed in every case
if (body.velocity.x > 0)
{
bodySpeed = Vector3.Magnitude(new Vector3(body.velocity.x, body.velocity.y, 0f));
}
if (body.velocity.x < 0)
{
bodySpeed = -Vector3.Magnitude(new Vector3(body.velocity.x, body.velocity.y, 0f));
}
if (body.velocity.x == 0)
{
bodySpeed = 0f;
}
//Update the PID controllers
balancePid.Update(setPoint, headAngle, P, I, D, Time.fixedDeltaTime, maxOutput, minOutput);
headAirPid.Update(setPoint, headAngle, 0.05f, 0f, 0.015f, Time.fixedDeltaTime, maxOutput, minOutput);
//Debug.Log(-balancePid.Output()+" "+headAngle+" "+bodySpeed);
if (body.IsTouchingLayers(-1) == true && Mathf.Abs(headAngle) < 80) //If the body is touching anything, and is somewhat upright
{
if (jump == 1)
{
Jump();
}
body.AddTorque(-balancePid.Output() * torque * Time.fixedDeltaTime * 10); //Add torque to the body according to PID outputs
}
if (body.IsTouchingLayers(-1) == true && Mathf.Abs(headAngle) > 80) //If the body is touching anything, and the head is not upright
{
head.AddForce(new Vector2(0f, jump * recoveryForce)); //If the jump button is pressed, add a recoveryForce to the head to return it to an upright position
}
if (body.IsTouchingLayers(-1) == false && Mathf.Abs(headAngle) < 80) //If the body isn't touching anything and the head is upright
{
head.AddForce(headForceVector * -headAirPid.Output()); //Add force to the head according to PID output
}
}