Java 类edu.wpi.first.wpilibj.GenericHID 实例源码

public DirectionalButton(Direction direction, GenericHID joystick)
{
    neededAngle = 1;
    this.direction = direction;
    this.joystick = joystick;

    switch (direction)
    {
    case UP:
        neededAngle = 0;
        break;
    case DOWN:
        neededAngle = 180;
        break;
    case LEFT:
        neededAngle = 270;
        break;
    case RIGHT:
        neededAngle = 90;
        break;
    }
}

protected void execute() {
    //here we already have to access the io object. 
    //read the up/down and left/right from io.
    //then pass those values to the mecanum drive function.

    SmartDashboard.putNumber("[MC] X", oi.getXbox().getX(GenericHID.Hand.kLeft));
    SmartDashboard.putNumber("[MC] Y", oi.getXbox().getY(GenericHID.Hand.kLeft));
    SmartDashboard.putNumber("[MC] Theta", oi.getXbox().getThrottle());
    X = oi.getXbox().getX(GenericHID.Hand.kLeft);
    if ( Math.abs(X) <= 0.25) {
        X = 0.0;
    } else {
    }
    Y = oi.getXbox().getY(GenericHID.Hand.kLeft);
    if (Math.abs(Y) <= 0.25) {
        Y = 0.0;
    }
    throttle = oi.getXbox().getThrottle();
    if (Math.abs(throttle) <= 0.25) {
        throttle = 0.0;          
    }       

    driveTrain.mecanumDrive(X, Y, throttle, gyro.getAngle());//- (RobotTemplate.gyroOff * Timer.getFPGATimestamp())))
    //driveTrain.mecanumDrive(oi.getXbox().getX(GenericHID.Hand.kLeft), oi.getXbox().getY(GenericHID.Hand.kLeft), oi.getXbox().getTwist(), gyro.getAngle());
}

protected void execute()
{
    if (OI.primaryXboxController.getBumper(GenericHID.Hand.kLeft))
    {
        if (!runShooter)
        {
            shooter.runShooter(shooterSpeed);
            runShooter = true;
        }

        else
        {
            shooter.runShooter(0);
            runShooter = false;
        }            
    }
}

public void winchUp(XboxController controller) {
    double y = deadZoneInput(controller.getY(GenericHID.Hand.kLeft), .1);       

if(y > .1)
    speedController1.set(y);
else
    speedController1.set(0);
  }

private boolean shouldUseFieldOrientedDrive() {
  double absXL = Math.abs(joystick.getX(GenericHID.Hand.kLeft));
  absXL = absXL < DEADZONE ? 0 : absXL;
  double absXR = Math.abs(joystick.getX(GenericHID.Hand.kRight));
  absXR = absXR < DEADZONE ? 0 : absXR;
  double abxYL = Math.abs(joystick.getY(GenericHID.Hand.kLeft));
  abxYL = abxYL < DEADZONE ? 0 : abxYL;
  double absYR = Math.abs(joystick.getY(GenericHID.Hand.kRight));
  absYR = absYR < DEADZONE ? 0 : absYR;
  return absXL > absXR || abxYL > absYR;
}

private boolean allInputsInDeadZone() {
  return Math.abs(getJoystickZ()) <  TRIGGER_DEADZONE &&
          Math.abs(joystick.getX(GenericHID.Hand.kLeft)) < DEADZONE &&
          Math.abs(joystick.getY(GenericHID.Hand.kLeft)) < DEADZONE &&
          Math.abs(joystick.getX(GenericHID.Hand.kRight)) < DEADZONE &&
          Math.abs(joystick.getY(GenericHID.Hand.kRight)) < DEADZONE;

}

public void tankDrive(
        GenericHID leftStick, int leftAxis, GenericHID rightStick, int rightAxis,
        boolean inverted, boolean squaredInput)
{
    tankDrive(
            leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis),
            inverted, squaredInput);
}

@Override
public void tankDrive(
        GenericHID leftStick, int leftAxis, GenericHID rightStick, int rightAxis,
        boolean inverted)
{
    tankDrive(
            leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), inverted, false);
}

public void tankDrive(
        GenericHID leftStick, GenericHID rightStick, boolean inverted, boolean squaredInput)
{
    tankDrive(
            leftStick, Joystick.AxisType.kY.value, rightStick, Joystick.AxisType.kY.value,
            inverted, squaredInput);
}

@Override
public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean inverted)
{
    tankDrive(
            leftStick, Joystick.AxisType.kY.value, rightStick, Joystick.AxisType.kY.value,
            inverted, false);
}

@Override
public void tankDrive(GenericHID leftStick, GenericHID rightStick)
{
    tankDrive(
            leftStick, Joystick.AxisType.kY.value, rightStick, Joystick.AxisType.kY.value,
            false, false);
}

public void arcadeDrive(
        GenericHID driveStick, int driveAxis, GenericHID turnStick, int turnAxis,
        boolean inverted, boolean squaredInput)
{
    arcadeDrive(
            driveStick.getRawAxis(driveAxis), turnStick.getRawAxis(turnAxis),
            inverted, squaredInput);
}

@Override
public void arcadeDrive(
        GenericHID driveStick, int driveAxis, GenericHID turnStick, int turnAxis,
        boolean inverted)
{
    arcadeDrive(
            driveStick.getRawAxis(driveAxis), turnStick.getRawAxis(turnAxis),
            inverted, false);
}

@Override
public void arcadeDrive(
        GenericHID driveStick, int driveAxis, GenericHID turnStick, int turnAxis)
{
    arcadeDrive(
            driveStick.getRawAxis(driveAxis), turnStick.getRawAxis(turnAxis),
            false, false);
}

public void arcadeDrive(GenericHID stick, boolean inverted, boolean squaredInput)
{
    arcadeDrive(
            stick.getRawAxis(Joystick.AxisType.kY.value),
            stick.getRawAxis(Joystick.AxisType.kX.value),
            inverted, squaredInput);
}

@Override
public void arcadeDrive(GenericHID stick, boolean inverted)
{
    arcadeDrive(
            stick.getRawAxis(Joystick.AxisType.kY.value),
            stick.getRawAxis(Joystick.AxisType.kX.value),
            inverted, false);
}

@Override
public void arcadeDrive(GenericHID stick)
{
    arcadeDrive(
            stick.getRawAxis(Joystick.AxisType.kY.value),
            stick.getRawAxis(Joystick.AxisType.kX.value),
            false, false);
}

/**
 * Receive a joystick and then map controls to it.
 *
 * @param joysticks The joysticks used for buttons
 */
public OI(GenericHID... joysticks) {
    // button map
    liftUpButton = new JoystickButton(joysticks[0], 5);
    liftDownButton = new JoystickButton(joysticks[0], 3);

    forkInButton = new JoystickButton(joysticks[0], 1);
    forkOutButton = new JoystickButton(joysticks[0], 2);

    extGrabButton = new JoystickButton(joysticks[0], 6);
    extThrowButton = new JoystickButton(joysticks[0], 4);

    reverseButton = new JoystickButton(joysticks[0], 12);

    // button controls
    liftUpButton.whileHeld(new Lift(1));
    liftDownButton.whileHeld(new Lift(-1));

    liftUpButton.whenReleased(new Lift(0));
    liftDownButton.whenReleased(new Lift(0));

    forkInButton.whileHeld(new Fork(1));
    forkOutButton.whileHeld(new Fork(-0.666));

    forkInButton.whenReleased(new Fork(0));
    forkOutButton.whenReleased(new Fork(0));

    extGrabButton.whileHeld(new ExtArm(1));
    extThrowButton.whileHeld(new ExtArm(-1));

    extGrabButton.whenReleased(new ExtArm(0));
    extThrowButton.whenReleased(new ExtArm(0));

    reverseButton.whenPressed(new ReverseDrive());
}

@Override
public void tick() {
    Joystick joy = UDPreference.joystick;
    if (UDPreference.JOYSTICK_LAYOUT.toLowerCase().equals("xbox_stick")) {
        UDPreference.drive.mecanumDrive_Cartesian(joy.getX(GenericHID.Hand.kLeft), joy.getY(GenericHID.Hand.kLeft), joy.getX(GenericHID.Hand.kRight), 0);
    }
}

@Override
public void tick() {
    Joystick joy = UDPreference.joystick;
    if (UDPreference.JOYSTICK_LAYOUT.toLowerCase().equals("xbox_stick")) {
        UDPreference.drive.tankDrive(joy.getY(GenericHID.Hand.kLeft), joy.getY(GenericHID.Hand.kRight), UDPreference.DRIVE_SQUAREDINPUTS);
    }
}

public void execute(XboxController controller) {
    double xleft = controller.getX(GenericHID.Hand.kLeft);
    double yleft = controller.getY(GenericHID.Hand.kLeft);
    double xright = controller.getX(GenericHID.Hand.kRight);

    drive.mecanumDrive_Cartesian(joystickSensitivity(xleft), joystickSensitivity(yleft), joystickSensitivity(xright),0);
}

protected boolean isFinished() 
{
    if((Timer.getFPGATimestamp()-startTime) > shooterRunTime)
        return true;
    //for debug
    if(OI.primaryXboxController.getBumper(GenericHID.Hand.kLeft) || 
            OI.primaryXboxController.getRawButton(1))
        return true;

    return false;
}

public double getThrottle() {
  return -xboxController.getY(GenericHID.Hand.kLeft);
}

public double getTurn() {
  return -xboxController.getX(GenericHID.Hand.kRight);
}

public boolean isQuickTurn() {
  return xboxController.getStickButton(GenericHID.Hand.kRight);
}

public void mecanumDrive(XboxController controller) {       
  double x = deadZoneInput(controller.getX(GenericHID.Hand.kLeft), .3); 
  double y = deadZoneInput(controller.getY(GenericHID.Hand.kLeft), .1) * .5; 
  double rotation = deadZoneInput(controller.getX(GenericHID.Hand.kRight), .1) * .7;

  if(controller.getTriggerAxis(GenericHID.Hand.kRight) != 0.0) {
        double trigger = controller.getTriggerAxis(GenericHID.Hand.kRight);
        y = y * (trigger + 1);
        rotation = rotation * (1-(trigger*.5));
  }

    StringBuilder sb = new StringBuilder();

    if(controller.getBumper(GenericHID.Hand.kLeft)) {
        rFM.changeControlMode(TalonControlMode.PercentVbus);
        rRM.changeControlMode(TalonControlMode.PercentVbus);
        lFM.changeControlMode(TalonControlMode.PercentVbus);
        lRM.changeControlMode(TalonControlMode.PercentVbus);

        robotDrive41.mecanumDrive_Cartesian(x, y, rotation, 0);     
    }
    else {
        y = y * -1;
        //sb.append("rawx:" + rotation + ", rawy:" + y);
        //double motorOutput = lFM.getOutputVoltage() / lFM.getBusVoltage();

        double z = Math.sqrt(rotation * rotation + y * y);
        double rad = Math.acos(Math.abs(rotation)/z);
        double angle = rad * 180.0 / Math.PI;

        double tcoeff = -1 + (angle/90.0)*2.0;
        double turn = tcoeff * Math.abs(Math.abs(y) - Math.abs(rotation));
        turn = Math.round(turn*100.0)/100.0;

        double move = Math.max(Math.abs(y), Math.abs(rotation));

        double left = 0;
        double right = 0;

        if ((rotation >= 0 && y >=0) || (rotation < 0 && y < 0))
        {
            left = move;
            right = turn;               
        }
        else {
            right = move;
            left = turn;                                
        }

        if (y < 0)
        {
            left = 0 - left;
            right = 0 - right;              
        }

        double rightTargetSpeed = right * 1200;
        rFM.changeControlMode(TalonControlMode.Speed);
        rFM.set(rightTargetSpeed);

        //rRM.changeControlMode(TalonControlMode.Follower);
        //rRM.set(0);

        rRM.changeControlMode(TalonControlMode.Speed);
        rRM.set(rightTargetSpeed);

        double leftTargetSpeed = left * 1200;
        lFM.changeControlMode(TalonControlMode.Speed);
        lFM.set(leftTargetSpeed);

        //lRM.changeControlMode(TalonControlMode.Follower);
        //lRM.set(1);

        lRM.changeControlMode(TalonControlMode.Speed);
        lRM.set(leftTargetSpeed);           
    }
}

public TriggerButton(int axis, GenericHID joystick)
{
    triggerAxis = axis;
    this.joystick = joystick;
}

private void handleInput(GenericHID g) {

    // slow down turning sensitivity by multiplying stick input bhy 0.75
    RobotMap.driveTrain.arcadeDrive(
            g.getRawAxis(GamepadState.AXIS_LY) * (InterfaceFlip.isFlipped ? 1 : -1), 
            g.getRawAxis(GamepadState.AXIS_RX) * 0.75);

    //D-Pad Controls

      switch (oi.gamepad.getPOV()) {
        //D-pad right
        case 90:
            if (!povPressed) {
                Scheduler.getInstance().add(null); //new MoveTailDown());
            }
            povPressed = true;
                break;
            //D-pad left
        case 270:
            if (!povPressed) {
                Scheduler.getInstance().add(null);//new MoveTailUp());
            }
            povPressed = true;
                break;
            //D-pad up
case 0:
    // Use speed mode if not currently used
    if (!povPressed) {
        Scheduler.getInstance().add(new MoveAimerUp());
    }
    povPressed = true;
    break;
//D-pad down
case 180:
    if (!povPressed) {
        Scheduler.getInstance().add(new MoveAimerDown());
    }
    povPressed = true;
    break;
case -1:
    if (povPressed == true) {
    Scheduler.getInstance().add(new StopAimer());
    }
    povPressed = false;
    break;
}

     //left Trigger Code
      if (oi.gamepad.getRawAxis(GamepadState.AXIS_LT) >= .2) {
        Scheduler.getInstance().add(new Intake());

    leftTriggerPressed = true;
} else {
    leftTriggerPressed = false;
}
      //right Trigger Code
      if (oi.gamepad.getRawAxis(GamepadState.AXIS_RT) >= .2) {  
        Scheduler.getInstance().add(new SpinShooterWheels());
        isSpinning = true;
        rightTriggerPressed = true;
} else if (rightTriggerPressed){
    rightTriggerPressed = false;
    isSpinning = false;
    shooterNotActive = true;
    Scheduler.getInstance().add(new StopBallMotors());

}
    //  if (rightTriggerPressed && shooterNotActive) {
      //    gameMode = 3;
      //    Scheduler.getInstance().add(new SpinShooterWheels());
      //    shooterNotActive = false;
     // }


  }

@Override
  protected void execute() {
//    TRIGGER_DEADZONE = SmartDashboard.getNumber("trig", TRIGGER_DEADZONE);

    double z; // this will contain the rotation rate (from either the joystick or the PID as appropriate)
    double joystickZ = getJoystickZ(); // temp variable to hold the joystick rotation rate

    if (shouldBeOpenLoopSteering(joystickZ)) {
      // robot should be in open loop steering mode where the joystick triggers control the rotation rate

      // if we are in PID steering mode, switch to open loop steering mode
      if (driveMode == DriveMode.PID) {
        switchToOpenLoopSteering();
      }

      // use the joystick to control the rotation rate
      z = joystickZ / 2;

      getPIDController().setSetpoint(Robot.spatialAwarenessSubsystem.getHeading());
    } else {
      // robot should be in pid steering mode where the PIDController controls the rotation rate

      // if we are in open loop steering, switch to PID steering
      if (driveMode == DriveMode.OpenLoop) {
        switchToPidSteering();
      }
      // otherwise use the PID to control the rotation rate
      z = pidZ;
    }

    if (shouldUseFieldOrientedDrive()) {
      // use the left analog stick for field oriented
      Robot.driveSubsystem.mecanumDrive(
              joystick.getX(GenericHID.Hand.kLeft),
              joystick.getY(GenericHID.Hand.kLeft),
              z,
              Robot.spatialAwarenessSubsystem.getHeading());
    } else {
      // otherwise use the right analog stick for robot oriented
      Robot.driveSubsystem.mecanumDrive(
              joystick.getX(GenericHID.Hand.kRight),
              joystick.getY(GenericHID.Hand.kRight),
              z,
              0);
    }
  }

private double getJoystickZ() {
  double leftTriggerValue = joystick.getTriggerAxis(GenericHID.Hand.kLeft);
  double rightTriggerValue = joystick.getTriggerAxis(GenericHID.Hand.kRight);
  return rightTriggerValue - leftTriggerValue;
}

@Override
public void tankDrive(GenericHID leftStick, int leftAxis, GenericHID rightStick, int rightAxis)
{
    tankDrive(
            leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), false, false);
}

public HumanNumberInputEvent(GenericHID source, double number) {
    super(source);
    this.number = number;
}

public HumanToggleInputEvent(GenericHID source, boolean toggle) {
    super(source);
    this.toggle = toggle;
}

public HumanInputEvent(GenericHID source) {
    this.source = source;
}

public final GenericHID getSource() {
    return source;
}

public Controller(GenericHID controller) {
    this.controller = controller;
}

public GenericHID get() {
    return controller;
}

public void drive(GenericHID moveStick, final int moveAxis,
        GenericHID rotateStick, final int rotateAxis) {
    robotDrive.arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis);
}

public JoystickAxisButton(GenericHID joystick, int axis, double value) {
    this.joystick = joystick;
    this.axis = axis;
    this.value = value;
}

public Bumper(GenericHID joystick, int TriggerAxis) {
    super(joystick, TriggerAxis);
   this.TriggerAxis = TriggerAxis;
    this.joystick = joystick;
}