Add object IntakeArm to robot.java

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / IntakeArm.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;

/***
 * The IntakeArm consists of two rollers that are controlled by one motor, with
 * a potentiometer on it.
 *
 * The motor controls the rollers, making them roll forwards and backwards. The
 * Intake rollers are on the back of the robot. As the rollers run, they intake
 * the ball.
 *
 * @author superuser
 *
 */

public class IntakeArm extends Subsystem {

  private CANTalon intakeRoller;
  private CANTalon intakeArm;
  private AnalogPotentiometer intakePot;
  private double[] potAngles = { 0, 45, 90 };

  public IntakeArm() {
    intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);
    intakeArm = new CANTalon(Constants.IntakeArm.ARM_PORT);
    intakePot = new AnalogPotentiometer(Constants.IntakeArm.POT_CHANNEL,
        Constants.IntakeArm.FULL_RANGE, Constants.IntakeArm.OFFSET);
  }

  /***
   * This method sets the voltage of the motor to intake the ball. The voltage
   * values are constants in Constants class
   */
  public void intakeBall() {
    intakeRoller.set(Constants.IntakeArm.INTAKE_SPEED);
  }

  /***
   * This method sets the voltage of the motor to output the ball. The voltage
   * values are constants in Constants class
   */
  public void outputBall() {
    intakeRoller.set(Constants.IntakeArm.OUTPUT_SPEED);
  }

  /***
   * This method gets you the current voltage of the motor that controls the
   * intake arm roller. The range of voltage is from [-1,1]. A negative voltage
   * makes the motor run backwards.
   *
   * @return Returns the voltage of the motor that controls the roller. The
   *         range of the voltage goes from [-1,1]. A negative voltage indicates
   *         that the motor is running backwards.
   */

  public double getRollerVoltage() {
    return intakeRoller.get();
  }

  /***
   * 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 voltage
   *          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 voltage) {
    if (voltage > 1)
      voltage = 1;
    else if (voltage < -1)
      voltage = -1;

    intakeArm.set(voltage);
  }

  /***
   * This method gets you the current voltage of the motor that controls the
   * intake arm. The range of voltage is from [-1,1]. A negative voltage makes
   * the motor run backwards.
   *
   * @return Returns the voltage of the motor that controls the arm. The range
   *         of the voltage goes from [-1,1]. A negative voltage indicates that
   *         the motor is running backwards.
   */

  public double getArmSpeed() {
    return intakeArm.get();
  }

  /***
   * This method checks to see if the presence of the ball inside is true or
   * false.
   *
   * @return Returns whether the ball is inside as true or false
   */

  public boolean isBallInside() {
    return true;
  }

  /***
   * This method checks to see if the motors controlling the rollers are
   * currently running.
   *
   * @return Returns whether the motors are currently running, and returns the
   *         state of the condition (true or false).
   *
   */

  public boolean areRollersRolling() {
    if (Math.abs(getRollerVoltage()) < 0.02)
      return false;
    return true;
  }

  /***
   * This method gets the angle of the potentiometer on the Intake Arm.
   *
   * @return angle of potentiometer
   */

  public double getArmAngle() {
    return intakePot.get() + Constants.IntakeArm.ZERO_ANGLE;
  }

  public void stop() {
    setArmSpeed(0);
  }

  public double getAngleForLevel(double targetLevel) {
    return potAngles[(int) (targetLevel - 1)];
  }

  @Override
  protected void initDefaultCommand() {

  }
}