public void storeLoaderAngle(String sPosition) {
   float volts = (float) leftAngleCh.getVoltage();
   float voltsR = (float) rightAngleCh.getVoltage();
   m_preferences.putFloat(sPosition, volts);
   m_preferences.putFloat(sPosition + "R", voltsR);
   System.out.println(sPosition + " = " + volts);
 }
 /** 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);
   }
 }
 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 boolean isDetensioned() {
   double curr_volts = sensor.getAverageVoltage();
   boolean on_target =
       ((curr_volts > (slack_tension_volts - threshold_volts))
           && (curr_volts < (slack_tension_volts + threshold_volts)));
   boolean at_min = leftMin.get();
   return (on_target || at_min);
 }
 // basic arcadeDrive: y=forward/backward speed, x=left/right speed
 public void arcadeDrive(double y, double x) {
   Preferences p = Preferences.getInstance();
   final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
   robotDrive.arcadeDrive(y, x + 0.05);
   SmartDashboard.putNumber("DriveTrainGyro", -gyro.getAngle()); // upside down
   SmartDashboard.putNumber("RangefinderVoltage", ultraDist.getVoltage());
 } // end arcadeDrive
  public void teleopPeriodic() {
    smart.putNumber("distance", ultrasonic.getVoltage());
    setSuckingLED();
    setLEDTeamColour();

    // smart dashboard stuff
    smart.putBoolean("Fully Pressurized", compressor.getPressureSwitchValue());

    smart.putBoolean("fast gear", sol1.get() == true && sol2.get() == false);
    smart.putBoolean("slow gear", sol1.get() == false && sol2.get() == true);

    if (gyro.getAngle() > 360 || gyro.getAngle() < -360) {
      gyro.reset();
    }
    smart.putBoolean("good angle", gyro.getAngle() < 30 && gyro.getAngle() > -30);
    smart.putBoolean(
        "too far right", (gyro.getAngle() > 30 && gyro.getAngle() < 180) || gyro.getAngle() < -180);
    smart.putBoolean(
        "too far left", (gyro.getAngle() < -30 && gyro.getAngle() > -180) || gyro.getAngle() > 180);

    if (ultrasonic.getVoltage() < 0.55) {
      tooClose = true;
    } else {
      tooClose = false;
    }
    smart.putBoolean("Too close", tooClose);

    if (ultrasonic.getVoltage() > 0.8) {
      tooFar = true;
    } else {
      tooFar = false;
    }
    smart.putBoolean("Too far", tooFar);

    if (ultrasonic.getVoltage() >= 0.55 && ultrasonic.getVoltage() <= 0.8) {
      inRange = true;
    } else {
      inRange = false;
    }
    smart.putBoolean("In range", inRange);
  }
 public void autonomousPeriodic() {
   relay.set(Relay.Value.kOn);
   smart.putNumber("distance", ultrasonic.getVoltage());
   drive.setRun(false);
   loadAndShoot.setRun(false);
   //    relay.set(Relay.Value.kOn);
   if (autoTimer.get() < autoMoveForwardTime) {
     System.out.println(
         "Moving forward, Timer at "
             + autoTimer.get()
             + ", Ultrasonic at "
             + ultrasonic.getAverageVoltage());
   }
   if (autoTimer.get() >= autoMoveForwardTime && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
     stop();
     if (!autonomousStopped) {
       autonomousStopped = true;
       autoTimeAtStop = autoTimer.get();
     }
     System.out.println("Stopped, Ultrasonic at " + ultrasonic.getAverageVoltage());
   }
   if (autoTimer.get() >= autoTimeAtStop + 1 && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
     autoStop = true;
     shoot();
     System.out.println("Shooting, Ultrasonic at " + ultrasonic.getAverageVoltage());
   }
 }
 // aligns robot for shooting
 public void alignToShoot(double left, double right) {
   Preferences p = Preferences.getInstance();
   final double kShootLimitVoltage = p.getDouble("DriveTrainShootLimitVoltage", 0.0);
   double maxVoltage = ultraDist.getVoltage();
   for (int i = 0; i < 10; i++) {
     double voltage = ultraDist.getVoltage();
     if (voltage > maxVoltage) {
       maxVoltage = voltage;
     }
   }
   SmartDashboard.putNumber(
       "RangefinderVoltage", maxVoltage); // want to see on competition dashboard!!
   System.out.println(
       "RangefinderVoltage: " + maxVoltage + " kShootLimitVoltage: " + kShootLimitVoltage);
   if (maxVoltage <= kShootLimitVoltage) {
     hasBeenAchieved = true;
   }
   if (left <= 0.0 && hasBeenAchieved) {
     left = 0.0;
   }
   arcadeDrive(left, right);
 } // end alignToShoot
 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 double getTensionLevel() {
   return sensor.getAverageVoltage();
 }
 protected double returnPIDInput() {
   return sensor.getAverageVoltage();
 }
 // The absolute returns a voltage, 0 - 5, thus, any voltage *72 will = deg.
 public double getAbsoluteCurrentPosition() {
   return analogInputAbsolute.getAverageVoltage() * 72;
 }
 public double getAbsoluteVoltage() {
   return analogInputAbsolute.getAverageVoltage();
 }
 /**
  * Read the battery voltage from the specified AnalogChannel.
  *
  * <p>This accessor assumes that the battery voltage is being measured through the voltage divider
  * on an analog breakout.
  *
  * @return The battery voltage.
  */
 public double getBatteryVoltage() {
   // The Analog bumper has a voltage divider on the battery source.
   // Vbatt *--/\/\/\--* Vsample *--/\/\/\--* Gnd
   //         680 Ohms            1000 Ohms
   return m_batteryChannel.getAverageVoltage() * (1680.0 / 1000.0);
 }