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

public OI() {

        driveStick = new Joystick(RobotMap.DRIVE_STICK_NUMBER);
        driveButtons = new JoystickButton[13];

        auxiliaryStick = new Joystick(RobotMap.AUXILLIARY_STICK_NUMBER);
        auxiliaryButtons = new JoystickButton[13];

        for(int i = 1; i <= driveButtons.length - 1; i++) {
            driveButtons[i] = new JoystickButton(driveStick, i);
        }

        for(int i=1; i <= auxiliaryButtons.length - 1; i++){
            auxiliaryButtons[i] = new JoystickButton(auxiliaryStick, i);
        }

        //this.getButton(RobotMap.SHOOTER_CONTROL_BUTTON).whileHeld(new ShooterControl());
        this.getButton(2).whenPressed(new openIntake());
        this.getButton(3).whenPressed(new closeIntake());
        this.getButton(4).toggleWhenPressed(new IntakeIn());
        this.getButton(5).toggleWhenPressed(new IntakeOut());
        this.getButton(5).toggleWhenPressed(new stopIntake());
        this.getButton(4).whenPressed(new driveForward(20, .25));

    }

public OI() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

    logitech = new Joystick(0);

    shooterbutton = new JoystickButton(logitech, 1);
    shooterbutton.whileHeld(new shoot());


    // SmartDashboard Buttons
    SmartDashboard.putData("Autonomous Command", new AutonomousCommand());
    SmartDashboard.putData("shoot", new shoot());

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    shootBackwardsButton = new JoystickButton(logitech, 2);
    shootBackwardsButton.whileHeld(new ShootReverse());

    LiftUPButton = new JoystickButton(logitech, 3);
    LiftReservseButton = new JoystickButton(logitech, 4);

    LiftUPButton.whileHeld(new LiftUP());
    LiftReservseButton.whileHeld(new LiftReverse());
}

public OI() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

    xBoxController = new Joystick(0);

    aButton = new JoystickButton(xBoxController, 1);
    bButton = new JoystickButton(xBoxController, 2);
    xButton = new JoystickButton(xBoxController, 3);
    aButton.whenPressed(new RelayOn());
    //b button operated by default command only?
    bButton.whenPressed(new AllForward());
    xButton.whenPressed(new MotorPositionCheck());


    // SmartDashboard Buttons
    SmartDashboard.putData("Autonomous Command", new AutonomousCommand());
    SmartDashboard.putData("RelayOn", new RelayOn());
    SmartDashboard.putData("AllForward", new AllForward());
    SmartDashboard.putData("MotorPositionCheck", new MotorPositionCheck());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}

public void drive(Joystick mStick) {
    //drive.arcadeDrive(mStick.getThrottle()*(-0.7), mStick.getX()*(-0.7));
    //
    if(mStick.getRawButton(7) && mStick.getRawButton(8)){
        drive.tankDrive(mStick.getY()*(-1.0), mStick.getThrottle()*(-1.0));
    }else if(mStick.getRawButton(8)){
        drive.tankDrive(mStick.getY()*(-0.9), mStick.getThrottle()*(-0.9));
    }else if(mStick.getRawButton(7)){
        drive.tankDrive(mStick.getY()*(-0.8), mStick.getThrottle()*(-0.8));
    }else if(mStick.getRawButton(5) && mStick.getRawButton(6)){
        drive.tankDrive(mStick.getY()*(-0.4), mStick.getThrottle()*(-0.4));
    }
    else if(mStick.getRawButton(5)){
        drive.tankDrive(mStick.getY()*(-0.6), mStick.getThrottle()*(-0.6));
    }
    else if(mStick.getRawButton(6)){
        drive.tankDrive(mStick.getY()*(-0.5), mStick.getThrottle()*(-0.5));
    }
    else {
        drive.tankDrive(mStick.getY()*(-0.7), mStick.getThrottle()*(-0.7));
    }
}

public Controller(int port) {

        // Controller
        joystick = new Joystick(port);

        // Buttons
        buttonA = new JoystickButton(joystick, Mappings.BUTTON_A);
        buttonB = new JoystickButton(joystick, Mappings.BUTTON_B);
        buttonX = new JoystickButton(joystick, Mappings.BUTTON_X);
        buttonY = new JoystickButton(joystick, Mappings.BUTTON_Y);
        buttonLeftBumper = new JoystickButton(joystick, Mappings.BUTTON_LEFTBUMPER);
        buttonRightBumper = new JoystickButton(joystick, Mappings.BUTTON_RIGHTBUMPER);

        // Axes
        axisLeftX = new JoystickAxis(joystick, Mappings.AXIS_LEFT_X, AXIS_THRESHOLD);
        axisLeftY = new JoystickAxis(joystick, Mappings.AXIS_LEFT_Y, AXIS_THRESHOLD);
        axisRightX = new JoystickAxis(joystick, Mappings.AXIS_RIGHT_X, AXIS_THRESHOLD);
        axisRightY = new JoystickAxis(joystick, Mappings.AXIS_RIGHT_Y, AXIS_THRESHOLD);
        axisLeftTrigger = new JoystickAxis(joystick, Mappings.AXIS_LEFT_TRIGGER, AXIS_THRESHOLD);
        axisRightTrigger = new JoystickAxis(joystick, Mappings.AXIS_RIGHT_TRIGGER, AXIS_THRESHOLD);
    }

public OI() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS

    joystick = new Joystick(0);



    // SmartDashboard Buttons
    SmartDashboard.putData("GearGroup", new GearGroup());
    SmartDashboard.putData("OpenLeft", new OpenLeft());
    SmartDashboard.putData("OpenRight", new OpenRight());
    SmartDashboard.putData("Open", new Open());
    SmartDashboard.putData("PinchLeft", new PinchLeft());
    SmartDashboard.putData("PinchRight", new PinchRight());
    SmartDashboard.putData("Pinch", new Pinch());
    SmartDashboard.putData("WaitForGear", new WaitForGear());
    SmartDashboard.putData("WaitForUnload", new WaitForUnload());
    SmartDashboard.putData("OpenGate", new OpenGate());
    SmartDashboard.putData("CloseGate", new CloseGate());

// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
}

public Robot() {
    stick = new Joystick(0);
    driveLeftFront = new Victor(LEFT_FRONT_DRIVE);
    driveLeftRear = new Victor(LEFT_REAR_DRIVE);
    driveRightFront = new Victor(RIGHT_FRONT_DRIVE);
    driveRightRear = new Victor(RIGHT_REAR_DRIVE);
    enhancedDriveLeft = new Victor(ENHANCED_LEFT_DRIVE);
    enhancedDriveRight = new Victor(ENHANCED_RIGHT_DRIVE);
    gearBox = new DoubleSolenoid(GEAR_BOX_PART1, GEAR_BOX_PART2);
    climber1 = new Victor(CLIMBER_PART1);
    climber2 = new Victor(CLIMBER_PART2);
    vexSensorBackLeft = new Ultrasonic(0, 1);
    vexSensorBackRight = new Ultrasonic(2, 3);
    vexSensorFrontLeft = new Ultrasonic(4, 5);
    vexSensorFrontRight = new Ultrasonic(6, 7);

    Skylar = new RobotDrive(driveLeftFront, driveLeftRear, driveRightFront, driveRightRear);
    OptimusPrime = new RobotDrive(enhancedDriveLeft, enhancedDriveRight);
}

public OI(){
    joystick = new Joystick(0);
    jyButton1 = new JoystickButton(joystick, 1);

    xbox = new Joystick(1);
    xbButton1 = new JoystickButton(xbox, 1);
    xbButton2 = new JoystickButton(xbox, 2);
    xbButton3 = new JoystickButton(xbox, 3);
    xbButton4 = new JoystickButton(xbox, 4);
    xbButton5 = new JoystickButton(xbox, 5);
    xbButton6 = new JoystickButton(xbox, 6);


    jyButton1.whileHeld(new FineControl());

    xbButton1.whileHeld(new Shoot());
    xbButton2.toggleWhenPressed(new IntakeToggle());
    xbButton3.toggleWhenPressed(new ToggleShooter());
    xbButton4.whenPressed(new ClawSet());
    xbButton5.whenPressed(new GripControl(0));
    xbButton6.whenPressed(new GripControl(1));

}

public ToggleButton(Joystick stick, int id, Togglable togglable) {
    super(stick, id);
    this.obj = togglable;

    this.setPressAction(new ButtonAction() {
        @Override
        public void onAction() {
            obj.enable();
        }
    });

    this.setReleaseAction(new ButtonAction() {
        @Override
        public void onAction() {
            obj.disable();
        }
    });
}

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
@Override
public void robotInit() {

       driveLeftA = new CANTalon(2);
       driveLeftB = new CANTalon(1);
       driveRightA = new CANTalon(3);
       driveRightB = new CANTalon(4);

       climberMotorA = new Talon(1);
       climberMotorB = new Talon(2);

       drive = new RobotDrive(driveLeftA, driveLeftB, driveRightA, driveRightB);

       joystick = new Joystick(0);

       SmartDashboard.putNumber("SlowClimber", .5);
       SmartDashboard.putNumber("FastClimber", 1);
}

public XboxController(final int port) {
    super(port); // Extends Joystick...

    /* Initialize */
    this.port = port;
    this.controller = new Joystick(this.port); // Joystick referenced by
                                                // everything
    this.dPad = new DirectionalPad(this.controller);
    this.lt = new Trigger(this.controller, HAND.LEFT);
    this.rt = new Trigger(this.controller, HAND.RIGHT);
    this.a = new JoystickButton(this.controller, A_BUTTON_ID);
    this.b = new JoystickButton(this.controller, B_BUTTON_ID);
    this.x = new JoystickButton(this.controller, X_BUTTON_ID);
    this.y = new JoystickButton(this.controller, Y_BUTTON_ID);
    this.lb = new JoystickButton(this.controller, LB_BUTTON_ID);
    this.rb = new JoystickButton(this.controller, RB_BUTTON_ID);
    this.back = new JoystickButton(this.controller, BACK_BUTTON_ID);
    this.start = new JoystickButton(this.controller, START_BUTTON_ID);
    this.rightClick = new JoystickButton(this.controller, RIGHT_CLICK_ID);
    this.leftClick = new JoystickButton(this.controller, LEFT_CLICK_ID);
}

@Override
protected void execute() {
    Joystick joystickDrive = Robot.oi.getJoystickDrive();
    this.joystickX = joystickDrive.getAxis(Joystick.AxisType.kX) * Robot.driveTrain.turnMultiplier;
    this.joystickY = joystickDrive.getAxis(Joystick.AxisType.kY) *-1;

    VisionState vs = VisionState.getInstance();

    if (vs == null || !vs.wantsControl()) {
        // endAutoTurn is harmless when not needed but required
        //  if the driver changes her mind after initiating auto-targeting..
        Robot.driveTrain.endAutoTurn();

        this.scaledThrottle = Robot.driveTrain.scaleThrottle(joystickDrive.getAxis(Joystick.AxisType.kThrottle));
        if ((Math.abs(this.joystickX) < 0.075) &&
                (Math.abs(this.joystickY) < 0.075)) {
            Robot.driveTrain.stop();
        }
        else {
            Robot.driveTrain.arcadeDrive(joystickY * scaledThrottle, joystickX * scaledThrottle);
        }
    } else {
        Robot.driveTrain.trackVision();
    }
}

private void moveLauncherWithJoystick() {
    double joystickY = Robot.oi.aimStick.getAxis((Joystick.AxisType.kY));
    if (Math.abs(joystickY) > MIN_JOYSTICK_MOTION) {
        if (isJoystickIdle) {
            aimMotor.enableControl();
            isJoystickIdle = false;
            System.out.println("Enabling Aim Control");
        }
        readSetPoint();
        offsetSetPoint(joystickY * JOYSTICK_SCALE);
    } else {
        if (!isJoystickIdle) {
            aimMotor.disableControl();
            isJoystickIdle = true;
            System.out.println("Disabling Aim Control");
        }
    }
    // aimMotor.disableControl();
}

public ButtonTracker(Joystick Joystick, int Channel) {
    mChannel = Channel;
    mJoystick = Joystick;

    if (!usedNumbers.containsKey(Joystick)) {
        usedNumbers.put(Joystick, new ButtonTracker[17]);
    }

    if (usedNumbers.get(Joystick)[Channel] != null) {
        // SmartDashboard.putBoolean("ERROR", true);
        System.out.println("MORE THAN ONE BUTTON TRACKER PER BUTTON.");
        DriverStation.reportError("MORE THAN ONE BUTTON TRACKER PER BUTTON!", false);
    }

    usedNumbers.get(Joystick)[Channel] = this;
}

public static void initialize()
{
    if (!initialized) {
        mFrontLeft = new CANTalon(LEFT_FRONT_TALON_ID);
        mBackLeft = new CANTalon(LEFT_REAR_TALON_ID);
        mFrontRight = new CANTalon(RIGHT_FRONT_TALON_ID);
        mBackRight = new CANTalon(RIGHT_REAR_TALON_ID);

        drive = new RobotDrive(mFrontLeft, mBackLeft, mFrontRight, mBackRight);

        drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, false);
        drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false);
        drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
        drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, false);   

        leftStick = new Joystick(LEFT_JOYSTICK_ID);
        rightStick = new Joystick(RIGHT_JOYSTICK_ID);

        initialized = true;
    }
}

public ButtonTracker(Joystick Joystick, int Channel)
{
    mChannel = Channel;
    mJoystick = Joystick;

    if (!usedNumbers.containsKey(Joystick))
        {
            usedNumbers.put(Joystick, new ButtonTracker[17]);
        }

    if (usedNumbers.get(Joystick)[Channel] != null)
        {
            // SmartDashboard.putBoolean("ERROR", true);
            // System.out.println("MORE THAN ONE BUTTON TRACKER PER BUTTON.");
            DriverStation.reportError("MORE THAN ONE BUTTON TRACKER PER BUTTON!", false);
        }

    usedNumbers.get(Joystick)[Channel] = this;
}

public OI() {

        driveStick = new Joystick(RobotMap.DRIVE_STICK_NUMBER);
        driveButtons = new JoystickButton[13];

        auxiliaryStick = new Joystick(RobotMap.AUXILIARY_STICK_NUMBER);
        auxiliaryButtons = new JoystickButton[13];

        for(int i = 1; i <= driveButtons.length - 1; i++) {
            driveButtons[i] = new JoystickButton(driveStick, i);
        }

        for(int i=1; i <= auxiliaryButtons.length - 1; i++){
            auxiliaryButtons[i] = new JoystickButton(auxiliaryStick, i);
        }

        //this.getButton(RobotMap.SHOOTER_CONTROL_BUTTON).whileHeld(new ShooterControl());
        this.getButton(RobotMap.WINCH_UP_BUTTON).whenPressed(new WinchUp());
        this.getButton(RobotMap.WINCH_DOWN_BUTTON).whileHeld(new WinchDown());
        this.getButton(RobotMap.INTAKE_FORWARD_BUTTON, true).toggleWhenPressed(new ForwardIntake());
        this.getButton(RobotMap.INTAKE_BACKWARD_BUTTON, true).whileHeld(new ReverseIntake());       
        this.getButton(RobotMap.REVERSE_DRIVE_BUTTON).whenPressed(new FlipChassisDirection());
        this.getButton(RobotMap.SHIFT_DOWN_BUTTON, true).whileHeld(new ShiftDown());
        this.getButton(RobotMap.SHIFT_UP_BUTTON, true).whileHeld(new ShiftUp());

        this.getButton(RobotMap.TURN_ENCODERS_OFF, true).toggleWhenPressed(new RemoveEncoderInfluence());

    }

/**
 * Drives the robot with the aid of a joystick deadband, directional deadbands,
 * and software gear ratios.
 * 
 * @param joystick
 *            A 3-axis joystick to control 2 axis movement plus turning.
 */
public void drive (Joystick joystick)
{
    if (joystick.getTrigger() == true)
        this.drive(joystick.getMagnitude(),
                joystick.getDirectionRadians(),
                joystick.getZ());
    else
        this.drive(joystick.getMagnitude(),
                joystick.getDirectionRadians(),
                0);
}

protected void execute() {
    Joystick control = Robot.oi.xBoxController;
    double speed = control.getRawAxis(1)* (InterfaceFlip.isFlipped ? 1 : -1);
    double turn = control.getRawAxis(4) * 0.8;
    DriveTrain.robotDrive.arcadeDrive(speed, turn);

}

public void hybridDrive(Joystick driveStick, boolean isInverted) {
    double lspeed = 0;
    double rspeed = 0;

    double RT = driveStick.getRawAxis(3);
    double LT = driveStick.getRawAxis(2);
    double RJ = driveStick.getRawAxis(5);
    double LJ = driveStick.getRawAxis(1);

    if (isInverted) {
        // straight drive control, RT forward LT backward
        lspeed = RT - LT;
        rspeed = lspeed;

        // tank drive control
        lspeed -= LJ;
        rspeed -= RJ;

    } else {
        lspeed = driveStick.getRawAxis(2) - driveStick.getRawAxis(3);
        rspeed = driveStick.getRawAxis(2) - driveStick.getRawAxis(3);

        // tank drive control
        lspeed -= RJ;
        rspeed -= LJ;

    }
}

/** method called by ManualCommandDrive
  * 
  * @param joy
  */
 public void driftDrive(Joystick joy) {
    double joyVal = Xbox.LEFT_X(joy);
    if (!IN_DRIFT) { // initiate drift if it hasn't been initiated already
        IN_DRIFT = true;
        if (joyVal > 0) { DRIFT_IS_CW = true; }
    }
    if (DRIFT_IS_CW) { joyVal = -1 * joyVal; } // align joyVal to the write negative and positive values


    ROT_SPEED = (Xbox.LT(joy) + Xbox.LT(joy)) * 10d;
    Drift status = updateDriftStatus(joyVal);
switch(status) {
 case DONUT:
                ROT_RADIUS = (Xbox.LEFT_X(joy)) + Constants.DRIFT_DEADZONE; // put stick on a 0-0.9 scale
                    ROT_RADIUS  = 1 - ROT_RADIUS; // flip it to a .1-1 scale, outermost being .1 and innermost being 1
                    ROT_RADIUS *= Constants.MAX_DRIFT_RADIUS; // apply the .1-1 scale to the max radius
                OFFSET = (int) (Constants.MAX_DRIFT_OFFSET * Math.abs(Xbox.LEFT_X(joy))); /// sets offset for max on tight radius
                driveMecanumRot(ROT_RADIUS, ROT_SPEED, OFFSET);
                break;
 case DEAD:
                // this will just continue the last known radius, offset, and speed
                break;
 case POWERSLIDE:
                driveAngle(ROT_SPEED, 90 - OFFSET);
                break;
 case TURNOVER:         
                DRIFT_IS_CW = !DRIFT_IS_CW;
                break;
}
 }

public void safeArcadeDrive(Joystick joystick, JoystickButton safetyButton, boolean squaredInputs){
    if(safetyButton.get()){
        robotDrive.arcadeDrive(joystick, squaredInputs);
    }
    else{
        robotDrive.stopMotor();
    }
}

public void safeTankDrive(Joystick leftJoystick, Joystick rightJoystick, JoystickButton[] safetyButtons, boolean squaredInputs){
    if(getAll(safetyButtons)){
        robotDrive.tankDrive(leftJoystick, rightJoystick, squaredInputs);
    }
    else{
        robotDrive.stopMotor();
    }
}

public CANTalonTester() {
    drive_stick = new Joystick(DRIVE_STICK);
    turn_stick = new Joystick(TURN_STICK);
    left_master = new CANTalon(DERICA_LEFT_A);
    left_slave = new CANTalon(DERICA_LEFT_B);
    right_master = new CANTalon(DERICA_RIGHT_A);
    right_slave = new CANTalon(DERICA_RIGHT_B);
}

/**
 * Creates a new GamepadState based off of a gamepad's current state.
 * @param j The gamepad object
 * @return A gamepad state
 */
public static GamepadState makeState(Joystick j) {
    double[] axes = new double[j.getAxisCount()];
    for(int i = 0; i < axes.length; i ++)
        axes[i] = j.getRawAxis(i);

    boolean[] buttons = new boolean[j.getButtonCount()];
    for(int i = 0; i < buttons.length; i ++)
        buttons[i] = j.getRawButton(i + 1);

    return (new GamepadState(axes, buttons, j.getPOV(), System.currentTimeMillis()));
}

public FieldCentricDriveInput(Joystick stick,GyroCorrection gyro,boolean rotToVector)
{
    super(stick);
    this.gyro=gyro;
    this.rotToVector=rotToVector;
    fieldInput=new SmoothVectorInput(SMOOTH_LEN,new Input<Vector>(){

        @Override
        public Vector get() {
            // TODO Auto-generated method stub
            return getRaw();
        }});
}

public void robotInit() {
    this.mod1Spin = new TalonSRX(Constants.MOD1SPIN);
    this.mod1Drive = new TalonSRX(Constants.MOD1DRIVE);
    this.spinEncoder1 = new Encoder(0, 0); //Needs real values
    this.driveEncoder1 = new Encoder(0, 0); //Needs real values

    this.mod2Spin = new TalonSRX(Constants.MOD2SPIN);
    this.mod2Drive = new TalonSRX(Constants.MOD2DRIVE);
    this.spinEncoder2 = new Encoder(0, 0); //Needs real values
    this.driveEncoder2 = new Encoder(0, 0); //Needs real values

    this.mod3Spin = new TalonSRX(Constants.MOD3SPIN);
    this.mod3Drive = new TalonSRX(Constants.MOD3DRIVE);
    this.spinEncoder3 = new Encoder(0, 0); //Needs real values
    this.driveEncoder3 = new Encoder(0, 0); //Needs real values

    this.mod4Spin = new TalonSRX(Constants.MOD4SPIN);
    this.mod4Drive = new TalonSRX(Constants.MOD4DRIVE);
    this.spinEncoder4 = new Encoder(0, 0); //Needs real values
    this.driveEncoder4 = new Encoder(0, 0); //Needs real values

    this.xboxDrive = new Joystick(Constants.XBOXDRIVEPORT);

    this.frontLeft = new SwerveModule("frontLeft", mod1Drive, mod1Spin, spinEncoder1, driveEncoder1); // needs completion0
    this.backLeft = new SwerveModule("backLeft", mod2Drive, mod2Spin, spinEncoder2, driveEncoder2);
    this.frontRight = new SwerveModule("frontRight", mod3Drive, mod3Spin, spinEncoder3, driveEncoder3);
    this.backRight = new SwerveModule("backRight", mod4Drive, mod4Spin, spinEncoder4, driveEncoder4);

    this.swerveDrive = new SwerveDrive(this.frontLeft, this.backLeft, this.frontRight, this.backRight, 25, 25);

    crab = new CrabDrive(swerveDrive, xboxDrive, "crabDrive", 1);
    spin = new SpinDrive(swerveDrive, xboxDrive, "spinDrive", 2);
    unicorn = new UnicornDrive(swerveDrive, xboxDrive, "unicornDrive", 3);
    drive = new DriveBase(crab, spin, unicorn, "driveBase", 0);
    drive.init();
}

public OI() {

        inputRecordings = new ArrayList<double[]>();
        joystick1 = new Joystick(0);
        joystick2 = new Joystick(1);
        operator = new Joystick(2);

        new JoystickButton(joystick1, 1).whenPressed(new ShootIntoGoal());
        new JoystickButton(joystick1, 3).whenPressed(new SetShooterToTestSpeed());
        new JoystickButton(joystick1, 5).whileHeld(new TurnToOpponentAlliance());
        new JoystickButton(joystick1, 6).whenPressed(new DecreaseShooterTestSpeed());
        new JoystickButton(joystick1, 7).whileHeld(new TurnToOurAlliance());
        new JoystickButton(joystick1, 8).whenPressed(new IncreaseShooterTestSpeed());

        new JoystickButton(operator, 1).whenPressed(new ShooterToBottom());
        new JoystickButton(operator, 2).whenPressed(new ToggleShooterPiston());
        new JoystickButton(operator, 3).whileHeld(new ManualShooterAngle());
//      new JoystickButton(operator, 4).whenPressed(new ArmsUp());
        new JoystickButton(operator, 5).whileHeld(new ShooterManualJogUp());
        new JoystickButton(operator, 6).whileHeld(new ShooterOuttake());
        new JoystickButton(operator, 7).whenPressed(new ShootFullSpeed());
//      new JoystickButton(operator, 8).whenPressed(new ArmsDown());
        new JoystickButton(operator, 9).whileHeld(new ShooterManualJogDown());
        new JoystickButton(operator, 10).whileHeld(new ShooterIntake());
        new JoystickButton(operator, 11).whenPressed(new SafeArmsToggle());
        new JoystickButton(operator, 12).whenPressed(new ShootIntoGoal());

    }

public void robotInit() {

        frontLeft = new VictorSP(0);
        backLeft = new VictorSP(1);       
        frontRight = new VictorSP(2);
        backRight = new VictorSP(3);
        intakeMotor = new VictorSP(4);
        compressor = new Compressor(0);
        intakeSolenoid = new Solenoid(0);


        driveTrain = new RobotDrive(frontLeft, backLeft, frontRight, backRight);

        driveTrain.setSafetyEnabled(false);
        driveTrain.setExpiration(0.1);
        driveTrain.setSensitivity(0.5);
        driveTrain.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, false);
        driveTrain.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
        driveTrain.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false);
        driveTrain.setInvertedMotor(RobotDrive.MotorType.kRearRight, false);

        XboxController = new Joystick(2);

        rightJoystick = new Joystick(1);

        leftJoystick = new Joystick(0);
    }

/**
 * Constructor
 * 
 * @param parent
 */
DirectionalPad(final Joystick parent) {

    /* Initialize */
    this.parent = parent;
    this.up = new DPadButton(this, DPAD.UP);
    this.upRight = new DPadButton(this, DPAD.UP_RIGHT);
    this.right = new DPadButton(this, DPAD.RIGHT);
    this.downRight = new DPadButton(this, DPAD.DOWN_RIGHT);
    this.down = new DPadButton(this, DPAD.DOWN);
    this.downLeft = new DPadButton(this, DPAD.DOWN_LEFT);
    this.left = new DPadButton(this, DPAD.LEFT);
    this.upLeft = new DPadButton(this, DPAD.UP_LEFT);
}

/**
 * This function is run when the robot is first started up and should be used for any initialization code.
 */
@Override
public void robotInit()
{
    // UI
    mShooterJoystick = new Joystick(PortMap.sOPERATOR_JOYSTICK_PORT);
    mDriveJoystick = new Joystick(PortMap.sDRIVER_JOYSTICK_PORT);
    mDriverController = new DriverJoystick(mDriveJoystick);
    mOperatorController = new OperatorJoystick(mShooterJoystick);

    //Shooter
    mShooterMotor = new Talon(PortMap.sSHOOTER_MOTOR);
    mShooterSolenoid = new Solenoid(PortMap.sSHOOTER_PISTON);
    mShooter = new SnobotShooter(mShooterMotor, mShooterSolenoid, mOperatorController);

    //Drive Train
    mLeftMotor = new Talon(PortMap.sLEFT_DRIVE_MOTOR);
    mRightMotor = new Talon(PortMap.sRIGHT_DRIVE_MOTOR);
    mDriveTrain = new SnobotDriveTrain(mLeftMotor, mRightMotor, mDriverController);

    // Intake
    mUpperIntakeMotor = new Talon(PortMap.sUPPER_INTAKE_MOTOR);
    mLowerIntakeMotor = new Talon(PortMap.sLOWER_INTAKE_MOTOR);
    mIntake = new SnobotIntake(mLowerIntakeMotor, mUpperIntakeMotor, mOperatorController);

    addModule(mDriveTrain);
    addModule(mShooter);
    addModule(mIntake);

    initializeLogHeaders();

    // Now all the preferences should be loaded, make sure they are all
    // saved
    PropertyManager.saveIfUpdated();
}

public SnobotDriveXbaxJoystick(Joystick aJoystick, ILogger aLogger, AutonomousFactory aAutonFactory)
{

    mJoystick = aJoystick;
    mLogger = aLogger;

    mSwitchAppViewLatcher = new LatchedButton();
    mSwitchToFrontCameraLatcher = new LatchedButton();
    mSwitchToRearCameraLatcher = new LatchedButton();
    mRestartAppLatcher = new LatchedButton();
    mDriveToPegToggleButton = new ToggleButton();
    mDriveSmoothlyToPositionToggleButton = new ToggleButton();
}

public SnobotOperatorXbaxJoystick(Joystick aJoystick, ILogger aLogger)
{
    mJoystick = aJoystick;

    mToggleGreenLight = new ToggleButton(true);
    mToggleBlueLight = new ToggleButton(true);
    mLogger = aLogger;
}

public void arcade(Joystick stick, boolean twostick)
{
    double y = stick.getY();
    double x;
    if (twostick)
        {
            x = stick.getRawAxis(4);
        }
    else
        {
            x = stick.getX();
        }

    if (Math.abs(y) <= Constants.JOYSTICK_DEADBAND_V)
        {
            y = 0;
        }

    if (Math.abs(x) <= Constants.JOYSTICK_HEADBAND_H)
        {
            x = 0;
        }

    // "Exponential" drive, where the movements are more sensitive during slow movement,
    // permitting easier fine control
    x = Math.pow(x, 3);
    y = Math.pow(y, 3);
    arcade(y, x);
}

protected void execute() {
    Joystick joystickDrive = Robot.oi.getJoystickDrive();
    double deltaHeading = Robot.driveTrain.getCurrentHeading() - m_initialHeading;
    double joystickY = -joystickDrive.getAxis(Joystick.AxisType.kY);
    double outputMagnitude = .35;
            //Robot.driveTrain.scaleThrottle(joystickY);
    double curve = deltaHeading * - m_kp;
    System.out.println(deltaHeading);
    Robot.driveTrain.drive(outputMagnitude, curve);
}

public void arcadeDrive(Joystick stick) {
    double y = stick.getY() * (((stick.getThrottle() * -1) + 1) / -2);
    double x = stick.getX() * (((stick.getThrottle() * -1) + 1) / 2);
    if (Math.abs(y) <= Constants.deadBand) {
        y = 0;
    }
    if (Math.abs(x) <= Constants.horDeadBand) {
        x = 0;
    }
    x *= Math.abs(x);
    arcadeDrive(y, x);
}

public Robot() {
    //make objects for drive train, joystick, and gyro
    myRobot = new RobotDrive(new CANTalon(leftMotorChannel), new CANTalon(leftRearMotorChannel),
      new CANTalon(rightMotorChannel), new CANTalon(rightRearMotorChannel));
    myRobot.setInvertedMotor(MotorType.kFrontLeft, true); // invert the left side motors
    myRobot.setInvertedMotor(MotorType.kRearLeft, true); // you may need to change or remove this to match your robot

    joystick = new Joystick(0);
    gyro = new AnalogGyro(gyroChannel);
}

public Robot()
{
    //make objects for the drive train, gyro, and joystick
    myRobot = new RobotDrive(new CANTalon(leftMotorChannel), new CANTalon(
      leftRearMotorChannel), new CANTalon(rightMotorChannel),
      new CANTalon(rightRearMotorChannel));
    gyro = new AnalogGyro(gyroChannel);
    joystick = new Joystick(joystickChannel);
}

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);
}