/// <summary>
/// Attempts to call the FieldOriented function, fails if Pidgeon is not connected
/// </summary>
/// <param name="x">X axis</param>
/// <param name="y">Y axis</param>
/// <returns>True if succeeded, else false</returns>
static bool TryFieldOriented(out float adjustmentAngle)
{
float[] ypr = new float[3];
try
{
Pidgy.GetYawPitchRoll(ypr);
}
catch (Exception)
{
adjustmentAngle = 0;
return(false);
}
adjustmentAngle = FieldOriented(ypr[0]);
return(true);
}
public static void Main()
{
CTRE.UsbHostDevice.GetInstance().SetSelectableXInputFilter(CTRE.UsbHostDevice.SelectableXInputFilter.BothDInputAndXInput);
/* loop forever */
while (true)
{
/* get buttons */
for (uint i = 1; i < 20; ++i)
{
Buttons[i].last = Buttons[i].now;
Buttons[i].now = _gamepad.GetButton(i);
}
/* if button one was pressed, toggles field oriented*/
FieldOriented1 = Buttons[0].WasPressed() ? !FieldOriented1 : FieldOriented1;
/* kills drivetrain if button 2 is pressed or held */
if (Buttons[1].now)
{
motorA.Set(0);
motorB.Set(0);
motorC.Set(0);
motorA.ConfigNeutralMode(CTRE.TalonSrx.NeutralMode.Brake);
motorB.ConfigNeutralMode(CTRE.TalonSrx.NeutralMode.Brake);
motorC.ConfigNeutralMode(CTRE.TalonSrx.NeutralMode.Brake);
EStop = true;
}
/* turns off estop mode if button 10 was pressed*/
if (Buttons[9].WasPressed())
{
EStop = false;
motorA.ConfigNeutralMode(motorAInitialNeutralMode);
motorB.ConfigNeutralMode(motorBInitialNeutralMode);
motorC.ConfigNeutralMode(motorCInitialNeutralMode);
}
/* resets yaw if button six was pressed*/
if (Buttons[5].WasPressed())
{
try
{
Pidgy.SetYaw(0);
}
catch (Exception) { }
}
/* adjust orientation by 120 degrees if button three was pressed*/
Orientation += Buttons[2].WasPressed() ? (float)System.Math.PI * 2 / 3 : 0;
/*adjust orientation back by 120 degrees if button four was pressed*/
Orientation -= Buttons[3].WasPressed() ? (float)System.Math.PI * 2 / 3 : 0;
/* adjust orientation by 180 degrees if button five was pressed*/
Orientation += Buttons[4].WasPressed() ? (float)System.Math.PI : 0;
/* drive robot using gamepad */
Drive();
/* print whatever is in our string builder */
Debug.Print(stringBuilder.ToString());
stringBuilder.Clear();
/* feed watchdog to keep Talon's enabled */
CTRE.Watchdog.Feed();
/* run this task every 20ms */
Thread.Sleep(20);
}
}
