Add constant arm length and command getArmHeight

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index ae5941b3e21d9892b3865f655d7f29c64b9c8988..216d816d57c721dfb2550ee433b72f088e0d2a6f 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -77,7 +77,8 @@ public class Constants {
                public static final double OUTPUT_SPEED = -0.5;
                public final static double FULL_RANGE = 270.0; // in degrees
                public final static double OFFSET = -135.0; // in degrees
-               public final static double ZERO_ANGLE = 0;
+               public final static double ZERO_ANGLE = 180; // in degrees
+               public final static double ARM_LENGTH = 15; // in inches
        }
 
        public static class DefenseArm {

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
index 1314a544995ffc8b6370d80a9ae33d72c8ad3783..28dbb139910464e5c271387851ef91d18bf57670 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
@@ -123,7 +123,11 @@ public class IntakeArm extends Subsystem {
         * @return angle of potentiometer
         */
        public double getArmAngle() {
-               return intakePot.get();
+               return intakePot.get() + Constants.IntakeArm.ZERO_ANGLE;
+       }
+
+       public double getArmHeight() {
+               return Constants.IntakeArm.ARM_LENGTH * Math.sin(getArmAngle());
        }
 
        @Override