C++ DoubleSolenoid类代码示例

	void RawControl::armHeightPnu(bool one, bool two) {
		if (one) {
			low->Set(DoubleSolenoid::kForward);
		} else
			low->Set(DoubleSolenoid::kReverse);
		if (two) {
			high->Set(DoubleSolenoid::kForward);

		} else
			high->Set(DoubleSolenoid::kReverse);
	}

	void loadCatapult()
	{
		if (buttonOne.Get()==1 && buttonTwo.Get()==1 && dogSolenoid.Get()==DoubleSolenoid::kReverse)
		{
			dogSolenoid.Set(DoubleSolenoid::kForward);
			Wait(0.5);
			ratchetSolenoid.Set(DoubleSolenoid::kForward);
			Wait(0.5);
			catapultMotor.Set(1);
		}
	}

    // Test Autonomous
    void TestAutonomous()
    {
        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);
        winch.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(0))
        {
            // 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);

            // 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);
            Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);

            // Step 7: drop the arm for a clean shot and shoot
            arm.Set(DoubleSolenoid::kForward);

            // UNTESTED KICKED OFF FIELD
            Wait(1.0); // Ken
            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);
        winch.Set(0.0);
    }

	void autonomousCatapultRelease()
	{
		if ((leftLimitSwitch.Get()== 0 || rightLimitSwitch.Get()== 0) && winchMotor.Get() == 0)
		{
			stopDriving(); // Stops the drive so the robot doesn't flip on itself or something
			winchMotor.Set(0); // Redundant line for extra safety that can be removed after testing (The winch should already be off)
			dogSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the dog gear
			Wait(0.2); // Giving the pistons time to disengage properly
			ratchetSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the ratchet
			Wait(5); // Waits 5 seconds after shooting before starting to load the catapult
		}
	}

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

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

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

	RobotDemo(void):
		myRobot(LEFT_DRIVE_PWM, RIGHT_DRIVE_PWM),	// these must be initialized in the same order
		stick(1),									// as they are declared above.
		stick2(2),
		gamepad(3),
		collectorMotor(PICKUP_PWM),
		indexerMotor(INDEX_PWM),
		shooterMotor(SHOOTER_PWM),
		armMotor (ARM_PWM),
		shifter(SHIFTER_A,SHIFTER_B),
		greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
		yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
		potentiometer(ARM_ROTATION_POT),
		indexSwitch(INDEXER_SW),
		compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE)
	{
		m_collectorMotorRunning = false;
		m_shooterMotorRunning   = false;
		
		dsLCD = DriverStationLCD::GetInstance();
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);

		dsLCD->UpdateLCD();
		myRobot.SetExpiration(0.1);
		shifter.Set(DoubleSolenoid::kReverse);
	}

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

	// 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 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 TeleopPeriodic(void)
		{
			Scheduler::GetInstance()->Run();

			myDrive_all->TankDrive(drivestick_left->GetY(), drivestick_right->GetY());

			ABCheck = Xbox_Button_A->Get(); // Gets the value of the A button on the xbox
			BBCheck = Xbox_Button_B->Get(); // Gets the value of the B button on the xbox
			if (ABCheck == 1) // If A button is pressed
			{
				FirstSolenoid->Set(DoubleSolenoid::kForward); // Make piston extend
			}
			if (BBCheck == 1) // If B button is pressed
			{
				FirstSolenoid->Set(DoubleSolenoid::kReverse); // Make piston retract
			}
			if ((ABCheck == 0) && (BBCheck == 0)) // If A and B buttons are not pressed
			{
				FirstSolenoid->Set(DoubleSolenoid::kOff); // Make piston stay where it is
			}

			XBCheck = Xbox_Button_X->Get(); // Gets value of the X button on the xbox
			YBCheck = Xbox_Button_Y->Get(); // Get value of the Y button on the xbox

			if (XBCheck == 1)//If X button is pressed
			{
				if (limitSwitch->Get() == 0) // If limitswitch is pressed
				{
					intake_Motor->Set(0); // Stop test motor
				}
				if (limitSwitch->Get() == 1) // If limitswitch is not pressed
				{
					intake_Motor->Set(1); // Test motor goes forward at 0.5 speed
				}
			}

			if (YBCheck == 1) // If Y button is pressed
			{
				intake_Motor->Set(-1); // Test motor goes backward at -0.5 speed
			}

			if ((XBCheck == 0) && (YBCheck == 0)) // If X and Y buttons are not pressed
			{
				intake_Motor->Set(0); // Test motor stops
			}
		}

    /**
     * Initialization code for test mode should go here.
     * 
     * Use this method for initialization code which will be called each time
     * the robot enters test mode.
     */
    void TestInit()
    {
printf(">>> TestInit\n");
#ifdef HAVE_COMPRESSOR
	compressor->Start();
#endif
#ifdef HAVE_ARM
	arm->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_INJECTOR
	injectorL->Set(DoubleSolenoid::kOff);
	injectorR->Set(DoubleSolenoid::kOff);
#endif
#ifdef HAVE_EJECTOR
	ejector->Set(false);
#endif
#ifdef HAVE_LEGS
	legs->Set(false);
#endif

#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
	jagVbus(topWheel1, 0.0);
#endif
#ifdef HAVE_TOP_PWM1
	topWheel1->Set(0.0);
#endif
#ifdef HAVE_TOP_CAN2
	jagVbus(topWheel2, 0.0);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
	jagVbus(bottomWheel1, 0.0);
#endif
#ifdef HAVE_BOTTOM_PWM1
	bottomWheel1->Set(0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
	jagVbus(bottomWheel2, 0.0);
#endif
#endif
printf("<<< TestInit\n");
    }

	void TeleopPeriodic()
	{
		SmartDashboard::PutNumber("joystickX",stick.GetX());
		SmartDashboard::PutNumber("joystickY",stick.GetY());
		//SmartDashboard::PutBoolean("fucking buttons", stick.GetRawButton(1));

		//SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage());
		SmartDashboard::PutBoolean("infra",infra.Get());

		SmartDashboard::PutNumber("accelX",accel.GetX());
		SmartDashboard::PutNumber("accelY",accel.GetY());
		SmartDashboard::PutNumber("accelZ",accel.GetZ());

		servo.Set(
			trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick
		);
		SmartDashboard::PutNumber("servo", servo.Get());


		jag1.Set(stick.GetY());
		jag2.Set(stick.GetY());
		//tal1.Set(stick.GetY());

		SmartDashboard::PutNumber("jag1", jag1.Get());
		SmartDashboard::PutNumber("jag2", jag2.Get());


		/*SmartDashboard::PutNumber("encpos", enc.Get());
		SmartDashboard::PutNumber("encspd", enc.GetRate());*/

		if (stick.GetRawButton(1) && !actuatePressed) {
			pistonVal=!pistonVal;
			piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse);

			actuatePressed = true;
		}
		else if (!stick.GetRawButton(1))
			actuatePressed = false;

		SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward);


	}