/** occasionally builds a line and prints to output window */
 void Instrument()
 {
     if (--_timeToPrint <= 0)
     {
         _timeToPrint = 20;
         _sb.Clear();
         _sb.Append("pos=");
         _sb.Append(_talon.GetSelectedSensorPosition(0));
         _sb.Append(" vel=");
         _sb.Append(_talon.GetSelectedSensorVelocity(0));
         _sb.Append(" err=");
         _sb.Append(_talon.GetClosedLoopError(0));
         _sb.Append(" out%=");
         _sb.Append(_talon.GetMotorOutputPercent());
         Debug.Print(_sb.ToString());
     }
 }
        public static void Process(TalonSRX talon)
        {
            /* simple timeout to reduce printed lines */
            if (++_instrumLoops1 > 10)
            {
                _instrumLoops1 = 0;

                /* get closed loop info */
                float pos = talon.GetSelectedSensorPosition(0);
                float spd = talon.GetSelectedSensorVelocity(0);
                int   err = talon.GetClosedLoopError(0);

                /* build the string */
                _sb.Clear();

                _sb.Append(pos);
                if (_sb.Length < 16)
                {
                    _sb.Append(' ', 16 - _sb.Length);
                }

                _sb.Append(spd);
                if (_sb.Length < 32)
                {
                    _sb.Append(' ', 32 - _sb.Length);
                }

                _sb.Append(err);
                if (_sb.Length < 48)
                {
                    _sb.Append(' ', 48 - _sb.Length);
                }

                Debug.Print(_sb.ToString()); /* print data row */

                if (++_instrumLoops2 > 8)
                {
                    _instrumLoops2 = 0;

                    _sb.Clear();

                    _sb.Append("Position");
                    if (_sb.Length < 16)
                    {
                        _sb.Append(' ', 16 - _sb.Length);
                    }

                    _sb.Append("Velocity");
                    if (_sb.Length < 32)
                    {
                        _sb.Append(' ', 32 - _sb.Length);
                    }

                    _sb.Append("Error");
                    if (_sb.Length < 48)
                    {
                        _sb.Append(' ', 48 - _sb.Length);
                    }

                    Debug.Print(_sb.ToString()); /* print columns */
                }
            }
        }
        public static void Main()
        {
            /* Hardware */
            TalonSRX _talon = new TalonSRX(1);

            /** Use a USB gamepad plugged into the HERO */
            GameController _gamepad = new GameController(UsbHostDevice.GetInstance());

            /* String for output */
            StringBuilder _sb = new StringBuilder();

            /** hold bottom left shoulder button to enable motors */
            const uint kEnableButton = 7;

            /* Loop tracker for prints */
            int _loops = 0;

            /* Initialization */
            /* Factory Default all hardware to prevent unexpected behaviour */
            _talon.ConfigFactoryDefault();

            /* Config sensor used for Primary PID [Velocity] */
            _talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,
                                                Constants.kPIDLoopIdx,
                                                Constants.kTimeoutMs);

            /**
             * Phase sensor accordingly.
             * Positive Sensor Reading should match Green (blinking) Leds on Talon
             */
            _talon.SetSensorPhase(false);

            /* Config the peak and nominal outputs */
            _talon.ConfigNominalOutputForward(0, Constants.kTimeoutMs);
            _talon.ConfigNominalOutputReverse(0, Constants.kTimeoutMs);
            _talon.ConfigPeakOutputForward(1, Constants.kTimeoutMs);
            _talon.ConfigPeakOutputReverse(-1, Constants.kTimeoutMs);

            /* Config the Velocity closed loop gains in slot0 */
            _talon.Config_kF(Constants.kPIDLoopIdx, Constants.kF, Constants.kTimeoutMs);
            _talon.Config_kP(Constants.kPIDLoopIdx, Constants.kP, Constants.kTimeoutMs);
            _talon.Config_kI(Constants.kPIDLoopIdx, Constants.kI, Constants.kTimeoutMs);
            _talon.Config_kD(Constants.kPIDLoopIdx, Constants.kD, Constants.kTimeoutMs);

            /* loop forever */
            while (true)
            {
                /* Get gamepad axis */
                double leftYstick = -1 * _gamepad.GetAxis(1);

                /* Get Talon/Victor's current output percentage */
                double motorOutput = _talon.GetMotorOutputPercent();

                /* Prepare line to print */
                _sb.Append("\tout:");
                /* Cast to int to remove decimal places */
                _sb.Append((int)(motorOutput * 100));
                _sb.Append("%");                    // Percent

                _sb.Append("\tspd:");
                _sb.Append(_talon.GetSelectedSensorVelocity(Constants.kPIDLoopIdx));
                _sb.Append("u");                    // Native units

                /**
                 * When button 1 is held, start and run Velocity Closed loop.
                 * Velocity Closed Loop is controlled by joystick position x2000 RPM, [-2000, 2000] RPM
                 */
                if (_gamepad.GetButton(1))
                {
                    /* Velocity Closed Loop */

                    /**
                     * Convert 2000 RPM to units / 100ms.
                     * 4096 Units/Rev * 2000 RPM / 600 100ms/min in either direction:
                     * velocity setpoint is in units/100ms
                     */
                    double targetVelocity_UnitsPer100ms = leftYstick * 2000.0 * 4096 / 600;
                    /* 2000 RPM in either direction */
                    _talon.Set(ControlMode.Velocity, targetVelocity_UnitsPer100ms);

                    /* Append more signals to print when in speed mode. */
                    _sb.Append("\terr:");
                    _sb.Append(_talon.GetClosedLoopError(Constants.kPIDLoopIdx));
                    _sb.Append("\ttrg:");
                    _sb.Append(targetVelocity_UnitsPer100ms);
                }
                else
                {
                    /* Percent Output */

                    _talon.Set(ControlMode.PercentOutput, leftYstick);
                }

                /* Print built string every 10 loops */
                if (++_loops >= 10)
                {
                    _loops = 0;
                    Debug.Print(_sb.ToString());
                }
                /* Reset built string */
                _sb.Clear();

                //if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) // check if gamepad is plugged in OR....
                if (_gamepad.GetButton(kEnableButton))                 // check if bottom left shoulder buttom is held down.
                {
                    /* then enable motor outputs*/
                    Watchdog.Feed();
                }

                /* wait a bit */
                System.Threading.Thread.Sleep(20);
            }
        }