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

/**
 * Get the twist value of the current joystick.
 * This depends on the mapping of the joystick connected to the current port.
 *
 * @param port The USB port for this joystick.
 */
float GetTwist(UINT32 port)
{
    Joystick *stick = getJoystick(port);
    if (stick == NULL)
        return 0;
    return stick->GetTwist();
}

/**
 * @brief Sets a cached joystick value.
 * @param joy_id Which joystick to set the cached value for.
 * @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
 */
void Proxy::SetJoystick(int joy_id, Joystick & stick)
{
	wpi_assert(joy_id < NUMBER_OF_JOYSTICKS+1 && joy_id >= 0);
	char tmp[32];
	sprintf(tmp, "Joy%d", joy_id);
	string name = tmp;
	if(!disableAxes[joy_id-1]) {
		set(name + 'X', stick.GetX());
		set(name + 'Y', stick.GetY());
		set(name + 'Z', stick.GetZ());
		set(name + 'R', stick.GetTwist());
		set(name + 'T', stick.GetThrottle());
		for(int AxisId=1; AxisId<=6; AxisId++) {
			sprintf(tmp, "%sA%d", name.c_str(), AxisId);
			set(tmp, stick.GetRawAxis(AxisId));
		}
	} else {
		if(!stick.GetRawButton(disableAxes[joy_id-1])) {
			set(name + 'X', stick.GetX());
			set(name + 'Y', stick.GetY());
			set(name + 'Z', stick.GetZ());
			set(name + 'R', stick.GetTwist());
			set(name + 'T', stick.GetThrottle());
			for(int AxisId=1; AxisId<=6; AxisId++) {
				sprintf(tmp, "%sA%d", name.c_str(), AxisId);
				set(tmp, stick.GetRawAxis(AxisId));
			}
		}
	}
	
	if(!disableButtons[joy_id-1]) {
		for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
			sprintf(tmp, "%sB%d", name.c_str(), i);
			set(tmp,stick.GetRawButton(i));
		}
		set(name + "BT", stick.GetTrigger());
	} else {
		if(!stick.GetRawButton(disableButtons[joy_id-1])) {
			for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
				sprintf(tmp, "%sB%d", name.c_str(), i);
				set(tmp,stick.GetRawButton(i));
			}
			set(name + "BT", stick.GetTrigger());
		}
	}
}

	void OperatorControl()
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl() && IsEnabled())
		{
			//MECANUM DRIVE
			float yValue = stick->GetY();
			float xValue = stick->GetX();
			float rotation = stick->GetTwist();
			myRobot.MecanumDrive_Cartesian(xValue, yValue, rotation, 0.0);
			Wait(0.005);
		}
	}

	/** Function to retrieve the user inputs
	 * Inputs (For all user input modes):
	 * * Potentiometers 
	 * * Limit switches
	 * * the requested state for the robot (from the joystick buttons)
	 * For manual mode only:
	 * * The intake and ejection wheel speeds - based on the operator joystick throttle
	 * * The arm position - based on the operator joystick Y axis  
	 */
    void GetUserInputs(RobotStates_t &requestedState)
    {
    	float throttleRaw = 0.0;
        //Read potentiometer goes from -5 to 28
        potentiometerValueRight = armPotentiometer1.GetValue();
        //potentiometerValueLeft = armPotentiometer2.GetValue();
        ballCaughtLimitSwitch1 = limitSwitch1.Get(); //TODO 0 is no ball 1 is ball 
        //Read Joystick state buttons
        requestedState = ReadJoystickButtons();
        throttleRaw = DeadZone(Operator_Control_Stick.GetTwist(), EJWHEEL_MM_DEADZONE_VAL);
        manualModeThrottlePosition = (throttleRaw * -1); //invert throttle value to match the joystick
		manualModeArmPosition = DeadZone(Operator_Control_Stick.GetY(), ARM_MM_DEADZONE_VAL);
		
    }

 /**
  * Runs the motors with Mecanum drive.
  */
 void OperatorControl()
 {
     robotDrive.SetSafetyEnabled(false);
     while (IsOperatorControl() && IsEnabled())
     {
         bool reset_yaw_button_pressed = stick.GetRawButton(1);
         if ( reset_yaw_button_pressed ) {
             ahrs->ZeroYaw();
         }
         bool rotateToAngle = false;
         if ( stick.GetRawButton(2)) {
             turnController->SetSetpoint(0.0f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(3)) {
             turnController->SetSetpoint(90.0f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(4)) {
             turnController->SetSetpoint(179.9f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(5)) {
             turnController->SetSetpoint(-90.0f);
             rotateToAngle = true;
         }
         double currentRotationRate;
         if ( rotateToAngle ) {
             turnController->Enable();
             currentRotationRate = rotateToAngleRate;
         } else {
             turnController->Disable();
             currentRotationRate = stick.GetTwist();
         }
         try {
             /* Use the joystick X axis for lateral movement,          */
             /* Y axis for forward movement, and the current           */
             /* calculated rotation rate (or joystick Z axis),         */
             /* depending upon whether "rotate to angle" is active.    */
             robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
                                               currentRotationRate ,ahrs->GetAngle());
         } catch (std::exception ex ) {
             std::string err_string = "Error communicating with Drive System:  ";
             err_string += ex.what();
             DriverStation::ReportError(err_string.c_str());
         }
         Wait(0.005); // wait 5ms to avoid hogging CPU cycles
     }
 }

	void driveTrainValues(void) {
		
		//Testing the encoder...
		//Get the encoder value
		//encoder_value = right_encoder->Get();
		
		//Print encoder value
		//printf("right encoder value: %f ", encoder_value);
		
		
		//Shifting gears...
		//Low gear
		if (gamepad->GetRawButton(5))
		{
			printf("The left button is being pressed.\n");
			drivetrain_pressure->Set(false);
			drivetrain_vent->Set(true);
		}
		
		//Six is high gear
		if (gamepad->GetRawButton(6))
		{
			printf("The right button is being pressed.\n");
			drivetrain_pressure->Set(true);
			drivetrain_vent->Set(false);
		}
		
		

		//assign right joystick value to 'right'
		right=(gamepad->GetTwist());
		//find the change in joystick values
		rightchange=fabs(oldright-right);
		
		// if the change in joystick values is less than 0.2 then use the old right value vs the new one
		//Stops driver from accelerating the robot too quickly
		if(rightchange<=threshold) {
			useright=right;
		}
		//else... if the new right is greater than the old one use the old value plus the threshold (faster)
		//if the new right is less than the old right use the old value minue the threshold (slower)
		//so that really large movements of the joystick don't translate immediately into a really big acceleration
		else {
			if(oldright<right) {
				useright=oldright+threshold;
			}
			if(oldright>right) {
				useright=oldright-threshold;
			}
		}
		
		//Same as right, but on the left side
		left=(gamepad->GetY());
		leftchange=fabs(oldleft-left);

		if(leftchange<=threshold) {
			useleft=left;
		}
		else {
			if(oldleft<left) {
				useleft=oldleft+threshold;
			}
			if(oldleft>left) {
				useleft=oldleft-threshold;
			}
		}
		
		//printf("hello world");
		
		//make useright and useleft (the values sent to the robot) the old values for the next loop
		oldright=useright;
		oldleft=useleft;

	}

	/****************************************
	 * Runs the motors with arcade steering.*
	 ****************************************/
	void OperatorControl(void)
	{
//TODO put in servo for lower camera--look in WPI to set	
//		Watchdog baddog;
		
	//	baddog.Feed();
		myRobot.SetSafetyEnabled(true);
		//SL Earth.Start(); // turns on Earth
//		SmartDashboard *smarty = SmartDashboard::GetInstance();
		//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
		//char debugout [100];
		compressor.Start();
		gyro.Reset(); // resets gyro angle
		int rpmForShooter;

		
		while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
		{ 
//			baddog.Feed();
			//myRobot.SetSafetyEnabled(true);
			//myRobot.SetExpiration(0.1);
			float leftYaxis = driver.GetY();
			float rightYaxis = driver.GetTwist();//RawAxis(5);
			myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
			float random = gamecomponent.GetY();
			float lazysusan = gamecomponent.GetZ();
			//bool elevator = Frodo.Get();
			float angle = gyro.GetAngle();
			bool balance = Smeagol.Get();
			SmartDashboard::PutNumber("Gyro Value",angle);
			int NumFail = -1;
			//bool light = Pippin.Get();
			//SL float speed = Earth.GetRate();
			//float number = shooter.Get();
			//bool highspeed = button1.Get()
			//bool mediumspeed = button2.Get();
			//bool slowspeed = button3.Get();
			bool finder = autotarget.Get();
			//bool targetandspin = autodistanceandspin.Get();
			SmartDashboard::PutString("Targeting Activation","");
			//dslcd->Clear();
			//sprintf(debugout,"Number=%f",angle); 
			//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
			//SL sprintf(debugout,"Number=%f",speed);
			//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
			//sprintf(debugout,"Number=%f",number);
			//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
			//sprintf(debugout,"Finder=%u",finder);
			//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
			//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
		    // sprintf(debugout,"Number=%u",maxi);
			// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
						bool basketballpusher = julesverne.Get();
						bool bridgetipper = joystickbutton.Get();
						if (bridgetipper) // if joystick button 7 is pressed (is true)
						{	
							solenoid.Set(true); // then the first solenoid is on
						}
						
							else
							{
							//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
							solenoid.Set(false); //and then the first solenoid turns off
						}
						if (basketballpusher) // if joystick button 6 is pressed (is true)
						{
							shepard.Set(true); // then shepard is on the run
							//Wait(0.5); // and shepard waits for 0.5 seconds
						}
							else
							{		
							shepard.Set(false); // and then shepard turns off
						} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2			

			//}	
			//cheetah.Set(0.3*lazysusan);
//			smarty->PutDouble("pre-elevator",lynx.Get());
			lynx.Set(random);
//			smarty->PutDouble("elevator",lynx.Get());
			
//			smarty->PutDouble("joystick elevator",random);
			
			
			if (balance)						// this is the start of the balancing code
			{
				angle = gyro.GetAngle();
				myRobot.Drive(-0.03*angle, 0.0);
				Wait(0.005);
			}
			/*if (light)							//button 5 turns light on oand off on game controller
			     flashring.Set(Relay::kForward);
			     else
			            flashring.Set(Relay::kOff);
			 */           
			if (finder)
			{
				flashring.Set(Relay::kForward);
//.........这里部分代码省略.........

	void OperatorControl(void)
	{

		bool button1Bool;
		bool button2Bool;
		bool button3Bool;
		bool button4Bool;
		bool button5Bool;
		bool button6Bool;
		bool button7Bool;
		bool button8Bool;
		bool button9Bool;
		bool button10Bool;
		bool button11Bool;
		bool button12Bool;
		float LeftaxisYValue;
		float LeftaxisXValue;
	
		float RightaxisXValue;
		float RightaxisYValue;
		
		float TriggerValue;

		myRobot.SetSafetyEnabled(true);
		
		while (IsOperatorControl())
		{

			button1Bool = button1.Get();
			SmartDashboard::PutNumber("button1",button1Bool);
			button2Bool = button2.Get();
			SmartDashboard::PutNumber("button2",button2Bool);
			button3Bool = button3.Get();
			SmartDashboard::PutNumber("button3",button3Bool);
			button4Bool = button4.Get();
			SmartDashboard::PutNumber("button4",button4Bool);
			button5Bool = button5.Get();
			SmartDashboard::PutNumber("button5",button5Bool);
			button6Bool = button6.Get();
			SmartDashboard::PutNumber("button6",button6Bool);
			button7Bool = button7.Get();
			SmartDashboard::PutNumber("button7",button7Bool);
			button8Bool = button8.Get();
			SmartDashboard::PutNumber("button8",button8Bool);
			button9Bool = button9.Get();
			SmartDashboard::PutNumber("button9",button9Bool);
			button10Bool = button10.Get();
			SmartDashboard::PutNumber("button10",button10Bool);
			button11Bool = button11.Get();
			SmartDashboard::PutNumber("button11",button11Bool);
			button12Bool = button12.Get();
			SmartDashboard::PutNumber("button12",button12Bool);
			
			
			LeftaxisXValue = stick.GetX();
			SmartDashboard::PutNumber("Joystick1 X Axis",LeftaxisXValue);
			LeftaxisYValue = 0 - stick.GetY();
			SmartDashboard::PutNumber("Joystick1 Y Axis",LeftaxisYValue);
			
			
			RightaxisXValue = stick.GetTwist();
			SmartDashboard::PutNumber("Joystick2 X Axis", RightaxisXValue);
			RightaxisYValue = 0 - stick.GetRawAxis(5);
			SmartDashboard::PutNumber("Joystick2 Y Axis", RightaxisYValue);
			
			TriggerValue = stick.GetThrottle();
			SmartDashboard::PutNumber("Trigger value", TriggerValue);

			Wait(0.005);				// wait for a motor update time
		}
	}

	void OperatorControl(void)
	{
		// feed watchdog ---    TheVessel.SetSafetyEnabled(true);
		GetWatchdog().Kill();
		
		if (!game){
		AlmostZeroPi = manEnc->GetDistance() -1;//Zeros manipulator encoder-- only here for tele op practice w/out auto
		cmp->Start();
		}
		
		cd->MecanumDrive(0, 0, 0);
		minibot->Set(DoubleSolenoid::kForward);
		while(IsOperatorControl() && IsEnabled())
		{
			GetWatchdog().Kill();
			
			if(IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kReverse);
			else if(!IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kForward);
		
			cd->MecanumDrive(controller);//passes joystick controller's input to cd mechdrv
			
			if(stick->GetRawButton(1))
			{
			}
			else if(stick->GetRawButton(2))
			{
			}
			else if(stick->GetRawButton(3))
			{
				reset = true;//              ENABLES NO RESTRICTION MANIPULATOR MOVEMENT
			}
			else if(stick->GetRawButton(4))
			{
				cout << "Manipulator Encoder Value:   " << manEnc->Get() << "/n";//prints manip encoder to console if open;
				Wait(.1f);//pause so not above a billion times
			}
			else if(stick->GetRawButton(6))
			{
				open->Set(true);//self explanatory, eote pnuematics
				closed->Set(false);
			}
			else if(stick->GetRawButton(8))
			{
				closed->Set(true);
				open->Set(false);
			}
			
			

			ShoulderJag->Set(stick->GetY());//shoulder moves with stick->GetY()
			if (stick->GetTwist() < 0 || manEnc->GetDistance() < AlmostZeroPi || reset)//man extending, enc not at 0, or reset true
			{
				manJag->Set(stick->GetTwist()*.3);//shoulder above but for wrist, also, only 3/10ths power
			}

		
		}
		cmp->Stop();//stops compressor
		while (manEnc->GetDistance() < AlmostZeroPi)//brings manipulator back to 0 so not need reset
		{
			manJag->Set(.1);
		}
	}