Fix some errors

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DefenseArm.java

package org.usfirst.frc.team3501.robot.subsystems;

import org.usfirst.frc.team3501.robot.Constants;

import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.command.Subsystem;

public class DefenseArm extends Subsystem {
  private AnalogPotentiometer defenseArmPotentiometer;
  private AnalogPotentiometer defenseHandPotentiometer;
  private CANTalon defenseArm;
  private CANTalon defenseHand;
  private double hookHeight;
  private double footHeight;
  private double[] potAngles = { 0, 45, 90 };

  // angles corresponding to pre-determined heights we will need

  public DefenseArm() {
    defenseArmPotentiometer = new AnalogPotentiometer(
        Constants.DefenseArm.ARM_CHANNEL,
        Constants.DefenseArm.FULL_RANGE,
        Constants.DefenseArm.OFFSET);
    defenseHandPotentiometer = new AnalogPotentiometer(
        Constants.DefenseArm.HAND_CHANNEL,
        Constants.DefenseArm.FULL_RANGE,
        Constants.DefenseArm.OFFSET);

    defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT);
    defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT);
  }

  public double getArmPotAngle() {
    return defenseArmPotentiometer.get();
  }

  public double getHandPotAngle() {
    return defenseHandPotentiometer.get();
  }

  /***
   * This method takes an arm location as input (range of [0,2])
   * Returns the angle of the arm corresponding to that arm location
   *
   * @param desiredArmLocation
   *          takes an arm location ranging from [0,2]
   *          0 is the lowest position of arm
   *          2 is the highest position of arm
   * @return
   *         the angle of the arm corresponding to that arm location
   */
  public double getLevelValue(int level) {
    if (level >= potAngles.length)
      return potAngles[level];
    else
      return 0;
  }

  /***
   * This method sets the voltage of the arm motor. The range is from [-1,1]. A
   * negative voltage makes the direction of the motor go backwards.
   *
   * @param speed
   *          The voltage that you set the motor at. The range of the voltage of
   *          the arm motor is from [-1,1]. A
   *          negative voltage makes the direction of the motor go backwards.
   */

  public void setArmSpeed(double speed) {
    if (speed > 1)
      speed = 1;
    else if (speed < -1)
      speed = -1;

    defenseArm.set(speed);
  }

  /***
   * This method sets the voltage of the hand motor. The range is from [-1,1]. A
   * negative voltage makes the direction of the motor go backwards.
   *
   * @param speed
   *          The voltage that you set the motor at. The range of the voltage of
   *          the hand motor is from [-1,1]. A
   *          negative voltage makes the direction of the motor go backwards.
   */

  public void setHandSpeed(double speed) {
    if (speed > 1)
      speed = 1;
    else if (speed < -1)
      speed = -1;

    defenseHand.set(speed);
  }

  @Override
  protected void initDefaultCommand() {
  }
}