public void OnSetInput_JointVariable(TMP_InputField input) { int legNumber = FindLegNumber(input.gameObject, "Leg"); int jointNumber = FindJointNumber(input.gameObject); float newJointValue = float.Parse(input.text); //legs[legNumber][jointNumber] = newJointValue; if (_wifiServer != null) { string tag = "J" + legNumber.ToString() + jointNumber.ToString(); _wifiServer.GetComponent <WifiServer>().SendData(tag, Int32.Parse(input.text)); } if (jointNumber == 0) { JointCoxaMovement coxa = coxas.GetComponent <JointCoxaMovement>(); coxa.SetTrajectoryCoxa(new float[] { newJointValue }, new float[] { 1 }, legNumber); } if (jointNumber == 1) { JointFemurMovement femur = femurs.GetComponent <JointFemurMovement>(); femur.SetTrajectoryFemur(new float[] { newJointValue }, new float[] { 1 }, legNumber); } if (jointNumber == 2) { JointTibiaMovement tibia = tibias.GetComponent <JointTibiaMovement>(); tibia.SetTrajectoryTibia(new float[] { newJointValue }, new float[] { 1 }, legNumber); } }
private void WalkUneven(float direction) { float[] timers = { 0.001f, 1 / velocity, 2 / velocity, 1 / velocity, 2 / velocity }; for (int i = 0; i < 6; i++) { float[] increments = robotMove.GetWalkingIncrements(stepDistance, 0, direction, i); float[][] cartesian; if (i % 2 == 0) { cartesian = robotMove.GetWalkingCycleUneven(robotMove.l1 + robotMove.l2 + walkAmplitude, 0, -robotMove.l3 - walkHigh, stepHigh, increments[0], increments[1], true); } else { cartesian = robotMove.GetWalkingCycleUneven(robotMove.l1 + robotMove.l2 + walkAmplitude, 0, -robotMove.l3 - walkHigh, stepHigh, increments[0], increments[1], false); } float[][] jointsEven = robotMove.GetJointsWalkingCycleUneven(cartesian[0], cartesian[1], cartesian[2], i); 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("Imposible to move, invalid commands"); } } }