public void shooterLoft() {
   // Ajust loft
   double operatorLeftY = xboxOperator.getLeftY(); // operator has loft control
   int loftPotValue = loftPot.getValue();
   if (xboxOperator.getBtnX()) { // hold to ajust for close shot
     if (loftPotValue < closeShotValue - focusTolerance) {
       if (loftPotValue < closeShotValue - angleTolerance) {
         shooterLoft.set(-1);
       } else {
         shooterLoft.set(-0.001);
       }
     } else if (loftPotValue > closeShotValue + focusTolerance) {
       if (loftPotValue > closeShotValue + angleTolerance) {
         shooterLoft.set(1);
       } else {
         shooterLoft.set(0.001);
       }
     } else {
       shooterLoft.set(0);
     }
   } else if (xboxOperator.getBtnY()) { // hold to ajust for far shot
     if (loftPotValue < farShotValue - focusTolerance) {
       if (loftPotValue < farShotValue - angleTolerance) {
         shooterLoft.set(-1);
       } else {
         shooterLoft.set(-0.001);
       }
     } else if (loftPotValue > farShotValue + focusTolerance) {
       if (loftPotValue > farShotValue + angleTolerance) {
         shooterLoft.set(1);
       } else {
         shooterLoft.set(0.001);
       }
     } else {
       shooterLoft.set(0);
     }
   } else if (xboxOperator.getBtnB()) { // hold to ajust for feeding
     if (loftPotValue < shooterFeedingValue - focusTolerance) {
       if (loftPotValue < shooterFeedingValue - angleTolerance) {
         shooterLoft.set(-1);
       } else {
         shooterLoft.set(-0.001);
       }
     } else if (loftPotValue > shooterFeedingValue + focusTolerance) {
       if (loftPotValue > shooterFeedingValue + angleTolerance) {
         shooterLoft.set(1);
       } else {
         shooterLoft.set(0.001);
       }
     } else {
       shooterLoft.set(0);
     }
   } else if (loftPotValue > maxHeightValue
       && loftPotValue < minHeightValue
       && ((operatorLeftY > .3) || operatorLeftY < -.3)) { // move loft with left operator stick
     shooterLoft.set(operatorLeftY);
   } else {
     shooterLoft.set(0); // don't move loft
   }
 } // end of shooter loft
  public void drive() {
    // Driving controls
    /* flips which direction on the joysticks is 'forward'
     * the variable 'controlFlipClean' needs some explanation:
     * it forces the driver to release the button before it recognizes
     * another click. */
    if (xboxDriver.getBtnL3()) { // left stick click
      if (controlFlipClean) {
        controlFlipClean = false;
        controlFlip = !controlFlip;
      }
    } else {
      controlFlipClean = true;
    }

    // controls right side
    double yRight = -xboxDriver.getRightY();
    // controls left side
    double yLeft = xboxDriver.getLeftY();

    // trigger values will change the maximum speed of the robot
    double throttle = xboxDriver.getTriggers();

    // half speed
    if (throttle > 0.5) {
      yRight = yRight * 0.5;
      yLeft = yLeft * 0.5;
    }
    // quarter speed
    if (throttle < -0.5) {
      yRight = yRight * 0.25;
      yLeft = yLeft * 0.25;
    }

    // flip controls if we need to. Make them go 'backwards'.
    // We need to swap and yRight and yLeft values.
    if (controlFlip) {
      double tempRight = yRight;
      yRight = yLeft;
      yLeft = tempRight;
    }

    /* DEADZONE **
     * The springs in the joysticks wear out so they don't always come
     * back to the centre. To fix this we're going to ignore a small region
     * around the centre.
     */
    if (yRight > -DEADZONE && yRight < DEADZONE) {
      yRight = 0;
    }
    if (yLeft > -DEADZONE && yLeft < DEADZONE) {
      yLeft = 0;
    }

    vicDriveRight.set(yRight);
    vicDriveLeft.set(yLeft);
  } // end of drive
 public void armUse() {
   // Arm contols
   int armPotValue = armPot.getValue();
   if (xboxDriver.getBtnB()) { // arm for slot feeding
     if (armPotValue < armSlotFeedingValue - angleTolerance) {
       armVictor.set(-.2);
     } else if (armPotValue < armSlotFeedingValue - focusTolerance) {
       armVictor.set(-.02);
     } else if (armPotValue > armSlotFeedingValue + angleTolerance) {
       armVictor.set(.5);
     } else if (armPotValue > armSlotFeedingValue + focusTolerance) {
       armVictor.set(.05);
     } else {
       armVictor.set(0);
     }
   } else if (xboxDriver.getBtnY()) { // arm for shovel feeding
     if (armPotValue < armShovelFeedingValue - angleTolerance) {
       armVictor.set(-.2);
     } else if (armPotValue < armShovelFeedingValue - focusTolerance) {
       armVictor.set(-.02);
     } else if (armPotValue > armShovelFeedingValue + angleTolerance) {
       armVictor.set(.5);
     } else if (armPotValue > armShovelFeedingValue + focusTolerance) {
       armVictor.set(.05);
     } else {
       armVictor.set(0);
     }
   } else {
     if (xboxDriver.getBtnLB() && armPotValue > minArmValue) {
       armVictor.set(-.2); // driver left bumper = down, right bumper = up;
     } else if (xboxDriver.getBtnRB() && armPotValue < maxArmValue) {
       armVictor.set(.5);
     } else {
       armVictor.set(0);
     }
     if (xboxDriver.getBtnA()
         && armPotValue
             < climbStopValue) { // when driver controls, hold button A to use winch, release to
       // stop
       winchRelay.set(Relay.Value.kOn);
       winchRelay.setDirection(Relay.Direction.kReverse);
     } else {
       winchRelay.set(Relay.Value.kOff);
     }
   }
 } // end of arm use
  public void shooterUse() {
    // Betty operation
    if (xboxOperator.getBtnLB()) { // hold right bumper to fire
      starterDelay++;
      if (xboxOperator.getTriggers() < -0.5) { // feeder shot
        if (shootingSpeed > feederShotSpeed) { // accelerating shooter speed until top speed
          shootingSpeed = shootingSpeed + flyWheelAcceleration;
        } else {
          shootingSpeed = feederShotSpeed; // ensures desired value
        }
      } else { // normal shot
        if (shootingSpeed > normalShotSpeed) { // accelerating shooter speed until top speed
          shootingSpeed = shootingSpeed + flyWheelAcceleration;
        } else {
          shootingSpeed = normalShotSpeed; // ensures desired value
        }
      }
      shooterWheel.set(shootingSpeed);
      if (xboxOperator.getBtnRB() && starterDelay > 60) { // shoot normal shot
        bettyRelay.setDirection(Relay.Direction.kForward);
        bettyRelay.set(Relay.Value.kOn);
      } else if (xboxOperator.getBtnB() && starterDelay > 60) { // shoot normal shot
        bettyRelay.setDirection(Relay.Direction.kReverse);
        bettyRelay.set(Relay.Value.kOn);
      } else {
        bettyRelay.set(Relay.Value.kOff);
      }

    } else if (bettyOpen.get()) { // close betty if not shooting
      shootingSpeed = 0.0;
      bettyRelay.setDirection(Relay.Direction.kForward);
      bettyRelay.set(Relay.Value.kOn);
      shooterWheel.set(0);
      starterDelay = 0;
    } else {
      shootingSpeed = 0.0;
      bettyRelay.set(Relay.Value.kOff);
      shooterWheel.set(0);
      starterDelay = 0;
    }
  } // end of shooter use
 /** This function is called periodically during test mode */
 public void testPeriodic() {
   Watchdog.getInstance().feed(); // Tell watchdog we are running
   drive();
   shooterUse();
   //        shooterLoft();
   armUse();
   System.out.println(
       "loftPot value: " + loftPot.getValue() + "       armPot Value: " + armPot.getValue());
   if (xboxDriver
       .getBtnBACK()) { // when driver controls, hold button BACK to unwind winch, release to stop,
     // only for testing
     winchRelay.set(Relay.Value.kOn);
     winchRelay.setDirection(Relay.Direction.kForward);
   }
 }
  /** This function is called periodically during operator control */
  public void teleopPeriodic() {
    Watchdog.getInstance().feed(); // Tell watchdog we are running

    // activate the robot by pushing start button
    if (xboxDriver.getBtnSTART()) {
      teleopActive = true;
    }
    if (!teleopActive) {
      return; // only run if teleop is active
    }
    if (lightsTimer.get() > 90000) {
      lightsOutput1.set(true);
      lightsOutput2.set(true);
      lightsOutput3.set(false);
    }

    drive();
    shooterUse();
    //        shooterLoft();
    armUse();
  }