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

/**
 * Get the throttle 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 GetThrottle(UINT32 port)
{
    Joystick *stick = getJoystick(port);
    if (stick == NULL)
        return 0;
    return stick->GetThrottle();
}

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

// Called repeatedly when this Command is scheduled to run
void DriveBot::Execute() {
	Joystick* stick = Robot::oi->getGamePad();
	
	if(Robot::mover->speedSwitch->Get()){

			Robot::mover->rightMotor0->Set(deadband(stick->GetThrottle()*-0.75,.01));
			Robot::mover->rightMotor1->Set(deadband(stick->GetThrottle()*-0.75,.01));

			Robot::mover->leftMotor2->Set(deadband(stick->GetY()*0.75,.01));
			Robot::mover->leftMotor3->Set(deadband(stick->GetY()*0.75,.01));
	}else{
			Robot::mover->rightMotor0->Set(stick->GetThrottle()*-0.25);
			Robot::mover->rightMotor1->Set(stick->GetThrottle()*-0.25);

			Robot::mover->leftMotor2->Set(stick->GetY()*0.25);
			Robot::mover->leftMotor3->Set(stick->GetY()*0.25);
	}
}

/**
 * @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 Proxy166::SetJoystick(int joy_id, Joystick & stick)
{
	wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0);
	//semTake(JoystickLocks[joy_id], WAIT_FOREVER);
	Joysticks[joy_id].X = stick.GetX();
	Joysticks[joy_id].Y = stick.GetY();
	Joysticks[joy_id].Z = stick.GetZ();
	Joysticks[joy_id].throttle = stick.GetThrottle();
	for(unsigned i=0;i<NUMBER_OF_JOY_BUTTONS;i++) {
		Joysticks[joy_id].button[i] = stick.GetRawButton(i);
	}
	//semGive(JoystickLocks[joy_id]);
}

	void DriveControl() { //Drives la wheels of the robot
		flightX = flightStick->GetRawAxis(0); //Pull joystick side motion for later use
		flightY = flightStick->GetRawAxis(1); //Pull joystick forward motion for later use (forward is -, backwards is +)
		flightZ = flightStick->GetRawAxis(4); //Pull joystick twist motion for later use
		flightThrottle = ((((shootStick->GetThrottle() - 1)*-1)/2) * .8 + .2); //Pull throttle to modify drive variables
																		//Throttle value is between .2 and 1.0

		if (fabs(flightX) < deadZone) { //Deaden x
			flightX = 0;
		}
		if (fabs(flightY) < deadZone) { //Deaden y
			flightY = 0;
		}
		if (fabs(flightZ) < deadZone) { //Deaden z
			flightZ = 0;
		}

		if (flightStick->GetRawButton(strafeButtonChannel)){ //Set drive to strafe mode
			driveMode = 0;
		}
		if (flightStick->GetRawButton(arcadeButtonChannel)){ //Set drive to arcade mode
			driveMode = 1;
		}
		if (flightStick->GetRawButton(fieldButtonChannel)){ //Set drive to field-centric mode
			driveMode = 2;
		}
		if (shootStick->GetRawButton(gyroResetChannel)){ //Reset gyro with the trigger
			yawGyro->Reset();
		}
		flightX = flightX * flightThrottle;
		flightY = flightY * flightThrottle;
		flightZ = flightZ * flightThrottle;

		if(driveMode == 1){
			robotDrive->MecanumDrive_Cartesian(flightZ, flightY, flightX, 0);
			SmartDashboard::PutString("DriveMode", "Arcade");
		}
		else if(driveMode == 2){
			robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, yawGyro->GetAngle());
			SmartDashboard::PutString("DriveMode", "Field");
		}
		else{
			robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, 0);
			SmartDashboard::PutString("DriveMode", "Strafe");
		}

		SmartDashboard::PutNumber("GyroAngle", yawGyro->GetAngle());
	}

		void OperatorControl (void) {
			GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
			
			DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
			
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
			
			SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
			SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
			SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
			
			while (IsOperatorControl() && IsEnabled()) {
				GetWatchdog().Feed(); // Feed the Watchdog.
				
				kinectMode = (bool) kinectModeSelector->GetSelected();
				demoMode = (bool) demoModeSelector->GetSelected();
				speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
				
				if (kinectMode) {
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
					
					if (!demoMode) {
						if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else {
							motorDriveLeft.Set(0);
							motorDriveRight.Set(0);
						}
					}
			
					if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER);
						motorFeed.Set(FEED_MOTOR_POWER);
						motorPickup.Set(PICKUP_MOTOR_POWER);
					} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
						motorFeed.Set(FEED_MOTOR_POWER * -1);
						motorPickup.Set(PICKUP_MOTOR_POWER * -1);
					} else {
						motorShooter.Set(0);
						motorFeed.Set(0);
						motorPickup.Set(0);
					}
				
					if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
						motorTurret.Set(TURRET_POWER);
					} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
						motorTurret.Set(TURRET_POWER * -1);
					} else {
						motorTurret.Set(0);
					}
				} else {
					if (joystickDriveLeft.GetThrottle() == 0) {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
						
						motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER);
							motorFeed.Set(FEED_MOTOR_POWER);
							motorPickup.Set(PICKUP_MOTOR_POWER);
						} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
							motorFeed.Set(FEED_MOTOR_POWER * -1);
							motorPickup.Set(PICKUP_MOTOR_POWER * -1);
						} else {
							motorShooter.Set(0);
							motorFeed.Set(0);
							motorPickup.Set(0);
						}
						
						if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
							motorTurret.Set(TURRET_POWER);
						} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
							motorTurret.Set(TURRET_POWER * -1);
						} else {
							motorTurret.Set(0);
						}
					} else {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
						
						motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
//.........这里部分代码省略.........

	void TeleopPeriodic()
	{
		//camera->GetImage(frame);
		//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
		//CameraServer::GetInstance()->SetImage(frame);


		printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());

		drive->ArcadeDrive(driveStick);
		drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
		//printf("%f\n", (1-stick->GetThrottle())/2);
		//leftMotor->Set(0.1);
		//rightMotor->Set(0.1);

		if (shootStick->GetRawAxis(3) > 0.5) {
			launch1->Set(1.0);
			launch2->Set(1.0);
		} else if (shootStick->GetRawAxis(2) > 0.5) {
			printf("Power Counter: %i\n", powerCounter);
			if (powerCounter < POWER_MAX) {
				powerCounter++;
				launch1->Set(-0.8);
				launch2->Set(-0.8);
			} else {
				launch1->Set(-0.6);
				launch2->Set(-0.6);
			}
		} else {
			launch1->Set(0.0);
			launch2->Set(0.0);
			powerCounter = 0.0;
		}

		//use this button to spin only one winch, to lift up.
		 if (shootStick->GetRawButton(7)) {
		 	otherWinch->Set(0.5);
		 } else if (shootStick->GetRawButton(8)) {
			 otherWinch->Set(-0.5);
		 } else {
		 	otherWinch->Set(0.0);
		 }

		if (shootStick->GetRawButton(5)) {
			winch->Set(-0.7);

			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(-0.5);
			}
		} else if (shootStick->GetRawButton(6)) {
			winch->Set(0.7);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(0.5);
			}
		} else {
			winch->Set(0.0);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//,				otherWinch->Set(0.0);
			}
		}
		

		if (shootStick->GetRawButton(1)) {
			launchPiston->Set(1);
		} else {
			launchPiston->Set(0);
		}

		if (shootStick->GetRawButton(3)) {
			Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
		}

		if (shootStick->GetRawButton(3) && debounce == false) {
			debounce = true;
			if (defenseUp) {
				defensePiston->Set(DoubleSolenoid::Value::kReverse);
				defenseUp = false;
			} else {
				defenseUp =true;
				defensePiston->Set(DoubleSolenoid::Value::kForward);
			}
		} else if (!shootStick->GetRawButton(3)){
			debounce = false;
		}
	}

	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 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;
//.........这里部分代码省略.........