public void Rotate(bool direction)
{
float[] timers = { 0.01f, 1 / velocity, 1 / velocity, 1 / velocity, 1 / velocity };
for (int i = 0; i < 6; i++)
{
float[][] cartesian;
if (i % 2 == 0)
{
cartesian = robotMove.GetRotateCycle(robotMove.l1 + robotMove.l2 + walkAmplitude, 0, -robotMove.l3 - walkHigh, stepHigh, direction, rotateStep, true);
}
else
{
cartesian = robotMove.GetRotateCycle(robotMove.l1 + robotMove.l2 + walkAmplitude, 0, -robotMove.l3 - walkHigh, stepHigh, direction, rotateStep, false);
}
float[][] jointsEven = robotMove.GetJointsWalkingCycle(cartesian[0], cartesian[1], cartesian[2]);
bool notNan = true;
for (int j = 0; j < jointsEven.Length; j++)
{
for (int k = 0; k < jointsEven[j].Length; k++)
{
if (float.IsNaN(jointsEven[j][k]))
{
notNan = false;
}
}
}
if (notNan)
{
coxa.SetTrajectoryCoxa(jointsEven[0], timers, i);
femur.SetTrajectoryFemur(jointsEven[1], timers, i);
tibia.SetTrajectoryTibia(jointsEven[2], timers, i);
}
else
{
print("Commands impossible to reach");
}
}
}
