/// <summary>
/// Zeros all motor set values, including the Teleop Set, and autonomous ExtSet
/// </summary>
private void Zero()
{
_fstate = FireingState.ReadyToFire;
_wheelSet = 0;
_elevatorSet = 0;
_triggerSet = DoubleSolenoid.Value.Off;
WheelExtSet = 0;
ElevatorExtSet = 0;
TriggerExtSet = DoubleSolenoid.Value.Off;
}
/// <summary>
/// Reads controller input we can make different methods based on different times and places and to save previous attemps
/// </summary>
private void ReadController()
{
//Controls the main spining wheels
if(Math.Abs(_wheelSet) < .0005)
{
if (xbox.GetRightBumper()) _wheelSet += .001;
}
else if( Math.Abs(_wheelSet - 1) < .005)
{
if (xbox.GetLeftBumper()) _wheelSet -= 0.001;
}
else
{
if (xbox.GetLeftBumper()) _wheelSet -= 0.001;
if (xbox.GetRightBumper()) _wheelSet += .001;
}
switch (_fstate)
{
case FireingState.ReadyToFire:
SmartWriter.WriteString("Frisbee Fireing State", "Ready To Fire", DebugMode.Competition);
if (xbox.GetRightTrigger() > .5)
{
_t = new Timer();
_t.Start();
_triggerSet = DoubleSolenoid.Value.Forward;
_fstate = FireingState.Fireing;
}
break;
case FireingState.Fireing:
SmartWriter.WriteString("Frisbee Fireing State", "Fireing", DebugMode.Competition);
if (_t.Get() > 1)
{
_t.Reset();
_fstate = FireingState.Reloading;
}
break;
case FireingState.Reloading:
SmartWriter.WriteString("Frisbee Fireing State", "Reloading", DebugMode.Competition);
if (_t.Get() > 2)
{
_t = null;
_fstate = FireingState.ReadyToFire;
}
break;
}
}
