Exemplo n.º 1
0
        /// <summary>
        /// Drives Kiwi around
        /// </summary>
        static void Drive()
        {
            if (EStop)
            {
                return;
            }

            float aSpeed, bSpeed, cSpeed;

            if (null == _gamepad)
            {
                _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance());
            }

            float x     = _gamepad.GetAxis(0);
            float y     = -1 * _gamepad.GetAxis(1);
            float twist = _gamepad.GetAxis(2);

            float adjustmentAngle = 0;

            if (FieldOriented1)
            {
                TryFieldOriented(out adjustmentAngle);
            }
            else
            {
                adjustmentAngle = Orientation;
            }

            //Always using radial coordinates allows for easy orientation adjustment outside of field oriented
            float magnitude        = (float)System.Math.Sqrt(x * x + y * y),
                  directionRadians = (float)System.Math.Atan(x / y);

            x = magnitude * (float)System.Math.Cos(directionRadians - adjustmentAngle);
            y = magnitude * (float)System.Math.Sin(directionRadians - adjustmentAngle);

            Deadband(ref x, .05f);
            Deadband(ref y, .05f);
            Deadband(ref twist, .05f);

            aSpeed = x / 2 - (float)System.Math.Sqrt(3) / 2 * y + twist / (2);
            bSpeed = x / 2 + (float)System.Math.Sqrt(3) / 2 * y + twist / (2);
            cSpeed = x * -1 + twist / 2;

            Deadband(ref aSpeed, .07f);
            Deadband(ref bSpeed, .07f);
            Deadband(ref cSpeed, .07f);

            motorA.Set(aSpeed);
            motorB.Set(bSpeed);
            motorC.Set(cSpeed);

            stringBuilder.Append("\t");
            stringBuilder.Append(x);
            stringBuilder.Append("\t");
            stringBuilder.Append(y);
            stringBuilder.Append("\t");
            stringBuilder.Append(twist);
        }