From: Lauren Meier <meier.lauren@gmail.com>
Date: Tue, 17 Nov 2015 05:28:25 +0000 (-0800)
Subject: add arm methods getCurrentLevel() and updateLimitStates()
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=refs%2Fheads%2Flauren%2FlimitSwitch;p=3501%2F2015-FRC-Spark

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 9b6d60d..fcec300 100644
--- 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 bc1c4ba..61d3470 100644
--- 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;
+  }
 }