Add buttons to move intake arm.

[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 leftIntakeArm;
  private CANTalon rightIntakeArm;
  private AnalogPotentiometer intakePot;
  public static double[] potAngles = { 0, 30, 45, 90 }; // TODO: correct angles
  public static double moveIntakeArmSpeed = 0;

  public IntakeArm() {
    intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);
    leftIntakeArm = new CANTalon(Constants.IntakeArm.LEFT_ARM_PORT);
    rightIntakeArm = new CANTalon(Constants.IntakeArm.RIGHT_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);
  }

  public void stopRollers() {
    intakeRoller.set(0);
  }

  /***
   * 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 voltages of the arm motors. 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 motors at. The range of the voltage
   *          of the arm motors is from [-1,1]. A negative voltage makes the
   *          direction of the motors go backwards.
   */

  public void setArmSpeeds(double voltage) {
    if (voltage > 1)
      voltage = 1;
    else if (voltage < -1)
      voltage = -1;

    leftIntakeArm.set(voltage);
    rightIntakeArm.set(voltage);
  }

  /***
   * This method gets you the current voltage of the left 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 left 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 getLeftArmSpeed() {
    return leftIntakeArm.get();
  }

  /***
   * This method gets you the current voltage of the right 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 right 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 getRightArmSpeed() {
    return rightIntakeArm.get();
  }

  public CANTalon getLeftIntakeArmMotor() {
    return leftIntakeArm;
  }

  public CANTalon getRightIntakeArmMotor() {
    return rightIntakeArm;
  }

  /***
   * 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() {
    setArmSpeeds(0);
  }

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

  @Override
  protected void initDefaultCommand() {

  }
}