// 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();
}
}
// RegisterButtons
// * Register all the buttons required
void RegisterButtons()
{
rightStick.EnableButton(BUTTON_SHIFT);
rightStick.EnableButton(BUTTON_REVERSE);
gamepad.EnableButton(BUTTON_LOAD);
gamepad.EnableButton(BUTTON_SHOOT);
gamepad.EnableButton(BUTTON_ARM);
gamepad.EnableButton(BUTTON_PASS);
}
// Code to be run during the remaining 2:20 of the match (after Autonomous())
//
// OperatorControl
// * Calls all the above methods
void OperatorControl()
{
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
* the only way we could get out robot code to work (reliably). Should this be set to false?
*/
robotDrive.SetSafetyEnabled(false);
Timer clock;
int sanity = 0;
int bigSanity = 0;
loading = false;
loaded = winchSwitch.Get();
RegisterButtons();
gamepad.Update();
leftStick.Update();
compressor.Start();
while (IsOperatorControl() && IsEnabled())
{
clock.Start();
HandleDriverInputs();
HandleShooter();
HandleArm();
// HandleEject();
while (!clock.HasPeriodPassed(LOOP_PERIOD)); // add an IsEnabled???
clock.Reset();
sanity++;
if (sanity >= 100)
{
bigSanity++;
sanity = 0;
dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "%d", bigSanity);
}
gamepad.Update();
leftStick.Update();
dsLCD->UpdateLCD();
}
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
gamepad.EnableButton(BUTTON_COLLECTOR_REV);
gamepad.EnableButton(BUTTON_SHOOTER);
gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
gamepad.EnableButton(BUTTON_STOP_ALL);
gamepad.EnableButton(BUTTON_JOG_FWD);
gamepad.EnableButton(BUTTON_JOG_REV);
stick2.EnableButton(BUTTON_SHIFT);
// Set inital states for all switches and buttons
gamepad.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
stick2.Update();
// Set initial states for all pneumatic actuators
shifter.Set(DoubleSolenoid::kReverse);
greenClaw.Set(DoubleSolenoid::kReverse);
yellowClaw.Set(DoubleSolenoid::kReverse);
compressor.Start ();
while (IsOperatorControl())
{
gamepad.Update();
stick2.Update();
indexSwitch.Update();
greenClawLockSwitch.Update();
yellowClawLockSwitch.Update();
HandleCollectorInputs();
HandleDriverInputsManual();
HandleArmInputs();
HandleShooterInputs();
HandleResetButton();
UpdateStatusDisplays();
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void HandleDriverInputsManual(void)
{
myRobot.ArcadeDrive(stick);
if(kEventClosed == stick2.GetEvent(BUTTON_SHIFT))
{
// Shift into high gear.
shifter.Set(DoubleSolenoid::kForward);
}
else if(kEventOpened == stick2.GetEvent(BUTTON_SHIFT))
{
// Shift into low gear.
shifter.Set(DoubleSolenoid::kReverse);
}
}
// HandleDriverInputs
// * Drive motors according to joystick values
// * Shift (Button 7 on left joystick)
// ----> ASSUMES kForward = high gear
void HandleDriverInputs()
{
if(kEventOpened == leftStick.GetEvent(BUTTON_SHIFT))
{
// Shift into high gear.
shifters.Set(DoubleSolenoid::kForward);
}
else if(kEventClosed == leftStick.GetEvent(BUTTON_SHIFT))
{
// Shift into low gear.
shifters.Set(DoubleSolenoid::kReverse);
}
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
// RegisterButtons
// * Register all the buttons required
void RegisterButtons()
{
leftStick.EnableButton(BUTTON_SHIFT);
gamepad.EnableButton(BUTTON_LOAD);
gamepad.EnableButton(BUTTON_SHOOT);
gamepad.EnableButton(BUTTON_ARM);
gamepad.EnableButton(BUTTON_PASS);
}
// LaunchCatapult
// * If in the correct state to launch (loaded), launches the catapult.
void LaunchCatapult()
{
if (loaded)
{
winch.Set(WINCH_FWD);
for (int i = 0; i < 75; i++)
{
if (IsOperatorControl())
{
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
Wait (0.01);
}
// Wait(CATAPULT_SHOOT_WAIT);
winch.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;
}
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
gamepad.EnableButton(BUTTON_COLLECTOR_REV);
gamepad.EnableButton(BUTTON_SHOOTER);
gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
stick2.EnableButton(BUTTON_SHIFT);
// Set inital states for all switches and buttons
gamepad.Update();
indexSwitch.Update();
stick2.Update();
// Set initial states for all pneumatic actuators
shifter.Set(DoubleSolenoid::kReverse);
greenClaw.Set(DoubleSolenoid::kReverse);
yellowClaw.Set(DoubleSolenoid::kReverse);
compressor.Start ();
while (IsOperatorControl())
{
gamepad.Update();
stick2.Update();
indexSwitch.Update();
HandleCollectorInputs();
HandleDriverInputsManual();
HandleArmInputs();
HandleShooterInputs();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f", potentiometer.GetVoltage()); dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
void UpdateStatusDisplays(void)
{
// Joystick values
SmartDashboard::PutNumber("stickX", stick.GetX());
SmartDashboard::PutNumber("stickY", stick.GetY());
SmartDashboard::PutBoolean("shift", stick2.GetState(BUTTON_SHIFT) ? kStateClosed : kStateOpen);
// Shooter/Indexer values
SmartDashboard::PutBoolean("indexSwitch", indexSwitch.GetState() ? kStateClosed : kStateOpen);
SmartDashboard::PutNumber("shooterMotor", shooterMotor.Get());
SmartDashboard::PutNumber("indexerMotor", indexerMotor.Get());
// Misc Motor Values
SmartDashboard::PutNumber("collectorMotor", collectorMotor.Get());
SmartDashboard::PutNumber("armMotor", armMotor.Get());
// Arm position via potentiometer voltage (2.5 volts is center position)
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "Voltage: %3.1f", potentiometer.GetVoltage());
SmartDashboard::PutNumber("Potentiometer", potentiometer.GetVoltage());
// Claw lock states
SmartDashboard::PutBoolean("Green Claw State", greenClawLockSwitch.GetState());
SmartDashboard::PutBoolean("Yellow Claw State", yellowClawLockSwitch.GetState());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "Green : %s",
greenClawLockSwitch.GetState() ? "Locked" : "Unlocked");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line5, "Yellow: %s",
yellowClawLockSwitch.GetState() ? "Locked" : "Unlocked");
// Pneumatic shifter count
SmartDashboard::PutNumber("Shift Count", m_shiftCount);
// State viariables
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "CMR: %s SMR: %s JTR: %s",
m_collectorMotorRunning ? "T" : "F",
m_shooterMotorRunning ? "T" : "F",
m_jogTimerRunning ? "T" : "F");
}
