// 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); }
// CheckLoad // * Checks if the winch switch has been pressed. // ----> If yes, makes sure the state of the motors and variables is correct. // * Should be called every tick of an operator control loop to ensure that // the motor is shut off as soon as possible. bool CheckLoad() { if (!IsEnabled()) { winch.Set(0.0); loaded = false; loading = false; return false; } // Switch is normally closed if (loading && winchSwitch.Get()) { winch.Set(0.0); // Stop and reset the timer loadingTimer.Stop(); loadingTimer.Reset(); loaded = true; loading = false; return false; } // Did the timer expire? Yikes shut down the winch if (loadingTimer.HasPeriodPassed(10)) { winch.Set(0.0); // Stop and reset the timer loadingTimer.Stop(); loadingTimer.Reset(); loaded = false; loading = false; return false; } return true; }
// 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); }
// 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); }
// InitiateLoad // * Begins a load of the catapult by running the winch motor. void InitiateLoad() { if (!loaded) { rightWinch.Set(WINCH_FWD); leftWinch.Set(-WINCH_FWD); // Start a timer loadingTimer.Start(); loading = true; } }
// 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; } }