Fix bug so that the sensitivity threshold worked correctly

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 566449b8286b1c0072eaaff849a1784bdec48086..79efd230ce862dfe0a450c0ba59d2fa5bc480a8d 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -77,9 +77,6 @@ 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 = 180; // in degrees
-               public final static double ARM_LENGTH = 15; // in inches
-               public final static double BALL_INTAKE_HEIGHT = 5; // random value
        }
 
        public static class DefenseArm {

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToBallIntakeHeight.java b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToBallIntakeHeight.java
new file mode 100644 (file)
index 0000000..805bd78
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToBallIntakeHeight.java
@@ -0,0 +1,5 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+public class MoveIntakeArmToBallIntakeHeight {
+
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToLevel.java b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToLevel.java
deleted file mode 100644 (file)
index d29386b..0000000
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToLevel.java
+++ /dev/null
@@ -1,52 +0,0 @@
-package org.usfirst.frc.team3501.robot.commands;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class MoveIntakeArmToLevel extends Command {
-
-       private double currentAngle;
-       private double targetAngle;
-       private double targetSpeed;
-       private double SENSITIVITY_THRESHOLD = 0.1;
-
-       public MoveIntakeArmToLevel(double level, double speed) {
-               requires(Robot.intakeArm);
-               targetAngle = Robot.intakeArm.getAngleForLevel(targetAngle);
-               targetSpeed = speed;
-
-       }
-
-       @Override
-       protected void initialize() {
-               currentAngle = Robot.intakeArm.getArmAngle();
-               double difference = targetAngle - currentAngle;
-               if (difference > 0) {
-                       Robot.intakeArm.setArmSpeed(targetSpeed);
-               } else {
-                       Robot.intakeArm.setArmSpeed(-targetSpeed);
-               }
-       }
-
-       @Override
-       protected void execute() {
-
-       }
-
-       @Override
-       protected boolean isFinished() {
-               currentAngle = Robot.intakeArm.getArmAngle();
-               double difference = Math.abs(targetAngle - currentAngle);
-               return (difference <= SENSITIVITY_THRESHOLD);
-       }
-
-       @Override
-       protected void end() {
-               Robot.intakeArm.stop();
-       }
-
-       @Override
-       protected void interrupted() {
-       }
-}