public override Int32 Run(ZSDK.Gamepad gamepad)
{
Single LeftVal = 0, RightVal = 0;
Boolean Slow = true;
ReadGamepad(gamepad, ref LeftVal, ref RightVal, ref Slow);
#if DEBUG
Debug.Print(ToString() + " [RAW] L:" + LeftVal.ToString() + " - R:" + RightVal.ToString());
#endif
/* Apply RPM conversion to values. */
if (USE_SPEED_MODE)
{
LeftVal *= MAX_RPM;
RightVal *= MAX_RPM;
}
/* Drive in slow mode. */
if (Slow)
{
LeftVal *= SLOW_MULT;
RightVal *= SLOW_MULT;
}
#if DEBUG
Debug.Print(ToString() + " [SET] L:" + LeftVal.ToString() + " - R:" + RightVal.ToString());
#endif
Right.Set(RightVal);
Left.Set(LeftVal);
return(0);
}
private void ReadGamepad(ZSDK.Gamepad gamepad, ref Single LeftVal, ref Single RightVal, ref Boolean Slow)
{
#if DEBUG
for (int idx = 0; idx < gamepad.Axes.Length; ++idx)
{
Debug.Print("[AXIS] [" + idx.ToString() + "] Value: " + gamepad.Axes[idx].ToString());
}
#endif
if (gamepad is ZSDK.LogitechGamepad)
{
/* Tank Drive */
LeftVal = (gamepad as ZSDK.LogitechGamepad).Axis_LY;
RightVal = (gamepad as ZSDK.LogitechGamepad).Axis_RY;
Boolean Fast = (gamepad as ZSDK.LogitechGamepad).Button_LB || (gamepad as ZSDK.LogitechGamepad).Button_RB;
Slow = !Fast; // Herp-Derp :P
}
else
{
/* Arcade fallback */
Single Y = gamepad.Axes[1], X = gamepad.Axes[2];
LeftVal = Y + X;
RightVal = Y - X;
Slow = !gamepad.Buttons[0];
}
Deadband(ref LeftVal);
Deadband(ref RightVal);
}
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);
}
public HERO(RobotModule[] Modules, Gamepad Gamepad)
{
RunLoop = true;
this.Modules = Modules;
this.Gamepad = Gamepad;
}
