implement ShootAtHighGoal and add some helper methods

[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.CANTalon;
import edu.wpi.first.wpilibj.DoubleSolenoid;
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 DoubleSolenoid leftIntake, rightIntake;
  public static double moveIntakeArmSpeed = 0;

  public IntakeArm() {
    intakeRoller = new CANTalon(Constants.IntakeArm.INTAKE_ROLLER_PORT);

    leftIntake = new DoubleSolenoid(Constants.IntakeArm.LEFT_INTAKE_MODULE,
        Constants.IntakeArm.LEFT_INTAKE_FORWARD,
        Constants.IntakeArm.LEFT_INTAKE_REVERSE);

    rightIntake = new DoubleSolenoid(Constants.IntakeArm.RIGHT_INTAKE_MODULE,
        Constants.IntakeArm.RIGHT_INTAKE_FORWARD,
        Constants.IntakeArm.RIGHT_INTAKE_REVERSE);
  }

  public void retractPistons() {
    leftIntake.set(Constants.IntakeArm.RETRACT);
    rightIntake.set(Constants.IntakeArm.RETRACT);
  }

  public void extendPistons() {
    leftIntake.set(Constants.IntakeArm.EXTEND);
    rightIntake.set(Constants.IntakeArm.EXTEND);
  }

  public boolean isExtended() {
    return (leftIntake.get() == Constants.IntakeArm.EXTEND
        && rightIntake.get() == Constants.IntakeArm.EXTEND);
  }

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

  @Override
  protected void initDefaultCommand() {

  }
}