add methods that return how far arm is sticking out and how high arm is at, and const...

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 6f2b3cd0b823d403dff401a1ec213d3994524b76..bd162c0d2b57435ae8b6af2ff539bea1ff64b1c4 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -89,6 +89,10 @@ public class Constants {
     public final static double FULL_RANGE = 270.0; // in degrees
     public final static double OFFSET = -135.0; // in degrees
     public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3 level
+    public final static double ARM_LENGTH = 0; // TODO: find actual length
+    public final static double HAND_LENGTH = 0; // TODO: find actual length
+    public final static double ARM_MOUNTED_HEIGHT = 0; // TODO: find actual
+                                                       // height
   }
 
   public enum Defense {

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
index 9b745760186031c8c9f4fe0b4e0b599ec500fc4e..7dfb6ddea4aa203db60ffca82947b01efef1264b 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
@@ -95,6 +95,27 @@ public class DefenseArm extends Subsystem {
     defenseHand.set(speed);
   }
 
+  public double getArmHorizontalDist() {
+    double arm = Constants.DefenseArm.ARM_LENGTH * Math.cos(getArmPotAngle());
+    double hand = Constants.DefenseArm.HAND_LENGTH
+        * Math.cos(getHandPotAngle());
+    return (arm + hand);
+  }
+
+  public double getArmHeight() {
+    double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
+    double arm = Constants.DefenseArm.ARM_LENGTH * Math.sin(getArmPotAngle());
+    double hand = Constants.DefenseArm.HAND_LENGTH
+        * Math.sin(getHandPotAngle());
+    return (armMounted + arm + hand);
+  }
+
+  public boolean isOutsideRange() {
+    if (getArmHorizontalDist() < 15)
+      return false;
+    return true;
+  }
+
   @Override
   protected void initDefaultCommand() {
   }