public override Int32 Run(ZSDK.Gamepad gamepad) { Boolean Fire = false; if (gamepad is ZSDK.LogitechGamepad) { if ((gamepad as ZSDK.LogitechGamepad).Button_A) { Fire = true; Mode = FireMode.Normal; } else if ((gamepad as ZSDK.LogitechGamepad).Button_Y) { Fire = true; Mode = FireMode.NoReset; } } else { if (gamepad.Buttons[0]) { Fire = true; Mode = FireMode.Normal; } else if (gamepad.Buttons[1]) { Fire = true; Mode = FireMode.NoReset; } } if (!Winch.GetForwardLimitOK() || !Winch.GetReverseLimitOK()) { Winch.Set(0); } if (!Dog.GetForwardLimitOK() || !Dog.GetReverseLimitOK()) { Dog.Set(0); } #if DEBUG Debug.Print(ToString() + " [STATE] " + State.ToString()); #endif switch (State) { default: case BallLaunchState.Unknown: // State 0 // Assume we are started Released Winch.Set(0); Dog.Set(0); State = BallLaunchState.Released; break; case BallLaunchState.Loaded: // State 1 Winch.Set(0); Dog.Set(0); if (Fire) { // Run Dog TalonSRX to fire. Dog.Set(DOG_FIRE_DIRECTION); State = BallLaunchState.Releasing; } break; case BallLaunchState.Loading: // State 2 // Start the winch. // Check for winch limit switch. Boolean WinchLimitOK = Winch.GetReverseLimitOK(); if (!WinchLimitOK) { Winch.Set(0); State = BallLaunchState.Loaded; } else { Boolean WinchStart = DateTime.Now.Subtract(WinchStartTime).Ticks <= WINCH_START_WAIT; Single WinchSpeed = WinchStart ? WINCH_DIRECTION_START : WINCH_DIRECTION_END; #if DEBUG Debug.Print(ToString() + " [WINCH SPEED] " + WinchSpeed.ToString()); #endif Winch.Set(WinchSpeed); } break; case BallLaunchState.Released: // State 3 // Reset the Dog Gear. // Check for dog limit switch. Boolean DogLimitOK = Dog.GetReverseLimitOK(); if (!DogLimitOK) { Dog.Set(0); switch (Mode) { default: case FireMode.Normal: State = BallLaunchState.Loading; break; case FireMode.NoReset: State = BallLaunchState.Loaded; break; } WinchStartTime = DateTime.Now; } else { Dog.Set(DOG_RESET_DIRECTION); } break; case BallLaunchState.Releasing: // State 4 // Wait for a complete fire. Boolean DogFireLimitOK = Dog.GetForwardLimitOK(); if (!DogFireLimitOK) { FireTime = DateTime.Now; State = BallLaunchState.Reseting; } break; case BallLaunchState.Reseting: Int64 fire_ticks = DateTime.Now.Subtract(FireTime).Ticks; if (fire_ticks >= FIRE_WAIT) { State = BallLaunchState.Released; } break; } return(0); }