add methods that return how far arm is sticking out and how high arm is at, and const...

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

  // 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 getArmHorizontalDisplacement() {
    double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH
        * Math.cos(getArmPotAngle());
    double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH
        * Math.cos(getHandPotAngle());
    return (armHorizontalDisplacement + handHorizontalDisplacement);
  }

  public double getArmVerticalDisplacement() {
    double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
    double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH
        * Math.sin(getArmPotAngle());
    double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH
        * Math.sin(getHandPotAngle());
    return (armMounted + armVerticalDisplacement + handVerticalDisplacement);
  }

  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() {
  }
}