C++ Joystick::GetRawButton方法代码示例

	bool getOrange()
	{
		return m_joy->GetRawButton(5);
	}

	bool getBack()
	{
		return m_joy->GetRawButton(7);
	}

	void OperatorControl(void)
	{
		// Teleoperated Code.
		/*double WaitDash = 0.0;
		double FireDash = 0.0;
		double intPause = 0.0;
		
		SmartDashboard::PutNumber("P", 3.0);
		SmartDashboard::PutNumber("W", 0.0);
		SmartDashboard::PutNumber("A", 0.0);
		
		WaitDash = SmartDashboard::GetNumber("P");
		FireDash = SmartDashboard::GetNumber("W");
		intPause = SmartDashboard::GetNumber("A");
		
		SmartDashboard::PutNumber("P", WaitDash);
		SmartDashboard::PutNumber("W", FireDash);
		SmartDashboard::PutNumber("A", WaitDash);
		*/	
		// Enable and start the compressor.
		//compressor->Enabled();
		compressor->Start();

		// Enable drive motor safety timeout.
		myRobot.SetSafetyEnabled(true);

		// Enable watchdog and initial feed.
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(1);
		GetWatchdog().Feed();

		// Set robot in low gear by default. Not active.
		//s[0]->Set(false);
		GetWatchdog().Feed();

		//bool blnShoot = false;
		bool blnLowHang = false;
		bool blnShift = false;

		GetWatchdog().Feed();

		bool blnShooterSpd = false;
		bool blnReverse = false;

		float fltShoot;
		float fltSpeed = 1;

		int intFail = 0;
		
		timerLowHang.Reset();
		timerShift.Reset();
		timerFire.Reset();
		timerShooter.Reset();
		timerDriveCtrl.Reset();
		timerCamera.Reset();
		timerReverse.Reset();

		timerLowHang.Start();
		timerShift.Start();
		timerFire.Start();
		timerShooter.Start();
		timerDriveCtrl.Start();
		timerCamera.Start();
		timerReverse.Start();
		
		GetWatchdog().Feed();

		//sd->sendIOPortData();

		// Local variables.
		//float fltStick1X, fltStick1Y;

		while (IsOperatorControl())
		{
			if(timerReverse.Get() > 0.5)
			{
				if(stick1->GetRawButton(8) && blnReverse == false )
				{
					blnReverse = true;
					GetWatchdog().Feed();
					timerReverse.Reset();
					timerReverse.Start();
				}
				else if(stick1->GetRawButton(9) && blnReverse == true)
				{
					blnReverse = false;
					GetWatchdog().Feed();
					timerReverse.Reset();
					timerReverse.Start();
				}
			}
			if(blnReverse == false)
			{
				fltStick1Y = stick1->GetY();
				fltStick1X = stick1->GetX();
				SmartDashboard::PutBoolean("Reverse",false);
				GetWatchdog().Feed();
			}
			else if(blnReverse == true)
			{
//.........这里部分代码省略.........

	bool getYellow()
	{
		return m_joy->GetRawButton(4);
	}

	void TeleopPeriodic()
	{
		char myString [STAT_STR_LEN];

		if (running)
		{
			enterHoldCommand = joystick->GetRawButton(BUT_JS_ENT_POS_HOLD);
			exitHoldCommand = joystick->GetRawButton(BUT_JS_EXIT_POS_HOLD);

			switch (liftState)
			{
				case raising:
					if (GetLiftLimitSwitchMax())
					{
						SetLiftMotor(MOTOR_SPEED_STOP);
						if(!liftEncFullRanged)
						{
							maxLiftEncDist = liftEncoder->GetDistance();
							liftEncFullRanged = true;
						}
						motorSpeed = -MOTOR_SPEED_DOWN;
						liftState = lowering;
						SetLiftMotor(motorSpeed);
					}

					if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
					{
						liftState = holding;
					}

					break;

				case lowering:
					if (GetLiftLimitSwitchMin())
					{
						SetLiftMotor(MOTOR_SPEED_STOP);
						if(!liftEncZeroed)
						{
							liftEncoder->Reset();
							liftEncZeroed = true;
						}
						motorSpeed=MOTOR_SPEED_UP;
						liftState = raising;
						SetLiftMotor(motorSpeed);
					}

					if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
					{
						liftState = holding;
					}
					break;

				case holding:
					if(!(controlLift->IsEnabled()))
					{
						pidPosSetPoint = SP_RANGE_FRACTION*maxLiftEncDist; //go to the midpoint of the range
						controlLift->SetSetpoint(pidPosSetPoint);
#if BUILD_VERSION == COMPETITION
						controlLift2->SetSetpoint(pidPosSetPoint);
#endif
						controlLift->Enable();
#if BUILD_VERSION == COMPETITION
						controlLift2->Enable();
#endif
					}

					if(exitHoldCommand)
					{
						controlLift->Disable();
#if BUILD_VERSION == COMPETITION
						controlLift2->Disable();
#endif
						motorSpeed = -MOTOR_SPEED_DOWN;
						liftState = lowering;
						SetLiftMotor(motorSpeed);
					}
				break;
			}
		}

		//status
		sprintf(myString, "running: %d\n", running);
		SmartDashboard::PutString("DB/String 0", myString);
		sprintf(myString, "State: %d\n", liftState);
		SmartDashboard::PutString("DB/String 1", myString);
		sprintf(myString, "motorSpeed: %f\n", motorSpeed);
		SmartDashboard::PutString("DB/String 2", myString);
		sprintf(myString, "lift encoder zeroed: %d\n", liftEncZeroed);
		SmartDashboard::PutString("DB/String 3", myString);
		sprintf(myString, "max enc set: %d\n", liftEncFullRanged);
		SmartDashboard::PutString("DB/String 4", myString);
		sprintf(myString, "maxLiftEncDist: %f\n", maxLiftEncDist);
		SmartDashboard::PutString("DB/String 5", myString);
		sprintf(myString, "enc dist: %f\n", liftEncoder->GetDistance());
		SmartDashboard::PutString("DB/String 6", myString);
		sprintf(myString, "pid: %d\n", controlLift->IsEnabled());
		SmartDashboard::PutString("DB/String 7", myString);
		sprintf(myString, "dist to sp : %f\n", DistToSetpoint());
		SmartDashboard::PutString("DB/String 8", myString);
		sprintf(myString, "at sp : %d\n", AtSetpoint());
//.........这里部分代码省略.........

	void OperatorControl()
	{
		compressor.Start();
		DriverStationLCD *screen = DriverStationLCD::GetInstance(); 
		while (IsOperatorControl())
		{
			screen -> PrintfLine(DriverStationLCD::kUser_Line1,"LeftTopM1_%f", logitech.GetRawAxis(2));
			screen -> PrintfLine(DriverStationLCD::kUser_Line2,"LeftBackM2_%f", logitech.GetRawAxis(2));
			screen -> PrintfLine(DriverStationLCD::kUser_Line3,"RightTopM1_%f", logitech.GetRawAxis(4));
			//screen -> PrintfLine(DriverStationLCD::kUser_Line4,"RightTopM2_%f", logitech.GetRawAxis(4));
			//screen -> PrintfLine(DriverStationLCD::kUser_Line5,"Button_%d", buttonOne.Get());
			//screen -> PrintfLine(DriverStationLCD::kUser_Line6,"toggle_%d", togglebuttonOne.Get());
			
			/** CATAPULT **/
			loadCatapult();
			shootCatapult(); //Press A to shoot
			
			/** RETRIEVAL **/
			if(logitech.GetRawButton(6)) //Press Upper Right Trigger to go down
			{		
				armSolenoidOne.Set(DoubleSolenoid::kForward);
				armSolenoidTwo.Set(DoubleSolenoid::kForward);
			}
			else if(logitech.GetRawButton(8)) //Press Lower Right Trigger to go up
			{
				armSolenoidOne.Set(DoubleSolenoid::kReverse);
				armSolenoidTwo.Set(DoubleSolenoid::kReverse);
			}
			else
			{
				armSolenoidOne.Set(DoubleSolenoid::kOff);
				armSolenoidTwo.Set(DoubleSolenoid::kOff);
			}
			
			/**DRIVING**/
			if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer)
			{
				leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1));
				leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1));
			}		
			else
			{
				leftFront.Set(0);
				leftBack.Set(0);
			}
			if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer)
			{
				rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4));
				rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4));
			}
			else
			{
				rightFront.Set(0);
				rightBack.Set(0);
			}
// END TANK DRIVE
			/***********************/
			if ((buttonOne.Get()) == 1)
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!");
			}
			else
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!");
			}
			if ((buttonTwo.Get()) == 1)
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!");
			}
			else
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!");
			}
			if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 0))
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Right Position");
			}
			//both on or off
			else if ((((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 1))||((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 0))
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Middle Position");
			}
			//Left button on && right off
			else if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 1))
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Position");
			}
			
			/***********************/
			screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Pressure:%f", compressor.GetPressureSwitchValue());
			screen -> PrintfLine(DriverStationLCD::kUser_Line5,"Left_%f Right_&f", leftFront.Get(), rightFront.Get());
			screen -> PrintfLine(DriverStationLCD::kUser_Line6,"But6_%d But8_%d", logitech.GetRawButton(6), logitech.GetRawButton(8));
			Wait(0.005);
			screen -> UpdateLCD();
		}
		compressor.Stop();
	}

	void OperatorControl(void)
	{
		AxisCamera &camera = AxisCamera::GetInstance();
		miniBotTime.Start();
		initRobot();
		
		debug("in telop");
		compressor.Start();
		GetWatchdog().SetEnabled(true);
		/*int l1, l2, l3;
		while (IsOperatorControl()) {
			GetWatchdog().Feed();
			//char val = (line1.Get() & 0x01) | (line2.Get() & 0x02) | (line3.Get() & 0x04);
			//if(l1 != line1.Get() || l2 != line2.Get() || l3 != line3.Get()) {
			//	cerr << "change " << (l1 = line1.Get()) << "\t" << (l2 = line2.Get()) << "\t" << (l3 = line3.Get()) << endl;
			//}
			cerr << "Change "<< line1.Get() <<"\t" << line2.Get() << "\t" << line3.Get() << endl;
			Wait(0.2);
		}*/
		char count=0, pneumaticCount=0;
		// was .125 when loop at .025
		lowPass lowSpeed(.04), lowStrafe(.04), lowTurn(.04), lowClaw(.04), lowArm(.04), lowArmLoc(.05);
		
		double ClawLocation=0, ArmLocation=0, OldArmLocation=0;
		
		while (IsOperatorControl() && !IsDisabled())
		{
			GetWatchdog().Feed();
			float speed = -1*stick.GetRawAxis(2);
			float strafe = stick.GetRawAxis(1);
			float turn = stick.GetRawAxis(3);

			if(!stick.GetRawButton(7)) {
				speed /= 2;
				strafe /= 2;
				turn /= 2;
			}
			if(stick.GetRawButton(8)) {
				speed /= 2;
				strafe /= 2;
				turn /= 2;
			}
			if(stick.GetRawButton(2)) {
				speed = 0;
				turn = 0;
			}
			
			Drive(lowSpeed(speed), lowTurn(turn), lowStrafe(strafe));		
			
			
			
#ifndef NDEBUG
			if(stick2.GetRawButton(10)) {
				robotInted = false;
				initRobot();
			}
#endif
			
			
			if(stick2.GetRawButton(7) && (miniBotTime.Get() >= 110 || (stick2.GetRawButton(9) && stick2.GetRawButton(10)))) { // launcher
				// the quick launcher
				MiniBot1a.Set(true);
				MiniBot1b.Set(false);
				
			} 
			if(!stick2.GetRawButton(10) && stick2.GetRawButton(9)) { // deploy in
				MiniBot2a.Set(false);
				MiniBot2b.Set(true);
				//MiniBot2a.Set(false);
				//MiniBot2b.Set(true);
			}
			if(stick2.GetRawButton(5)) { // top deploy out
				MiniBot2a.Set(true);
				MiniBot2b.Set(false);
			}
			
			
			if(stick2.GetRawButton(6)) { // open
				ClawOpen.Set(true);
				ClawClose.Set(false);
			}
			if(stick2.GetRawButton(8)) { // closed
				ClawOpen.Set(false);
				ClawClose.Set(true);
				ClawLocation += 2;
			}
			
			/*156 straight
			 * 56 90 angle
			 * 10 back
			 */
			
			if(stick2.GetRawButton(1)) { // top peg
				ClawLocation = 156;
				ArmLocation = 105;
			}
			if(stick2.GetRawButton(2)) {
				ClawLocation = 111; // the 90angle / middle peg
				ArmLocation = 50;
			}
//.........这里部分代码省略.........

	void TeleopPeriodic(void) 
	{	
		myarm->prepareSignal();
		// Call the drive routine to drive the robot.
		if(rightStick->GetRawButton(1)|| leftStick->GetRawButton(1))
			drive->MecanumDrive_Cartesian(rightStick->GetX()/2,leftStick->GetY()/2,leftStick->GetX()/2,0.00);
		else
			drive->MecanumDrive_Cartesian(rightStick->GetX(),leftStick->GetY(),leftStick->GetX(),0.00);
		GetStateForArm();
		//Wait(.1);
		if(modeArm==DDCArm::kManualOveride)
		{
			myarm->OperateArm(0,0,0,modeArm);
			//Shoulder movement
			if(leftStick->GetRawButton(3))
				myarm->MoveShoulder(1);
			else if(leftStick->GetRawButton(2))
				myarm->MoveShoulder(-1);
			else
				myarm->MoveShoulder(0);
			
			//Elbow Movement
			if(rightStick->GetRawButton(3))
				myarm->MoveElbow(1);
			else if (rightStick->GetRawButton(2))
				myarm->MoveElbow(-1);
			else
				myarm->MoveElbow(0);
			
			//Wrist Movement
			if(rightStick->GetRawButton(4))
				myarm->MoveWrist(-1);
			else if(rightStick->GetRawButton(5))
				myarm->MoveWrist(1);
			else
				myarm->MoveWrist(0);			
		}	
		
		else
			myarm->OperateArm(0.0,0.0,peg,modeArm);
		
		if(leftStick->GetRawButton(4))
			myarm->MoveClaw(-1);
		else if(leftStick->GetRawButton(5))
			myarm->MoveClaw(1);
		else
			myarm->MoveClaw(0);
		
		if(rightStick->GetRawButton(10))
		{
			printf("S: %f \n E: %f \n W: %f \n \n",myarm->GetShoulderVoltage(),myarm->GetElbowVoltage(), myarm->GetWristVoltage());
		}
		
		deploy->OperateDeployment(fire,pull);
		// Send Data to the Driver Station for Monitoring (w/in .
		//sendIOPortData();
		//Wait(.1);
	}

/**
 * Periodic code for teleop mode should go here.
 *
 * Use this method for code which will be called periodically at a regular
 * rate while the robot is in teleop mode.
 */
void RobotDemo::TeleopPeriodic() {
	/*ALTERNATE OPTION: use constants at the top of the code to pass in buttons associated
	 * with various actions.
	 * 
	 *FOR ALTERNATE LAYOUTS: 
	 *have one set of layouts commented out? 
	 *take input from Driver Station I/O tab to determine which set is used? 
	 * */
	
	
	//driver joystick: Extreme 3D
	bool compressor = stickDrive->GetRawButton(10);
	//drive-related
	bool stopDrive = stickDrive->GetRawButton(1);
	bool slowSpeed = stickDrive->GetRawButton(2);
	//arm controls--solenoids involved
	bool extendAll_driver = stickDrive->GetRawButton(9);
	bool retractAll_driver = stickDrive->GetRawButton(11);
	
	//spinner controls
	bool leftDrop_driver = stickDrive->GetRawButton(3);
	bool leftPick_driver = stickDrive->GetRawButton(5);
	bool rightDrop_driver = stickDrive->GetRawButton(4);
	bool rightPick_driver = stickDrive->GetRawButton(6);

	//other joystick: Attack 3
	//arm controls--solenoids involved
	bool extendAll_other = stickOther->GetRawButton(3);
	bool retractAll_other = stickOther->GetRawButton(2);
	bool leftArm_other = (stickOther->GetRawButton(6) || stickOther->GetRawButton(10));
	bool rightArm_other = (stickOther->GetRawButton(7) || stickOther->GetRawButton(11));
	//spinner controls
	bool leftDrop_other = (stickOther->GetRawButton(4) && !stickOther->GetRawButton(1));
	bool leftPick_other = (stickOther->GetRawButton(4) && stickOther->GetRawButton(1));
	bool rightDrop_other = (stickOther->GetRawButton(5) && !stickOther->GetRawButton(1));
	bool rightPick_other = (stickOther->GetRawButton(5) && stickOther->GetRawButton(1));
	
	SmartDashboard::PutBoolean("Left-Spinner", spinnerStatusLeft);
	SmartDashboard::PutBoolean("Right-Spinner", spinnerStatusLeft);
	SmartDashboard::PutBoolean("S1-Status", s1Status);
	SmartDashboard::PutBoolean("S2-Status", s2Status);
	SmartDashboard::PutBoolean("S3-Status", s3Status);
	SmartDashboard::PutNumber("Compressor-Pressure", this->compressor->GetPressureSwitchValue());
	
	
	if(compressor) {
		toggleCompressor();
	}

	++periodicCounter;
	
	float x = stickDrive->GetX();
	float y = stickDrive->GetY();
	float z = (stickDrive->GetRawAxis(3)) / 1.5; //actually twist--capped at 2/3 for speed control
	
	if(abs(x) < .125) x =0;
	if(abs(y) < .125) y =0;
	if(abs(z) < .125) z =0;
	
	if(!slowSpeed) {
		//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
		MecanumDrive(x, y, z, 0.0);
	} 
	
	else if(slowSpeed) { 
		if (x < 0) x = x/3; 
		else x = x/3; 
		
		if (y < 0) y = y/3;
		else y = y/3;
		
		if (z < 0) z = z/3;
		else z = z/3;
		
		//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
		MecanumDrive(x, y, z, 0.0);
	}
	
	//stop robot 
	if (stopDrive){
		stop();
		return;
	}
	
	//extend all arms
	if(extendAll_driver){ 
		extend(s1,s1Status); //spinner arms go out first
		extend(s3,s3Status);
		Wait(.2);
		extend(s2,s2Status);
		return;
	}
	
	//retract all arms
//.........这里部分代码省略.........

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
}

	void TeleopPeriodic()
	{
		/*if(!m_firstSample)
		{
			double v0X = m_curVX;
			double p0X = m_curX;
			double dT = (imu->GetLastSampleTime()-m_preT);
			m_curVX = imu->GetAccelX()*9.8*dT+v0X;
			m_curX = .5*imu->GetAccelX()*9.8*dT*dT+v0X*dT+p0X;
		}
		m_preT = imu->GetLastSampleTime();
		m_firstSample = false;*/

	    ostringstream os;
	    //os << "Value  " << imu->GetAngle()  << endl;
	    //os << "Pitch  " << imu->GetPitch()  << endl;
	    //os << "Roll   " << imu->GetRoll()   << endl;
	    //os << "Yaw    " << imu->GetYaw()    << endl;
	    os << "AccelX " << imu->GetAccelX()*9.8 << endl;
	    os << "AccelY " << imu->GetAccelY()*9.8 << endl;
	    os << "AccelZ " << imu->GetAccelZ()*9.8 << endl;
	    os << "AngleX " << imu->GetAngleX() << endl;
	    os << "AngleY " << imu->GetAngleY() << endl;
	    os << "AngleZ " << imu->GetAngleZ() << endl;
	    os << "LST    " << imu->GetLastSampleTime() << endl;
	    os << "D/s    " << imu->GetRateZ() << endl;

	    // DriverStation::ReportError(os.str());

		//myRobot.TankDrive(-stick.GetRawAxis(1),-stick.GetRawAxis(5),false);
	    float s1 = -stick.GetRawAxis(1);
	    float s2 = -stick.GetRawAxis(5);
	    float m1 = s1;
	    float m2 = s2;

	    m_rightBumper = stick.GetRawButton(6);
	    m_leftBumper = stick.GetRawButton(5);

	    if(m_rightBumper && !m_prevRightBumper)
	    {
	    	m_correcting1 = !m_correcting1;
	    	m_correcting2 = false;
	    }

	    if(m_leftBumper && !m_prevLeftBumper)
	    {
	    	m_correcting1 = false;
	    	m_correcting2 = !m_correcting2;
	    }

	    if(m_correcting1){
	    	os<<"CORRECTED: 1";
	    	m1 -= imu->GetRateZ()*0.9;
	    	m2 += imu->GetRateZ()*0.9;
	    }else if(m_correcting2){
	    	os<<"CORRECTED: 2";
	    	m1 -= imu->GetRateZ()*0.7;
	    	m2 += imu->GetRateZ()*0.7;
	    }

	    m_prevRightBumper = m_rightBumper;

		myRobot.TankDrive(m1,m2,false);

		os<<endl<<endl;
		cerr << os.str();

		//SmartDashboard::PutData("IMU", imu);
	}

   /**
    * Runs the motors with arcade steering.
    */
   void OperatorControl(void)
   {
       myRobot->SetSafetyEnabled(true);
       while (IsOperatorControl())
       {
           bool setLimit;
           double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise
           double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1
           //For shooter
           /*if (stick.GetRawButton(1) == true) {
                           //back of the robot is moved forward by pushing forward on the joystick
                           myRobot.ArcadeDrive((stick.GetY()),
                                   (stick.GetX()), false); // inverted drive control    
                       } else {
                           //front of the robot is moved forward by pushing forward on the joystick
                           myRobot.ArcadeDrive(-(stick.GetY()),
                                   (stick.GetX()), false); // normal drive control        
                       }
                       */
           //For manual button speed control, this sets the speed
            if (stick->GetRawButton(4) == true) {
            	
            	
            	
                      cim1->Set(cimValue1); //use the value from the throttle to set cim speed
                      cim2->Set(cimValue2);//Get speed from throttle, and then scale it
                      setLimit = true;
                      
                      //Open Ball Stopper
         
                      
                       }
                       else {
                           cim1->Set(0.0);
                           cim2->Set(0.0);
                           setLimit = false;
                           
                      //Close Ball Stopper  
      
                       }

                          
           
           //For precisebelt pickup
           if (stick->GetRawButton(1) == true) {
               //back of the robot is moved forward by pushing forward on the joystick
               if (setLimit == true) {
                   JagBelt->ArcadeDrive(0.1,
                       (stick->GetX()), false); // inverted drive control
               } else {
                   JagBelt->ArcadeDrive((stick->GetY()),
                       (stick->GetX()), false); // inverted drive control
               }
           } else {
               //front of the robot is moved forward by pushing forward on the joystick
               if (setLimit == true) {
                                   JagBelt->ArcadeDrive(-0.1,
                                       (stick->GetX()), false); // inverted drive control
                               } else {
                                   JagBelt->ArcadeDrive(-(stick->GetY()),
                                       (stick->GetX()), false); // inverted drive control
                               }        
           }
           //For normal belt pickup
           /*if (stick->GetRawButton(6) == true) {
                                       JagBelt->Drive(1.0, 0); //opens gripper
                                       
                                   } else {
                                       JagBelt->Drive(0.0, 0); //closes gripper
                                   }
                                   */
                       
                       
           
           //For drive
           
           if (rightstick->GetRawButton(1) == true) {
                           //back of the robot is moved forward by pushing forward on the joystick
                       if (rightstick->GetRawButton(10) == true) {
                           precisionMode= true;
                       }
                       else {
                           precisionMode= false;
                       }
                           myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control    
                       } else
                       {
                           if (rightstick->GetRawButton(10) == true) {
                                                       precisionMode= true;
                                                   }
                                                   else {
                                                       precisionMode= false;
                                                   }
                           //front of the robot is moved forward by pushing forward on the joystick
                           myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control        
                       }
           
//.........这里部分代码省略.........

	void TeleopPeriodic() override {
		float leftPower, rightPower; // Get the values for the main drive train joystick controllers
		leftPower = -leftjoystick->GetY();
		rightPower = -rightjoystick->GetY();

		float multiplier; // TURBO mode
		if (rightjoystick->GetRawButton(1))
		{
			multiplier = 1;
		} else {
			multiplier = 0.5;
		}

		// wtf is a setpoint - it's an angle to turn to
		if (leftjoystick->GetRawButton(6)) {
			turnController->Reset();
			turnController->SetSetpoint(0);
			turnController->Enable();
			ahrs->ZeroYaw();
			//ahrs->Reset();
		}

		// Press button to auto calculate angle to rotate bot to nearest ball
//		if(leftjoystick->GetRawButton(99))
//		{
//			ahrs->ZeroYaw();
//			turnController->Reset();
//			turnController->SetSetpoint(mqServer.GetDouble("angle"));
//			turnController->Enable();
//			aimState = 1;
//		}

		switch(aimState)
		{
		default:
		case 0: // No camera assisted turning
			//Drive straight with one controller, else: drive with two controllers
			if (leftjoystick->GetRawButton(1)) {
				drive->TankDrive(leftPower * multiplier, leftPower * multiplier,
						false);
			} else if (leftjoystick->GetRawButton(2)) {
				drive->TankDrive(leftPower * multiplier + rotateRate,
						leftPower * multiplier + -rotateRate, false);
			} else {
				drive->TankDrive(leftPower * multiplier, rightPower * multiplier,
						false);
			}
			break;
		case 1: // Camera assisted turning, deny input from controllers
			drive->TankDrive(rotateRate, -rotateRate, false);
			if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {
				aimState = 0; // Finished turning, auto assist off
				turnController->Disable();
				turnController->Reset();
			}
			break;
		}

		// That little flap at the bottom of the joystick
		float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);
		// Depending on the button, our intake will eat or shoot the ball
		if (controlstick->GetRawButton(2)) {
			intake->Set(-scaleIntake);
			shooter->Set(scaleIntake);
		} else if (controlstick->GetRawButton(1)) {
			intake->Set(scaleIntake);
			shooter->Set(-scaleIntake);
		} else {
			intake->Set(0);
			shooter->Set(0);
		}

		// Control the motor that lifts and descends the intake bar
		float intake_lever_power = 0;
		if (controlstick->GetRawButton(6)) {
			manual = true;
			intake_lever_power = .3;
//			intakeLever->Set(.30); // close
		} else if (controlstick->GetRawButton(4)) {
			manual = true;
			intake_lever_power = -.4;
//			intakeLever->Set(-.40); // open
		} else if (controlstick->GetRawButton(3)){
			manual = true;
			intake_lever_power = -scaleIntake;
//			intakeLever->Set(-scaleIntake);
		} else if (controlstick->GetRawButton(5)) {
			manual = true;
			intake_lever_power = scaleIntake;
//			intakeLever->Set(scaleIntake);
		} else {
			if (manual) {
				manual = false;
				lastLiftPos = intakeLever->GetEncPosition();
				intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);
				intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
				intakeLever->SetPID(1, 0.001, 0.0);
				intakeLever->EnableControl();
			}
			intake_hold = true;
//.........这里部分代码省略.........

   	 bool getStart()
	{
		return m_joy->GetRawButton(8);
	}

	void OperatorControl()
		{
			while (IsOperatorControl() && IsEnabled())
			{
				robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY()));
				SmartDashboard::PutNumber("StickZ",stick.GetZ());
				SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ()));


				finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y)));
//				scalerValue = stick.GetRawAxis(3);
				arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y)));
				manualShooter();
				shootingModes();
				toggleIntake();
				toggleIntakeMode();
				setScalerValue();
//				double volts = ourRangefinder->GetVoltage();
//				SmartDashboard::PutNumber("Voltage",volts);
				//t_motor.Set(stick2.GetZ());
				//t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis));
//				t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis));
//				finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis));
//				arm_Motor.Set(stick2.GetY());

		//		Current Control mode Debug
				SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent());
				SmartDashboard::PutNumber("Position",t_motor.GetPosition());
//				t_motor.Set(stick.GetAxis(Joystick::Slider));
//				if (stick.GetRawButton(3) == true){
//					t_motor.SetPosition(10000);
//				}

//			if (stick2.GetRawButton(7) == true and buttonpress == false){
//

//									}
//			else if (stick2.GetRawButton(3) == false and buttonpress == true){
//					buttonpress = false;// drive with arcade style (use right stick)
//
//						}
			SmartDashboard::PutBoolean("buttonpress state",buttonpress);

			if (stick2.GetRawButton(buttonBack) == true){
				stateDisarmed = true;
				stateArming1 = false;
				stateArming2 = false;
				stateArmed = false;
				stateFiring1 = false;
				stateFiring2 = false;
				t_motor.SetPosition(0);

			}
			toggleIntake();
//			if (stick2.GetRawButton(5) == true){
//				intake_Spin_Motor.Set(1);
//			}
//			if (stick2.GetRawButton(5) == false){
//				intake_Spin_Motor.Set(0);
//			}
			if (stick.GetRawButton(2) == true and buttonpress2 == false){
				myServo->SetAngle(175);
			}
			if (stick.GetRawButton(2) == false and buttonpress2 == true){
				myServo->SetAngle(5);


			}
//			servoSetPos(servoState);
//			myServo->Set(.5);
			Wait(0.005);// wait for a motor update time
//			motorSetPos(launcherState);
		}
	}