Delete MoveIntakeArmToHeight and created MoveIntakeArmToLevel

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToAngle.java
deleted file mode 100644 (file)
index a4e7f23..0000000
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToAngle.java
+++ /dev/null
@@ -1,56 +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 MoveIntakeArmToAngle extends Command {
-       private double targetAngle;
-       private double currentAngle;
-       private double targetSpeed;
-       private double SENSITIVITY_THRESHOLD = 0.1;
-       private double NO_MOVEMENT_SPEED = 0;
-
-       public MoveIntakeArmToAngle(double angle, double speed) {
-               requires(Robot.intakeArm);
-               targetAngle = angle;
-               targetSpeed = speed;
-       }
-
-       @Override
-       protected void initialize() {
-               currentAngle = Robot.intakeArm.getArmAngle();
-               double difference = targetAngle - currentAngle;
-               if (difference > 0) {
-                       Robot.intakeArm.setArmVoltage(targetSpeed);
-               } else {
-                       Robot.intakeArm.setArmVoltage(-targetSpeed);
-               }
-       }
-
-       @Override
-       protected void execute() {
-
-       }
-
-       @Override
-       protected boolean isFinished() {
-               currentAngle = Robot.intakeArm.getArmAngle();
-               double distance = Math.abs(currentAngle - targetAngle);
-               if (distance <= SENSITIVITY_THRESHOLD) {
-                       return true;
-               } else {
-                       return false;
-               }
-
-       }
-
-       @Override
-       protected void end() {
-               Robot.intakeArm.setArmVoltage(NO_MOVEMENT_SPEED);
-       }
-
-       @Override
-       protected void interrupted() {
-       }
-}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToHeight.java b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToHeight.java
deleted file mode 100644 (file)
index e4032c4..0000000
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToHeight.java
+++ /dev/null
@@ -1,56 +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 MoveIntakeArmToHeight extends Command {
-       private double targetHeight;
-       private double currentHeight;
-       private double targetSpeed;
-       private double SENSITIVITY_THRESHOLD = 0.1;
-       private double NO_MOVEMENT_SPEED = 0;
-
-       public MoveIntakeArmToHeight(double height, double speed) {
-               requires(Robot.intakeArm);
-               targetHeight = height;
-               targetSpeed = speed;
-       }
-
-       @Override
-       protected void initialize() {
-               currentHeight = Robot.intakeArm.getArmHeight();
-               double difference = targetHeight - currentHeight;
-               if (difference > 0) {
-                       Robot.intakeArm.setArmVoltage(targetSpeed);
-               } else {
-                       Robot.intakeArm.setArmVoltage(-targetSpeed);
-               }
-       }
-
-       @Override
-       protected void execute() {
-
-       }
-
-       @Override
-       protected boolean isFinished() {
-               currentHeight = Robot.intakeArm.getArmHeight();
-               double distance = Math.abs(currentHeight - targetHeight);
-               if (distance <= SENSITIVITY_THRESHOLD) {
-                       return true;
-               } else {
-                       return false;
-               }
-
-       }
-
-       @Override
-       protected void end() {
-               Robot.intakeArm.setArmVoltage(NO_MOVEMENT_SPEED);
-       }
-
-       @Override
-       protected void interrupted() {
-       }
-}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToLevel.java b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToLevel.java
new file mode 100644 (file)
index 0000000..d29386b
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToLevel.java
@@ -0,0 +1,52 @@
+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() {
+       }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
index 28dbb139910464e5c271387851ef91d18bf57670..9089852a6f790a69b63d6b3f1360801cb1049680 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java
@@ -22,12 +22,14 @@ public class IntakeArm extends Subsystem {
        private CANTalon intakeRoller;
        private CANTalon intakeArm;
        private AnalogPotentiometer intakePot;
+       private double[] potAngles = { 0, 45, 90 };
 
        public IntakeArm() {
                intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);
                intakeArm = new CANTalon(Constants.IntakeArm.ARM_PORT);
                intakePot = new AnalogPotentiometer(Constants.IntakeArm.POT_CHANNEL, Constants.IntakeArm.FULL_RANGE,
                                Constants.IntakeArm.OFFSET);
+
        }
 
        /***
@@ -70,7 +72,7 @@ public class IntakeArm extends Subsystem {
         *            makes the direction of the motor go backwards.
         */
 
-       public void setArmVoltage(double voltage) {
+       public void setArmSpeed(double voltage) {
                if (voltage > 1)
                        voltage = 1;
                else if (voltage < -1)
@@ -89,7 +91,7 @@ public class IntakeArm extends Subsystem {
         *         that the motor is running backwards.
         */
 
-       public double getArmVoltage() {
+       public double getArmSpeed() {
                return intakeArm.get();
        }
 
@@ -122,12 +124,17 @@ public class IntakeArm extends Subsystem {
         *
         * @return angle of potentiometer
         */
+
        public double getArmAngle() {
                return intakePot.get() + Constants.IntakeArm.ZERO_ANGLE;
        }
 
-       public double getArmHeight() {
-               return Constants.IntakeArm.ARM_LENGTH * Math.sin(getArmAngle());
+       public void stop() {
+               setArmSpeed(0);
+       }
+
+       public double getAngleForLevel(double targetLevel) {
+               return potAngles[(int) (targetLevel - 1)];
        }
 
        @Override