add todo's after reviewing code with danny

[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[] potHandAngles;
  private double[] potArmAngles;

  // 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);
    potHandAngles = createHandPotArray();
    potArmAngles = createArmPotArray();
  }

  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 getAngleForHandLocation(int desiredArmLocation) {
    return potHandAngles[desiredArmLocation];
  }

  public double getAngleForArmLocation(int desiredArmLocation) {
    return potArmAngles[desiredArmLocation];
  }

  public double[] createHandPotArray() {
    double[] arr = new double[3];

    for (int i = 0; i < 3; i++) {
      arr[i] = 45 * i;
    }
    return arr;
  }

  public double[] createArmPotArray() {
    double[] arr = new double[3];

    for (int i = 0; i < 3; i++) {
      arr[i] = 45 * i;
    }
    return arr;
  }

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

  // TODO: figure out if measurements are all in inches
  public double getArmHorizontalDist() {
    double arm = Constants.DefenseArm.ARM_LENGTH * Math.cos(getArmPotAngle());
    double hand = Constants.DefenseArm.HAND_LENGTH
        * Math.cos(getHandPotAngle());
    return (arm + hand);
  }

  public double getArmHeight() {
    double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
    double arm = Constants.DefenseArm.ARM_LENGTH * Math.sin(getArmPotAngle());
    double hand = Constants.DefenseArm.HAND_LENGTH
        * Math.sin(getHandPotAngle());
    return (armMounted + arm + hand);
  }

  public boolean isOutsideRange() {
    if (getArmHorizontalDist() < 15)
      return false;
    return true;
  }

  @Override
  protected void initDefaultCommand() {
  }
}