/// <summary>
/// Servo motor physics.
/// </summary>
private void FixedUpdate()
{
if (!IsMotorEnabled || IsFixed || !positionRegulatorProfile || !profile)
{
return;
}
// Run servo motor
if (IsMotorEnabled)
{
// Angle regulator controls servo velocity based on the position error
var targetVelocity = positionRegulator.Run(ServoAngleToJointSpace(targetAngle), GetJointAngle(), Time.fixedDeltaTime);
//Clamps target velocity according to servo profile
targetVelocity = Mathf.Clamp(targetVelocity, -profile.maxVelocity, profile.maxVelocity);
// Unity is quite bad at keeping target velocity, so we might use an extra regulator, which
// takes velocity error as an input. Even though we write its output value to joint.motor.targetVelocity,
// it works more like a torque control.
//Basically this line calculates the velocity error. Will add later on.
var velocityCorrection = velocityRegulator.Run(targetVelocity, GetServoVelocity(), Time.fixedDeltaTime);
velocityCorrection = Mathf.Clamp(velocityCorrection, -500, 500);
//joint motor applies maxForce Available and sets target vel
joint.motor = new JointMotor()
{
force = profile.maxForce, freeSpin = false, targetVelocity = targetVelocity + velocityCorrection
};
}
}
