/// <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); } }