C++ Switch::Get方法代码示例

inline void grabberPositionTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t backOutPtr, uint32_t grabPowerPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer
	Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers
	Talon *grabTalon = (Talon *) grabTalonPtr;
	Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr;
	PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
	bool *isGrabbing = (bool *) isGrabbingPtr;
	bool *backOut = (bool *) backOutPtr;
	double *grabPower = (double *) grabPowerPtr;
	Timer timer;
	timer.Start();

	*isGrabbing = true;//tells robot.cpp that thread is running

	while (grabInnerLimit->Get() && timer.Get() < Constants::grabDelay) {//starts to spin motor to pass startup current
		grabTalon->Set(1);//move in
	}

	while (pdp->GetCurrent(Constants::grabPdpChannel) < *grabPower && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled
		grabTalon->Set(1);
		SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard
	}

	if (*backOut) {
		grabTalon->Set(0);//stop moving
		timer.Reset();
		while (timer.Get() < Constants::liftBackoutTime && joystick->GetRawButton(Constants::pickupCancelButton) == false) {
			grabTalon->Set(-.75);
		}
	}

	grabTalon->Set(0);//stop moving
	timer.Stop();
	*isGrabbing = false;//tells that thread is over
}

inline void grabberChuteTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabOuterLimitPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer
	SmartDashboard::PutBoolean("Breakpoint 0", true);
	Wait(.5);

	Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers
	Talon *grabTalon = (Talon *) grabTalonPtr;
	Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr;
	Switch *grabOuterLimit = (Switch *) grabOuterLimitPtr;
	PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
	bool *isGrabbing = (bool *) isGrabbingPtr;
	Timer timer;
	timer.Start();

	SmartDashboard::PutBoolean("Breakpoint 1", true);
	Wait(.5);

	*isGrabbing = true;//tells robot.cpp that thread is running

	SmartDashboard::PutBoolean("Breakpoint 2", true);
	Wait(.5);

	while (grabOuterLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//starts to spin motor to pass startup current
		//grabTalon->Set(1);//move in
	}

	SmartDashboard::PutBoolean("Breakpoint 3", true);
	Wait(.5);

	timer.Reset();

	while (timer.Get() < Constants::grabChuteTime && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled
		//grabTalon->Set(1);
		SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard
		Wait(.5);
	}

	SmartDashboard::PutBoolean("Breakpoint 4", true);
	Wait(.5);

	grabTalon->Set(0);//stop moving
	timer.Stop();
	*isGrabbing = false;//tells that thread is over
}

	void Test() {
		robotDrive.SetSafetyEnabled(false);
		uint8_t toSend[10];//array of bytes to send over I2C
		uint8_t toReceive[10];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 7; //send 0 to arduino
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		bool isSettingUp = true;

		pickup.setGrabber(-1);
		pickup.setLifter(1);
		while (isSettingUp) {
			isSettingUp = false;
			if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0);
			}
			else {
				isSettingUp = true;
			}

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0);
			}
			else {
				isSettingUp = true;
			}
		}
		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		toSend[0] = 8;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		while(IsTest() && IsEnabled());

		toSend[0] = 0;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
	}

inline void lifterBrakeTaskFunc(uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t isBrakingPtr ...) {//uint is a pointer and not an integer
	Talon *liftTalon = (Talon *) liftTalonPtr;
	Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
	Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
	Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
	bool *isBraking = (bool *) isBrakingPtr;
	PIDController pid(Constants::liftBrakeP, Constants::liftBrakeI, Constants::liftBrakeD, liftEncoder, liftTalon);
	Timer timer;

	//TODO enable and test out brake
	if (!Constants::liftBrakeIsEnabled) {
		return;
	}

	timer.Start();

	while (timer.Get() < Constants::liftBrakeUpTime) {
		liftTalon->Set(Constants::liftBrakeUpPower);
	}

	liftTalon->Set(0);

	pid.Enable();
	pid.SetSetpoint(liftEncoder->Get());

	while (*isBraking) {
		if ((pid.Get() > 0 && liftLowerLimit->Get()) || (pid.Get() < 0 && liftUpperLimit->Get())){
			liftTalon->Set(0);
		}
		else {
			liftTalon->Set(pid.Get());
		}
		SmartDashboard::PutBoolean("Braking", true);


	}

	pid.Disable();
	SmartDashboard::PutBoolean("Braking", false);
	liftTalon->Set(0);
}

inline void lifterPositionTaskFunc(uint32_t joystickPtr, uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t pdpPtr, uint32_t heightPtr, uint32_t isLiftingPtr ...) {//uint is a pointer and not an integer
	double *height = (double *) heightPtr;//initializes double
	Joystick *joystick = (Joystick *) joystickPtr;
	Talon *liftTalon = (Talon *) liftTalonPtr;
	Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
	Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
	Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
	PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
	bool *isLifting = (bool *) isLiftingPtr;

	*isLifting = true;//tells robot.cpp that thread is running

	if (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height) {//checks to see if encoder is higher than it's supposed to be
		if (liftLowerLimit->Get() == false) {//starts to spin motor to pass startup current
			liftTalon->Set(1);//move down
			Wait(Constants::liftDelay);
		}
		while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftLowerLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too high and hasn't hit a limit switch or been cancelled
			SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder in SmartDashboard
			liftTalon->Set(.7);//move down
		}
	}
	else {
		if (liftUpperLimit->Get() == false) {//starts to spin motor to pass startup current
			liftTalon->Set(-1);//move up
			Wait(Constants::liftDelay);
		}
		while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftUpperLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too low and hasn't hit a limit switch or been cancelled
			SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder on SmartDashboard
			liftTalon->Set(-1);//move up
		}
	}

	liftTalon->Set(0);//stop
	*isLifting = false;//tells robot.cpp that thread is finished
}

	/**
	 * Runs the motors with Mecanum drive.
	 */
	void OperatorControl()//teleop code
	{
		robotDrive.SetSafetyEnabled(false);
		gyro.Reset();
		grabEncoder.Reset();
		timer.Start();
		timer.Reset();
		double liftHeight = 0; //variable for lifting thread
		int liftHeightBoxes = 0; //another variable for lifting thread
		int liftStep = 0; //height of step in inches
		int liftRamp = 0; //height of ramp in inches
		double grabPower;
		bool backOut;
		uint8_t toSend[10];//array of bytes to send over I2C
		uint8_t toReceive[10];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 1;//set the byte to send to 1
		i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
		bool isGrabbing = false;//whether or not grabbing thread is running
		bool isLifting = false;//whether or not lifting thread is running
		bool isBraking = false;//whether or not braking thread is running
		float driveX = 0;
		float driveY = 0;
		float driveZ = 0;
		float driveGyro = 0;
		bool liftLastState = false;
		bool liftState = false; //button pressed
		double liftLastTime = 0;
		double liftTime = 0;
		bool liftRan = true;
		Timer switchTimer;
		Timer grabTimer;
		switchTimer.Start();
		grabTimer.Start();


		while (IsOperatorControl() && IsEnabled())
		{
			// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
			// This sample does not use field-oriented drive, so the gyro input is set to zero.

			toSend[0] = 1;
			numToSend = 1;


			driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
			driveY = driveStick.GetRawAxis(Constants::driveYAxis);
			driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
			driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;


			if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
				toSend[0] = 6;
				numToSend = 1;

				if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
					driveY = 0;
					driveZ = 0;
				}
				else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
					driveX = 0;
					driveZ = 0;
				}
				else {//if Z is greater than X and Y, then it will only go in the direction of Z
					driveX = 0;
					driveY = 0;
				}
			}

			if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
				toSend[0] = 7;
				driveZ = 0;//Stops Z while Z lock is pressed
			}

			if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
				driveGyro = 0;//gyro stops while field lock is enabled
			}

			driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
			driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
			driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
			robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive




			if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
				pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
				if(grabTimer.Get() < 1) {
					toSend[0] = 6;
				}
			}
			else {
				pickup.setGrabber(0);
				grabTimer.Reset();
				toSend[0] = 6;
//.........这里部分代码省略.........

	void Autonomous()
	{
		Timer timer;
		float power = 0;
		bool isLifting = false;
		bool isGrabbing = false;
		double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
		double grabPower = Constants::grabAutoCurrent;
		bool backOut;

		uint8_t toSend[1];//array of bytes to send over I2C
		uint8_t toReceive[0];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 2;//set the byte to send to 1
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C

		bool isSettingUp = true;

		//pickup.setGrabber(-1); //open grabber all the way
		pickup.setLifter(0.8);

		while (isSettingUp && IsEnabled() && IsAutonomous()) {
			isSettingUp = false;
			/*if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0); //open until limit
			}
			else {
				isSettingUp = true;
			}*/

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0); //down till bottom
			}
			else {
				isSettingUp = true;
			}
		}

		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		if (grabStick.GetZ() > .8) {
			timer.Reset();
			timer.Start();
			while (timer.Get() < 1) {
				robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
				if(power>-.4){
					power-=0.005;
					Wait(.005);
				}
			}
			robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle());	// STOP!!!
			timer.Stop();
			timer.Reset();
			Wait(1);
		}
		power = 0;

		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		backOut = Constants::autoBackOut;
		pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
		Wait(.005);

		while (isGrabbing && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		liftHeight = 3*Constants::liftBoxHeight;
		Wait(.005);
		pickup.lifterPosition(liftHeight, isLifting, grabStick);
		Wait(.005);
		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous());	// while the nearest object is closer than 2 feet

		timer.Start();

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches  < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) {	// while the nearest object is further than 12 feet
			if (power < .45) { //ramp up the power slowly
				power += .00375;
			}
			robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
			float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12;	// distance from ultrasonic sensor
			SmartDashboard::PutNumber("Distance", distance);	// write stuff to smart dash
			SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
			SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
			SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
			SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
			SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
			SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);

			Wait(.005);
		}
//.........这里部分代码省略.........