add arm methods getCurrentLevel() and updateLimitStates()

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/RobotMap.java b/src/org/usfirst/frc3501/RiceCatRobot/RobotMap.java
index 9b6d60d2feec1dc6ec24bfa2e3f56c33888a7c7c..fcec3004b20c2b6d0831bf843cffd9c6c6cd0084 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/RobotMap.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/RobotMap.java
@@ -23,8 +23,7 @@ public class RobotMap {
 
   public static final int ARM_LEFT = 2, ARM_RIGHT = 7;
   public static final double ARM_HIGH_SPEED = 0.5, ARM_LOW_SPEED = 0.5;
-  public static final int ARM_LIMIT_1 = 0, ARM_LIMIT_2 = 1, ARM_LIMIT_3 = 2, ARM_LIMIT_4 = 3;
-
+  public static final int NUM_LIMITS = 4;
   // Claw
   public static final int SOLENOID_FORWARD = 0, SOLENOID_REVERSE = 1,
       MODULE_NUMBER = 0;

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;
+  }
 }