Esempio n. 1
0
    // apply control forces
    void FixedUpdate()
    {
        if (!_robot.moveEnabled)
        {
            return;
        }

        float displacement2d = _target2d.magnitude;

        _targetSpeed = lateralSpeedController.output(0f, -displacement2d);
        _targetSpeed = Mathf.Clamp(_targetSpeed, 0f, maxLateralSpeed);
        _targetTilt  = tiltController.output(_targetSpeed, rigidbody.velocity.magnitude);
        _targetTilt  = Mathf.Clamp(_targetTilt, 0f, maxTilt);

        Vector3 forward2d = new Vector3(transform.forward.x, 0f, transform.forward.z).normalized;
        Vector3 right2d   = new Vector3(transform.right.x, 0f, transform.right.z).normalized;

        // PITCH
        float forwardAxisAngle = Vector3.Angle(forward2d, _target2d);
        float forwardCosine    = Mathf.Cos(forwardAxisAngle * Mathf.Deg2Rad);

        _actualPitch = Vector3.Angle(transform.forward, Vector3.up) - 90f;         // avoids the 0-360 boundary
        _targetPitch = forwardCosine * _targetTilt;

        // ROLL
        float rightAxisAngle = Vector3.Angle(right2d, _target2d);
        float rightCosine    = Mathf.Cos(rightAxisAngle * Mathf.Deg2Rad);

        _actualRoll = Vector3.Angle(transform.right, Vector3.up) - 90f;         // avoids the 0-360 boundary
        _targetRoll = rightCosine * _targetTilt;


        float pitchOutput = pitchController.output(_targetPitch, _actualPitch);
        float rollOutput  = rollController.output(_targetRoll, _actualRoll);

        // THROTTLE
        float throttle = throttleController.output(_targetHeight, transform.position.y);

        // adapted from here
        // https://ghowen.me/build-your-own-quadcopter-autopilot/
        float flForce = Mathf.Clamp(throttle + rollOutput - pitchOutput, 0f, maxThrottle);
        float blForce = Mathf.Clamp(throttle + rollOutput + pitchOutput, 0f, maxThrottle);
        float frForce = Mathf.Clamp(throttle - rollOutput - pitchOutput, 0f, maxThrottle);
        float brForce = Mathf.Clamp(throttle - rollOutput + pitchOutput, 0f, maxThrottle);


        _flControlThrust = FL.up * flForce;
        _blControlThrust = BL.up * blForce;
        _frControlThrust = FR.up * frForce;
        _brControlThrust = BR.up * brForce;

        rigidbody.AddForceAtPosition(_flControlThrust, FL.position);
        rigidbody.AddForceAtPosition(_frControlThrust, FR.position);
        rigidbody.AddForceAtPosition(_blControlThrust, BL.position);
        rigidbody.AddForceAtPosition(_brControlThrust, BR.position);
        rigidbody.AddTorque(_controlTorque, ForceMode.Acceleration);
    }