HexapiBackground.IK.InverseKinematics.GetServoPositions C# (CSharp) Method

        private static string GetServoPositions(IList<double> coxaAngles, IList<double> femurAngles, IList<double> tibiaAngles)
        {
            StringBuilder.Clear();

            for (var legIndex = 0; legIndex <= 5; legIndex++)
            {
                coxaAngles[legIndex] = Math.Min(Math.Max(coxaAngles[legIndex], CoxaMin), CoxaMax);
                femurAngles[legIndex] = Math.Min(Math.Max(femurAngles[legIndex], FemurMin), FemurMax);
                tibiaAngles[legIndex] = Math.Min(Math.Max(tibiaAngles[legIndex], TibiaMin), TibiaMax);

                double coxaPosition;
                double femurPosition;
                double tibiaPosition;

                if (legIndex < 3)
                {
                    coxaPosition = Math.Round((-coxaAngles[legIndex] + 900)*1000/PwmDiv + PfConst);
                    femurPosition = Math.Round((-femurAngles[legIndex] + 900)*1000/PwmDiv + PfConst);
                    tibiaPosition = Math.Round((-tibiaAngles[legIndex] + 900)*1000/PwmDiv + PfConst);
                }
                else
                {
                    coxaPosition = Math.Round((coxaAngles[legIndex] + 900)*1000/PwmDiv + PfConst);
                    femurPosition = Math.Round((femurAngles[legIndex] + 900)*1000/PwmDiv + PfConst);
                    tibiaPosition = Math.Round((tibiaAngles[legIndex] + 900)*1000/PwmDiv + PfConst);
                }

                CoxaServoAngles[legIndex] = coxaPosition;
                FemurServoAngles[legIndex] = femurPosition;
                TibiaServoAngles[legIndex] = tibiaPosition;

                StringBuilder.Append($"#{LegServos[legIndex][0]}P{coxaPosition}");
                StringBuilder.Append($"#{LegServos[legIndex][1]}P{femurPosition}");
                StringBuilder.Append($"#{LegServos[legIndex][2]}P{tibiaPosition}");
            }

            StringBuilder.Append($"T{_gaitSpeedInMs}\rQ\r");

            return StringBuilder.ToString();
        }