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

        internal InverseKinematics(SparkFunSerial16X2Lcd display)
        {
            IkController.RangingEvent += RangingEventHandler;

            IkController.ImuEvent += ImuEventHandler;

            _display = display;

            _pi1K = Pi*1000D;

            for (var i = 0; i < 6; i++)
                LegServos[i] = new int[3];

            _movementStarted = false;

            for (var legIndex = 0; legIndex <= 5; legIndex++)
            {
                _legPosX[legIndex] = _initPosX[legIndex]; //Set start positions for each leg
                _legPosY[legIndex] = _initPosY[legIndex];
                _legPosZ[legIndex] = _initPosZ[legIndex];

                LegYHeightCorrector[legIndex] = 0;
            }

            _oscillateStopwatch.Start();
        }