// Runs during test mode
// Test
// *
void Test()
{
shifters.Set(DoubleSolenoid::kForward);
leftDriveEncoder.Start();
leftDriveEncoder.Reset();
int start = leftDriveEncoder.Get();
while (IsTest()) {
if (rightStick.GetRawButton(7)) {
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
else {
robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
}
if (gamepad.GetEvent(4) == kEventClosed) {
start = leftDriveEncoder.Get();
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
dsLCD->UpdateLCD();
gamepad.Update();
}
}
// HandleDriverInputs
// * Drive motors according to joystick values
// * Shift (Button 7 on left joystick)
// ----> ASSUMES kForward = high gear
void HandleDriverInputs()
{
if (kEventOpened == rightStick.GetEvent(BUTTON_SHIFT))
{
// Shift into high gear.
shifters.Set(DoubleSolenoid::kForward);
}
else if (kEventClosed == rightStick.GetEvent(BUTTON_SHIFT))
{
// Shift into low gear.
shifters.Set(DoubleSolenoid::kReverse);
}
if (kStateClosed == rightStick.GetState(BUTTON_REVERSE))
{
robotDrive.ArcadeDrive(-rightStick.GetY(), -rightStick.GetX());
}
else
{
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
}
// LaunchCatapult
// * If in the correct state to launch (loaded), launches the catapult.
void LaunchCatapult()
{
if (loaded)
{
rightWinch.Set(WINCH_FWD);
leftWinch.Set(-WINCH_FWD);
for (int i = 0; i < 37; i++)
{
if (IsOperatorControl())
{
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
Wait (0.01);
}
// Wait(CATAPULT_SHOOT_WAIT);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
loaded = false;
}
}
// LaunchCatapult
// * If in the correct state to launch (loaded), launches the catapult.
void LaunchCatapult()
{
if (loaded)
{
rightWinch.Set(WINCH_FWD);
leftWinch.Set(-WINCH_FWD);
// Change if "limit switch not hitting" problem occurs. make 75.
for (int i = 0; i < 37; i++)
{
if (IsOperatorControl())
{
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
Wait (0.01);
}
// Wait(CATAPULT_SHOOT_WAIT);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
loaded = false;
}
}
