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);
}