// Test Autonomous
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
if (ds->GetDigitalIn(1))
{
// STEP 5: Start the intake motor and backup to our origin position to pick up another ball
InitiateLoad();
intake.Set(-INTAKE_COLLECT);
while (CheckLoad());
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
Wait(1.0); // For the ball to collect
// STEP 6: Shut off the intake, bring up the arm and move to shooting position
intake.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
Wait (1.0); // "Settle down"
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// Step 7: drop the arm for a clean shot and shoot
arm.Set(DoubleSolenoid::kForward);
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// UNTESTED KICKED OFF FIELD
Wait(1.0); // For arm to go down
LaunchCatapult();
}
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
// Real Autonomous
// * Code to be run autonomously for the first ten (10) seconds of the match.
// * Launch catapult
// * Drive robot forward ENCODER_DIST ticks.
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// 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();
}
}
