C++ DoubleSolenoid::Set方法代码示例

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

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

	void shootCatapult()
	{
		if (logitech.GetRawButton(2) && buttonOne.Get()==0 && buttonTwo.Get()==0)
		{
			dogSolenoid.Set(DoubleSolenoid::kReverse);
			Wait(0.5);
			ratchetSolenoid.Set(DoubleSolenoid::kReverse);
			Wait(1);
		}
	}

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

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

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl()
	{
		comp->Start();
		
	
		while(IsOperatorControl())
		{
			
			if(wasenabled == false && IsEnabled() == true)
			{
				
				//arm->initialize();
				
			
				wasenabled = true;
				
				//Wait(.5);
					
			}
			
			wasenabled = IsEnabled();
			d->go();
			
			//r->Set((r->kForward));
			

			
			//ds->PrintfLine(ds->kUser_Line1, "button 11: %d " ,stick->GetRawButton(11));
			//ds->PrintfLine(ds->kUser_Line2, "button 2: %d", stick->GetRawButton(2));
					
			//arm->pickup();
			
			if(stick->GetRawButton(7))
			{
				armlock->Set(armlock->kForward);
			}
			else
			{
				armlock->Set(armlock->kReverse);
			}
			ds->PrintfLine(ds->kUser_Line2, "button 7: %d", stick->GetRawButton(7));
			
			//arm->shoot();
			
		ds->UpdateLCD();
		}
	}

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