public void SetPos(float angle0, float angle1, float angle2)
{
if (config.Kinematic)
{
float deltaangle0 = kinematicanglerange0.GetTarget() - chassis.GetComponent <chassis>().GetKinematicRestDeltaAngle0();
float deltaangle1 = kinematicanglerange1.GetTarget() - chassis.GetComponent <chassis>().GetKinematicRestDeltaAngle1();
float deltaangle2 = kinematicanglerange2.GetTarget() - chassis.GetComponent <chassis>().GetKinematicRestDeltaAngle2();
kinematicanglerange0.SetTarget(angle0);
kinematicanglerange1.SetTarget(angle1);
kinematicanglerange2.SetTarget(angle2);
deltaangle0 = kinematicanglerange0.Delta(deltaangle0, kinematicanglerange0.GetTarget());
deltaangle1 = kinematicanglerange0.Delta(deltaangle1, kinematicanglerange1.GetTarget());
deltaangle2 = kinematicanglerange0.Delta(deltaangle2, kinematicanglerange2.GetTarget());
chassis.GetComponent <chassis>().SetKinematicTargetDeltaAngles(deltaangle0, deltaangle1, deltaangle2);
}
else
{
chassis.GetComponent <chassis>().drive.AngleRange.SetTarget(angle0);
holder.GetComponent <holder>().drive.AngleRange.SetTarget(angle1);
lever.GetComponent <lever>().drive.AngleRange.SetTarget(angle2);
}
}
public void SetPos(float angle0, float angle1, float angle2, float angle3, float angle4, float angle5, bool gripped)
{
if (config.Kinematic)
{
float deltaangle0 = kinematicanglerange0.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle0();
float deltaangle1 = kinematicanglerange1.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle1();
float deltaangle2 = kinematicanglerange2.GetTarget() - chassis.GetComponent <chassismanipulator2>().GetKinematicRestDeltaAngle2();
kinematicanglerange0.SetTarget(angle0);
kinematicanglerange1.SetTarget(angle1);
kinematicanglerange2.SetTarget(-90 + angle2);
deltaangle0 = kinematicanglerange0.Delta(deltaangle0, kinematicanglerange0.GetTarget());
deltaangle1 = kinematicanglerange0.Delta(deltaangle1, kinematicanglerange1.GetTarget());
deltaangle2 = kinematicanglerange0.Delta(deltaangle2, kinematicanglerange2.GetTarget());
chassis.GetComponent <chassismanipulator2>().SetKinematicTargetDeltaAngles(deltaangle0, deltaangle1, deltaangle2);
}
else
{
DriveJoint drive0 = chassis.GetComponent <chassismanipulator2>().drive;
DriveJoint drive1 = lever.GetComponent <levermanipulator2>().drive;
DriveJoint drive2 = holder1.GetComponent <holder1manipulator2>().drive;
DriveJoint drive3 = holder2.GetComponent <holder2manipulator2>().drive;
drive0.AngleRange.SetTarget(angle0);
drive3.AngleRange.SetTarget(angle1);
drive2.AngleRange.SetTarget(drive3.AngleRange.GetTarget() + drive1.AngleRange.GetTarget());
drive1.AngleRange.SetTarget(-90 + angle2);
drive2.AngleRange.SetTarget(drive3.AngleRange.GetTarget() + drive1.AngleRange.GetTarget());
}
if (config.FingerDown)
{
capture.SetPos(0, GetPos1() + GetPos2() - 180, angle5);
}
else
{
capture.SetPos(angle3, angle4, angle5);
}
SetGripper(gripped);
}
public void Update()
{
float angle = GetAngle();
float deltaAngle = AngleRange.Delta(angle, AngleRange.GetTarget());
if (gameObject.GetComponent <Rigidbody>().isKinematic)
{
float step = Time.deltaTime * KinematicAngularVelocity * Mathf.Sign(deltaAngle);
angle = Mathf.Abs(deltaAngle) < Mathf.Abs(step) ? AngleRange.GetTarget() : angle + step;
commonJoint.KinematicRotate(Quaternion.AngleAxis(angle, Vector3.up));
return;
}
HingeJoint hingeJoint = pivotObject.GetComponent <HingeJoint>();
if (Mathf.Sign(deltaAngle) > 0)
{
deltaSAngle += deltaAngle;
}
else
{
deltaSAngle -= deltaAngle;
}
float deltaVelocity = -hingeJoint.velocity;
if (Mathf.Sign(deltaVelocity) == Mathf.Sign(deltaAngle))
{
deltaVelocity = -Mathf.Abs(deltaVelocity);
}
else
{
deltaVelocity = Mathf.Abs(deltaVelocity);
}
float kP = Proportional;
float kI = Integral;
float kD = Differential;
JointMotor motor = hingeJoint.motor;
motor.targetVelocity = -Mathf.Sign(deltaAngle) * 1000000000;
motor.force = kP * Mathf.Abs(deltaAngle) + kI * deltaSAngle + kD * deltaVelocity;
motor.freeSpin = true;
hingeJoint.motor = motor;
hingeJoint.useMotor = true;
}
