From: Cindy Zhang <cindyzyx9@gmail.com>
Date: Fri, 12 Feb 2016 02:31:57 +0000 (-0800)
Subject: add methods that return how far arm is sticking out and how high arm is at, and const... 
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=01654890b7ba07f98235c5fa43c9a98c089769ce;p=3501%2Fstronghold-2016

add methods that return how far arm is sticking out and how high arm is at, and constants that need filling in
---

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 6f2b3cd0..bd162c0d 100644
--- 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 9b745760..7dfb6dde 100755
--- 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() {
   }