uint [] _debLeftY = { 0, 0 }; // _debLeftY[0] is how many times leftY is zero, _debLeftY[1] is how many times leftY is not zeero.
public void Run()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* first choose the sensor */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
_talon.SetSensorPhase(false);
/* set closed loop gains in slot0 */
_talon.Config_kP(0, 0.2f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
_talon.Config_kI(0, 0f, kTimeoutMs);
_talon.Config_kD(0, 0f, kTimeoutMs);
_talon.Config_kF(0, 0f, kTimeoutMs); /* For position servo kF is rarely used. Leave zero */
/* use slot0 for closed-looping */
_talon.SelectProfileSlot(0, 0);
/* set the peak and nominal outputs, 1.0 means full */
_talon.ConfigNominalOutputForward(0.0f, kTimeoutMs);
_talon.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
_talon.ConfigPeakOutputForward(+1.0f, kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
/* how much error is allowed? This defaults to 0. */
_talon.ConfigAllowableClosedloopError(0, 0, kTimeoutMs);
/* put in a ramp to prevent the user from flipping their mechanism in open loop mode */
_talon.ConfigClosedloopRamp(0, kTimeoutMs);
_talon.ConfigOpenloopRamp(1, kTimeoutMs);
/* zero the sensor and throttle */
ZeroSensorAndThrottle();
/* loop forever */
while (true)
{
Loop10Ms();
//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();
}
/* print signals to Output window */
Instrument();
/* 10ms loop */
Thread.Sleep(10);
}
}