void OperatorControl()
{
// Loop counter to ensure that the program is running (debug helper
// that can be removed when things get more stable)
int sanity, bigSanity = 0;
gamepad.Update();
while (IsOperatorControl() && IsEnabled())
{
controls = Controls::GetInstance();
controls->SetSpeed(LEFT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
controls->SetSpeed(RIGHT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
gamepad.Update();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity);
dsLCD->UpdateLCD();
sanity++;
if (0 == sanity % 20)
{
bigSanity++;
}
Wait(0.05); // wait for a motor update time
}
}
// 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();
}
}
void HandleCollectorInputs ()
{
if (false == m_shooterMotorRunning)
{
if (kEventClosed == gamepad.GetEvent(BUTTON_COLLECTOR_FWD))
{
collectorMotor.Set(COLLECTOR_FWD);
m_collectorMotorRunning = true;
}
else if (kEventOpened == gamepad.GetEvent(BUTTON_COLLECTOR_FWD))
{
collectorMotor.Set(0.0);
m_collectorMotorRunning = false;
}
else if (kEventClosed == gamepad.GetEvent(BUTTON_COLLECTOR_REV))
{
collectorMotor.Set(COLLECTOR_REV);
m_collectorMotorRunning = true;
}
else if (kEventOpened == gamepad.GetEvent(BUTTON_COLLECTOR_REV))
{
collectorMotor.Set(0.0);
m_collectorMotorRunning = false;
}
}
}
// 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);
}
// 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);
}
// HandleShooter
// * Manage winch motor state.
// * Toggles collection and eject mode (Gamepad button 4)
// ----> ASSUMES positive values = collecting
void HandleShooter()
{
if (gamepad.GetEvent(BUTTON_LOAD) == kEventClosed)
{
InitiateLoad();
}
if (loading)
{
CheckLoad();
}
if (gamepad.GetEvent(BUTTON_SHOOT) == kEventClosed)
{
LaunchCatapult();
}
}
void HandleShooterInputs()
{
if (shooterTimer.HasPeriodPassed(SPINUP_TIME))
{
shooterTimer.Stop();
shooterTimer.Reset();
indexerMotor.Set(INDEXER_FWD);
}
else if (!m_collectorMotorRunning && !m_shooterMotorRunning)
{
if (kEventClosed == gamepad.GetEvent(BUTTON_SHOOTER))
{
shooterMotor.Set(SHOOTER_FWD);
shooterTimer.Start();
m_shooterMotorRunning = true;
}
}
else
{
if (indexSwitch.GetEvent() == kEventOpened)
{
indexerMotor.Set(0.0);
shooterMotor.Set(0.0);
m_shooterMotorRunning = 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);
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
}
}
// HandleArm
// * Manage solenoids for arm up-down
// ----> ASSUMES kForward on DoubleSolenoid is the down position.
// * Handle intake motors
void HandleArm()
{
if (gamepad.GetEvent(BUTTON_ARM) == kEventClosed && armDown)
{
arm.Set(DoubleSolenoid::kReverse);
armDown = false;
}
else if (gamepad.GetEvent(BUTTON_ARM) == kEventClosed)
{
arm.Set(DoubleSolenoid::kForward);
armDown = true;
}
if (gamepad.GetDPadEvent(BUTTON_INTAKE_COLLECT) == kEventClosed)
{
intake.Set(INTAKE_COLLECT);
}
else if (gamepad.GetDPadEvent(BUTTON_INTAKE_COLLECT) == kEventOpened)
{
intake.Set(0.0);
}
if(gamepad.GetDPadEvent(BUTTON_INTAKE_EJECT) == kEventClosed)
{
intake.Set(INTAKE_EJECT);
}
if (gamepad.GetDPadEvent(BUTTON_INTAKE_EJECT) == kEventOpened)
{
intake.Set(0.0);
}
}
// HandleEject
// * Handle eject piston.
void HandleEject()
{
if (gamepad.GetEvent(BUTTON_PASS) == kEventClosed)
{
ejectTimer.Start();
eject.Set(DoubleSolenoid::kForward);
}
if (ejectTimer.HasPeriodPassed(EJECT_WAIT))
{
ejectTimer.Stop();
ejectTimer.Reset();
eject.Set(DoubleSolenoid::kReverse);
}
}
void HandleResetButton(void)
{
if (gamepad.GetEvent(BUTTON_STOP_ALL) == kEventClosed)
{
collectorMotor.Set(0.0);
m_collectorMotorRunning = false;
shooterMotor.Set(0.0);
m_shooterMotorRunning = false;
indexerMotor.Set(0.0);
armMotor.Set(0.0);
jogTimer.Stop();
jogTimer.Reset();
m_jogTimerRunning = false;
}
}
void HandleShooterInputs()
{
if (!m_collectorMotorRunning && !m_shooterMotorRunning)
{
if (kEventClosed == gamepad.GetEvent(BUTTON_SHOOTER))
{
shooterMotor.Set(-0.5);
indexerMotor.Set(-0.5);
m_shooterMotorRunning = true;
}
}
else
{
if (indexSwitch.GetEvent() == kEventClosed)
{
indexerMotor.Set(0.0);
shooterMotor.Set(0.0);
m_shooterMotorRunning = false;
}
}
}
void HandleArmInputs(void)
{
if (gamepad.GetLeftY() < -0.1)
{
if (potentiometer.GetVoltage() < 4.5)
{
armMotor.Set(1.0);
}
else
{
armMotor.Set(0.0);
}
}
else if (gamepad.GetLeftY() > 0.1)
{
if (potentiometer.GetVoltage() > .5)
{
armMotor.Set(-1.0);
}
else
{
armMotor.Set(0.0);
}
}
else
{
armMotor.Set(0.0);
}
if (gamepad.GetEvent(BUTTON_CLAW_1_LOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_1_UNLOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kReverse);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_LOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_UNLOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kReverse);
}
}
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 Test()
{
menuType currentMenu = TOP;
menuType newMenu = TOP;
BaseMenu * menus[NUM_MENU_TYPE];
menus[TOP] = new TopMenu;
menus[ANALOG] = new AnalogMenu;
menus[DIGITAL_TOP] = new DigitalMenu;
menus[SOLENOID] = new SolenoidMenu;
menus[DIGITAL_PWM] = new PWMMenu;
menus[DIGITAL_IO] = new DigitalIOMenu;
menus[DIGITAL_RELAY] = new RelayMenu;
menus[DIGITAL_IO_STATE] = new DigitalIOStateMenu;
menus[DIGITAL_IO_CLOCK] = new DigitalIOClockMenu;
menus[DIGITAL_IO_ENCODER] = new DigitalIOEncoderMenu;
// Write out the TOP menu for the first time
menus[currentMenu]->UpdateDisplay();
// Initialize the button states on the gamepad
gamepad.Update();
// Loop counter to ensure that the program us running (debug helper
// that can be removed when things get more stable)
int sanity = 0;
while (IsTest())
{
// The dpad "up" button is used to move the menu pointer up one line
// on the LCD display
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kUp))
{
menus[currentMenu]->HandleIndexUp();
}
// The dpad "down" button is used to move the menu pointer down one line
// on the LCD display
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kDown))
{
menus[currentMenu]->HandleIndexDown();
}
// The dpad left button is used to exit a submenu when the menu pointer
// points to the "back" menu item and to decrease a value (where
// appropriate) on any other menu item.
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kLeft))
{
newMenu = menus[currentMenu]->HandleSelectLeft();
}
// Theoretically, both the select buttons could be pressed in the
// same 10 msec window. However, if using the dpad on the game
// game controller this is physically impossible so we don't
// need to worry about a previous value of newMenu being
// overwritten in the next bit of code.
// The dpad right button is used to enter a submenu when the menu pointer
// points to a submenu item and to increase a value (where appropriate)
// on any other menu item.
if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kRight))
{
newMenu = menus[currentMenu]->HandleSelectRight();
// Handle change from one menu to a sub menu
if (newMenu != currentMenu)
{
// When we enter a menu we need to set the record the
// menu to return to. We do *not* want to do this when
// returning from a menu to its calling menu.
menus[newMenu]->SetCallingMenu(currentMenu);
}
}
// Handle change from one menu to another
if (newMenu != currentMenu)
{
menus[newMenu]->UpdateDisplay();
currentMenu = newMenu;
}
// Set the motor speed(s) (if any have been enabled via the Digital PWM menu)
menus[DIGITAL_PWM]->SetSpeed(-1.0 * gamepad.GetRightY());
// Update gamepad button states
gamepad.Update();
// Update the display (we do this on every loop pass because some menus
// (analog, for example) need to have values updated even when there are
// no dpad events to handle)
menus[currentMenu]->UpdateDisplay();
// Dump the sanity time value to the LCD
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Sanity: %d", sanity);
dsLCD->UpdateLCD();
sanity++;
// Run the loop every 50 msec (20 times per second)
Wait(0.050);
}
}
void HandleArmInputs(void)
{
if (!m_jogTimerRunning)
{
if (gamepad.GetLeftY() < -0.1)
{
if (potentiometer.GetVoltage() < 4.5)
{
armMotor.Set(ARM_FWD);
}
else
{
armMotor.Set(0.0);
}
}
else if (gamepad.GetLeftY() > 0.1)
{
if (potentiometer.GetVoltage() > .5)
{
armMotor.Set(ARM_REV);
}
else
{
armMotor.Set(0.0);
}
}
else if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kUp))
{
armMotor.Set(ARM_FWD);
jogTimer.Start();
jogTimer.Reset();
m_jogTimerRunning = true;
}
else if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kDown))
{
armMotor.Set(ARM_REV);
jogTimer.Start();
jogTimer.Reset();
m_jogTimerRunning = true;
}
else
{
armMotor.Set(0.0);
}
}
else if (jogTimer.HasPeriodPassed(JOG_TIME))
{
armMotor.Set(0);
jogTimer.Stop();
jogTimer.Reset();
m_jogTimerRunning = false;
}
if (gamepad.GetEvent(BUTTON_CLAW_1_LOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_1_UNLOCKED) == kEventClosed)
{
greenClaw.Set(DoubleSolenoid::kReverse);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_LOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kForward);
}
else if (gamepad.GetEvent(BUTTON_CLAW_2_UNLOCKED) == kEventClosed)
{
yellowClaw.Set(DoubleSolenoid::kReverse);
}
}
