// 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); }
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; } } }
// 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); }
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"); }
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 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; } } }