From: Shivani Oghanta <shivani.oghanta@gmail.com>
Date: Thu, 11 Feb 2016 04:09:02 +0000 (-0800)
Subject: Delete MoveIntakeArmToHeight and created MoveIntakeArmToLevel
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=021350d2fae1a051b3b4dce3448d2fb247c096ac;p=3501%2Fstronghold-2016

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
index a4e7f23c..00000000
--- 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
index e4032c43..00000000
--- 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
index 00000000..d29386b7
--- /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 28dbb139..9089852a 100755
--- 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