Add pid subsystem

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / IntakeArm.java

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
index 4f3daa6be6b130664ec00a16333b97308b26d977..a9e9ecd5148c0f76dd045c8e38b4550dc47373cf 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
@@ -10,52 +10,56 @@ 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.
+ * 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.INTAKE_PORT);
+    intakeArm = new CANTalon(Constants.IntakeArm.ARM_PORT);
+    intakePot = new AnalogPotentiometer(Constants.IntakeArm.POT_CHANNEL,
+        Constants.IntakeArm.FULL_RANGE, Constants.IntakeArm.OFFSET);
   }
 
-  /*
-   * Intake only moves once at the beginning of the match. It lowers at the
-   * beginning of the match and is held there by mechanical stops until the end
-   * of the match.
-   *
-   * Must be used in a command that has a timer variable to stop it.
-   */
-
   /***
-   * These two methods (intakeBall and outputBall)sets the voltage of the motor.
-   * The voltage values are constants in Constants class
+   * 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.
+   * 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.
+   *         range of the voltage goes from [-1,1]. A negative voltage indicates
+   *         that the motor is running backwards.
    */
 
   public double getRollerVoltage() {
@@ -68,11 +72,11 @@ public class IntakeArm extends Subsystem {
    *
    * @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.
+   *          the arm motor is from [-1,1]. A negative voltage makes the
+   *          direction of the motor go backwards.
    */
 
-  public void setArmVoltage(double voltage) {
+  public void setArmSpeed(double voltage) {
     if (voltage > 1)
       voltage = 1;
     else if (voltage < -1)
@@ -83,15 +87,15 @@ public class IntakeArm extends Subsystem {
 
   /***
    * 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.
+   * 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.
+   * @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 getArmVoltage() {
+  public double getArmSpeed() {
     return intakeArm.get();
   }
 
@@ -116,6 +120,8 @@ public class IntakeArm extends Subsystem {
    */
 
   public boolean areRollersRolling() {
+    if (Math.abs(getRollerVoltage()) < 0.02)
+      return false;
     return true;
   }
 
@@ -124,8 +130,17 @@ public class IntakeArm extends Subsystem {
    *
    * @return angle of potentiometer
    */
-  public double getIntakePot() {
-    return intakePot.get();
+
+  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