private void FixedUpdate() { float motorTargetVelocity = _pidController.Update(UnityHingeJoint.angle, Time.deltaTime); if (_pidController.IsAtDeadBand) { JointSpring hingeJointSpring = UnityHingeJoint.spring; hingeJointSpring.damper = 50000.0f; hingeJointSpring.spring = 1e+08f; hingeJointSpring.targetPosition = UnityHingeJoint.angle; UnityHingeJoint.spring = hingeJointSpring; UnityHingeJoint.useSpring = true; UnityHingeJoint.useMotor = false; } else // drive to position { JointMotor motor = UnityHingeJoint.motor; motor.targetVelocity = motorTargetVelocity; motor.force = _maxForce; motor.freeSpin = false; UnityHingeJoint.motor = motor; UnityHingeJoint.useMotor = true; UnityHingeJoint.useSpring = false; } // Debug.Log("Joint Angle: " + _hingeJoint.angle); // Debug.Log("Motor Force: " + motor.force); // if (f % 10 == 0) { // Debug.Log("motor.force = " + motor.force); // Debug.Log("velocity = " + _hingeJoint.velocity); // Debug.Log("angle deg = " + _hingeJoint.angle); // // Debug.Log("angle rad = " + (Mathf.Deg2Rad * _hingeJoint.angle)); // } // f++; }
private void FixedUpdate() { Rigidbody slideBody = _prismaticJoint.gameObject.GetComponent <Rigidbody>(); float currentPosition = slideBody.transform.localPosition[(int)_driveAxis]; float force = _pidController.Update(currentPosition, Time.deltaTime); Vector3 globalUpForce = new Vector3(0, 0, 0); globalUpForce[(int)_driveAxis] = force; Vector3 localUpForce = slideBody.transform.worldToLocalMatrix.MultiplyVector(globalUpForce); slideBody.AddRelativeForce(localUpForce, ForceMode.Force); // add equal and opposite force Rigidbody opposingBody = _prismaticJoint.connectedBody; localUpForce = -1.0f * localUpForce; // flip opposingBody.AddRelativeForce(localUpForce, ForceMode.Force); }