private void UpdateHW()
{
// adjust tripod gait params
float angle = robot.GetTargetAngle();
float xIncl = Mathf.Abs(robot.body.GetInclination().x);
float zIncl = Mathf.Abs(robot.body.GetInclination().y);
hw.left = 1f - Mathf.Min(angle < 0f ? -angle * 4f : 0f, 2f);
hw.right = 1f - Mathf.Min(angle > 0f ? angle * 4f : 0f, 2f);
hw.sig = 1f + Mathf.Min(0.1f, xIncl * zIncl);
xIncl = 1f - Mathf.Min(0.5f, xIncl);
zIncl = 1f - Mathf.Min(0.5f, zIncl);
angle = 1f - Mathf.Sqrt(Mathf.Abs(angle));
float freq = 5f + 50f * angle * xIncl * zIncl * Mathf.Clamp(robot.body.GetSpeed() / 2f, 0.2f, 1f);
hw.cpg.freq = Mathf.Lerp(hw.cpg.freq, freq, 0.2f);
JointRotations jr = hw.GetRotations();
for (int i = 0; i < 6; i++)
{
hwAction[i * 4 + 0] = jr.rotShoulder[i];
hwAction[i * 4 + 1] = jr.rotKnee[i];
hwAction[i * 4 + 2] = jr.rotKnee[i] * 2f;
hwAction[i * 4 + 3] = jr.rotKnee[i] * -2f;
}
}
private void Awake()
{
cpg = GetComponent <Oscillator>();
phaseOffset = new float[N_LEGS];
dPhaseOffset = new float[N_LEGS];
tPhaseOffset = new float[N_LEGS];
GetKneeJointPhaseOffset(left, right, ref phaseOffset);
GetKneeJointPhaseOffset(left, right, ref tPhaseOffset);
amp = new float[N_LEGS];
dAmp = new float[N_LEGS];
tAmp = new float[N_LEGS];
GetShoulderJointAmp(left, right, ref amp);
GetShoulderJointAmp(left, right, ref tAmp);
jr = new JointRotations(N_LEGS);
}