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);
}
}
public static void Main()
{
/* Disable drivetrain/motors */
_rightTalon.Set(ControlMode.PercentOutput, 0);
_leftTalon.Set(ControlMode.PercentOutput, 0);
#if (fourWheeled)
_rightFollower.Follow(_rightTalon);
_leftFollower.Follow(_leftTalon);
#endif
/* Configure output and sensor direction */
_rightTalon.SetInverted(true);
_leftTalon.SetInverted(false);
#if (fourWheeled)
_rightFollower.SetInverted(true);
_leftFollower.SetInverted(false);
#endif
/* Mode print */
Debug.Print("This is arcade drive using Arbitrary Feedforward");
bool Btn1 = false;
bool Btn2 = false;
bool Btn3 = false;
bool Btn4 = false;
bool Btn10 = false;
bool VoltageComp = false;
bool CurrentLimit = false;
bool NeutralState = false;
bool RampRate = false;
bool FirstCall = true;
while (true)
{
/* Enable motor controllers if gamepad connected */
if (_gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Gamepad Stick Control */
float forward = -1 * _gamepad.GetAxis(1);
float turn = 1 * _gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref forward);
CTRE.Phoenix.Util.Deadband(ref turn);
turn *= 0.5f; //Scaled down for safety
forward *= 0.75f;
bool btn1 = _gamepad.GetButton(1);
bool btn2 = _gamepad.GetButton(2);
bool btn3 = _gamepad.GetButton(3);
bool btn4 = _gamepad.GetButton(4);
bool btn10 = _gamepad.GetButton(10);
if (btn1 && !Btn1)
{
VoltageComp = !VoltageComp;
FirstCall = true;
}
else if (btn2 && !Btn2)
{
CurrentLimit = !CurrentLimit;
FirstCall = true;
}
else if (btn3 && !Btn3)
{
NeutralState = !NeutralState;
FirstCall = true;
}
else if (btn4 && !Btn4)
{
RampRate = !RampRate;
FirstCall = true;
}
else if (btn10 && !Btn10)
{
VoltageComp = false;
CurrentLimit = false;
NeutralState = false;
RampRate = false;
FirstCall = true;
}
Btn1 = btn1;
Btn2 = btn2;
Btn3 = btn3;
Btn4 = btn4;
Btn10 = btn10;
if (VoltageComp)
{
_rightTalon.ConfigVoltageCompSaturation(10, 10);
_rightTalon.ConfigVoltageMeasurementFilter(16, 10);
_rightTalon.EnableVoltageCompensation(true);
_leftTalon.ConfigVoltageCompSaturation(10, 10);
_leftTalon.ConfigVoltageMeasurementFilter(16, 10);
_leftTalon.EnableVoltageCompensation(true);
if (FirstCall)
{
Debug.Print("Voltage Compensation: On");
}
}
else
{
_rightTalon.EnableVoltageCompensation(false);
_leftTalon.EnableVoltageCompensation(false);
if (FirstCall)
{
Debug.Print("Voltage Compensation: Off");
}
}
if (CurrentLimit)
{
_rightTalon.ConfigContinuousCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentDuration(0, 10);
_rightTalon.EnableCurrentLimit(true);
_leftTalon.ConfigContinuousCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentDuration(0, 10);
_leftTalon.EnableCurrentLimit(true);
if (FirstCall)
{
Debug.Print("Current Limit: On");
}
}
else
{
_rightTalon.EnableCurrentLimit(false);
_leftTalon.EnableCurrentLimit(false);
if (FirstCall)
{
Debug.Print("Current Limit: Off");
}
}
if (NeutralState)
{
_rightTalon.SetNeutralMode(NeutralMode.Coast);
_leftTalon.SetNeutralMode(NeutralMode.Coast);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Coast);
_leftFollower.SetNeutralMode(NeutralMode.Coast);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Coast");
}
}
else
{
_rightTalon.SetNeutralMode(NeutralMode.Brake);
_leftTalon.SetNeutralMode(NeutralMode.Brake);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Brake);
_leftFollower.SetNeutralMode(NeutralMode.Brake);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Brake");
}
}
if (RampRate)
{
_rightTalon.ConfigOpenloopRamp(3, 0);
_leftTalon.ConfigOpenloopRamp(3, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: On, 3 Seconds");
}
}
else
{
_rightTalon.ConfigOpenloopRamp(0.0f, 0);
_leftTalon.ConfigOpenloopRamp(0.0f, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: Off, 0 Seconds");
}
}
/* Use Arbitrary FeedForward to create an Arcade Drive Control by modifying the forward output */
_rightTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, -turn);
_leftTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, +turn);
if (FirstCall)
{
Debug.Print("");
}
FirstCall = false;
Thread.Sleep(5);
}
}
