From 702937bc5fd2580c343399fb61598b1ca942d852 Mon Sep 17 00:00:00 2001
From: Nam Tran <tranngocnam97@yahoo.com>
Date: Sun, 1 Feb 2015 16:51:20 -0500
Subject: [PATCH] Moved tote elevator commands to new package

---
 .../toteElevator/ToteElevatorDown.java        | 50 +++++++++++++++++
 .../commands/toteElevator/ToteElevatorUp.java | 54 +++++++++++++++++++
 2 files changed, 104 insertions(+)
 create mode 100644 src/org/team708/robot/commands/toteElevator/ToteElevatorDown.java
 create mode 100644 src/org/team708/robot/commands/toteElevator/ToteElevatorUp.java

diff --git a/src/org/team708/robot/commands/toteElevator/ToteElevatorDown.java b/src/org/team708/robot/commands/toteElevator/ToteElevatorDown.java
new file mode 100644
index 0000000..b73f3a7
--- /dev/null
+++ b/src/org/team708/robot/commands/toteElevator/ToteElevatorDown.java
@@ -0,0 +1,50 @@
+package org.team708.robot.commands.toteElevator;
+import org.team708.robot.Robot;
+import org.team708.robot.util.Math708;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ToteElevatorDown extends Command {
+	
+	private final double threshold = 1;
+
+    public ToteElevatorDown() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    	requires(Robot.toteElevator);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    	if (!Robot.toteElevator.elevatorDown) {
+    		Robot.toteElevator.lowerTote();
+    	}
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	return Math708.isWithinThreshold(Robot.toteElevator.getEncoderDistance(), -Robot.toteElevator.TOP_ENCODER_DISTANCE, threshold)
+    				|| Robot.toteElevator.elevatorDown;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	Robot.toteElevator.stopTote();
+    	Robot.toteElevator.elevatorDown = true;
+    	Robot.toteElevator.setToteCount(0);
+    	Robot.toteElevator.resetEncoder();
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems are scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}
diff --git a/src/org/team708/robot/commands/toteElevator/ToteElevatorUp.java b/src/org/team708/robot/commands/toteElevator/ToteElevatorUp.java
new file mode 100644
index 0000000..c742c3e
--- /dev/null
+++ b/src/org/team708/robot/commands/toteElevator/ToteElevatorUp.java
@@ -0,0 +1,54 @@
+package org.team708.robot.commands.toteElevator;
+
+import org.team708.robot.Robot;
+import org.team708.robot.util.Math708;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ToteElevatorUp extends Command {
+	
+	private final double threshold = 1;
+	private boolean atToteLimitMax;
+
+    public ToteElevatorUp() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    	requires(Robot.toteElevator);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	atToteLimitMax = (Robot.toteElevator.toteCount == Robot.toteElevator.TOTE_UPPER_LIMIT);
+    	
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+		if (!atToteLimitMax) {
+			Robot.toteElevator.raiseTote();
+		}
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	return Math708.isWithinThreshold(Robot.toteElevator.getEncoderDistance(), Robot.toteElevator.TOP_ENCODER_DISTANCE, threshold)
+    			|| atToteLimitMax;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	Robot.toteElevator.stopTote();
+    	Robot.toteElevator.setToteCount(Robot.toteElevator.getToteCount() + 1);
+    	Robot.toteElevator.resetEncoder();
+    	Robot.toteElevator.elevatorDown = false;
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems are scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}