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

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DefenseArm.java

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() {
   }