add arm methods getCurrentLevel() and updateLimitStates()

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / subsystems / Arm.java

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
index bc1c4ba9c8c9098d0d0cbfe7c2ef8ff2310e9935..61d3470adcd4a566eacc29725969963482cb906c 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
@@ -8,24 +8,24 @@ import edu.wpi.first.wpilibj.command.Subsystem;
 
 public class Arm extends Subsystem {
   private CANJaguar left, right;
-  private DigitalInput limit1, limit2, limit3, limit4;
-  private boolean limitSet1, limitSet2, limitSet3, limitSet4;
+  private DigitalInput[] limits;
+  private int currentLevel;
 
   public Arm() {
     left = new CANJaguar(RobotMap.ARM_LEFT);
     right = new CANJaguar(RobotMap.ARM_RIGHT);
-    limit1 = new DigitalInput(RobotMap.ARM_LIMIT_1);
-    limit2 = new DigitalInput(RobotMap.ARM_LIMIT_2);
-    limit3 = new DigitalInput(RobotMap.ARM_LIMIT_3);
-    limit4 = new DigitalInput(RobotMap.ARM_LIMIT_4);
-    limitSet1 = false;
-    limitSet2 = false;
-    limitSet3 = false;
-    limitSet4 = false;
+    limits = new DigitalInput[RobotMap.NUM_LIMITS];
+    currentLevel = 0;
   }
 
   public void initDefaultCommand() {
   }
+  
+  public void initializeArrayValues() {
+         for (int i = 0; i < RobotMap.NUM_LIMITS; i++) {
+                 limits[i] = new DigitalInput(i);
+         }
+  }
 
   public void fineTuneControl(double d) {
     if (Math.abs(d) < 0.05) {
@@ -55,4 +55,15 @@ public class Arm extends Subsystem {
     left.set(0);
     right.set(0);
   }
+  
+  public void updateLimitStates() {
+         for (int i = 0; i < RobotMap.NUM_LIMITS; i++) {
+                 if (limits[i].get() && i > currentLevel) currentLevel++;
+                 if (limits[i].get() && i <= currentLevel) currentLevel--;
+         }
+  }
+  
+  public int getCurrentLevel() {
+         return currentLevel;
+  }
 }