Fixed setpoint getting stuck without stacker moving

src/org/usfirst/frc/team3316/robot/humanIO/Joysticks.java

@@ -14,6 +14,7 @@
 import org.usfirst.frc.team3316.robot.stacker.commands.HoldContainer;
 import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToFloor;
 import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToStep;
+import org.usfirst.frc.team3316.robot.stacker.commands.MoveStackerToTote;
 import org.usfirst.frc.team3316.robot.stacker.commands.OpenGripper;
 import org.usfirst.frc.team3316.robot.stacker.commands.ReleaseContainer;
 
@@ -66,7 +67,7 @@ public boolean get()
 
 	public JoystickButton moveStackerToFloor, 
 						  moveStackerToStep, 
-						  pickup; //pickup replaces moveStackerToTote
+						  moveStackerToTote; //pickup replaces moveStackerToTote
 
 	public JoystickButton holdContainer,
 						  releaseContainer;
@@ -113,9 +114,9 @@ public void initButtons ()
 					(int) config.get("BUTTON_MOVE_STACKER_TO_STEP"));
 			moveStackerToStep.whenPressed(new MoveStackerToStep());
 
-			pickup = new JoystickButton(joystickOperator, 
+			moveStackerToTote = new JoystickButton(joystickOperator, 
 					(int) config.get("BUTTON_MOVE_STACKER_TO_TOTE"));
-			pickup.whenPressed(new PickupSequence());
+			moveStackerToTote.whenPressed(new MoveStackerToTote());
 
 			holdContainer = new JoystickButton(joystickOperator,
 					(int) config.get("BUTTON_HOLD_CONTAINER"));

src/org/usfirst/frc/team3316/robot/humanIO/SDB.java

@@ -19,6 +19,7 @@
 import org.usfirst.frc.team3316.robot.config.Config.ConfigException;
 import org.usfirst.frc.team3316.robot.logger.DBugLogger;
 
+import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class SDB 
@@ -56,6 +57,15 @@ public void run ()
 			put ("Speed Right", Robot.chassis.getSpeedRight());
 			put ("Speed Center", Robot.chassis.getSpeedCenter());
 
+			if (Robot.rollerGripper.getGamePieceCollected() == null)
+			{
+				put ("Game Piece Collected", null);
+			}
+			else
+			{
+				put ("Game Piece Collected", Robot.rollerGripper.getGamePieceCollected().toString());
+			}
+
 			/*
 			 * Integrator testing
 			 * should be removed
@@ -79,6 +89,11 @@ private void put (String name, boolean b)
 	    {
 	    	SmartDashboard.putBoolean(name, b);
 	    }
+
+	    private void put (String name, String s)
+	    {
+	    	SmartDashboard.putString(name, s);
+	    }
 	}
 
 	DBugLogger logger = Robot.logger;

src/org/usfirst/frc/team3316/robot/sequences/PickupSequence.java

@@ -19,10 +19,10 @@ public PickupSequence()
     {
         addSequential(new MoveStackerToTote());
         addSequential(new CloseGripper());
-        addSequential(new WaitForGamePiece());
+        //addSequential(new WaitForGamePiece());
 
         //TODO: check if this should be changed to a sequence of floor --> step
-        moveToEndPosition = new MoveStackerToFloor();
+        //moveToEndPosition = new MoveStackerToFloor();
     }
 
     protected void end ()
@@ -31,6 +31,6 @@ protected void end ()
     	 * This is in end because we don't want this to execute if the sequence
     	 * was cancelled (and interrupted was called instead of end)
     	 */
-    	moveToEndPosition.start();
+    	//moveToEndPosition.start();
     }
 }

src/org/usfirst/frc/team3316/robot/stacker/commands/MoveStacker.java

@@ -23,7 +23,7 @@ public abstract class MoveStacker extends Command
     public MoveStacker()
     {
         requires(Robot.stacker);
-        setpoint = setSetpointState();
+        setpoint = this.setSetpointState();
     }
 
     protected void initialize() {}
@@ -38,6 +38,7 @@ protected void execute()
 
     protected boolean isFinished()
     {
+
     	if (Robot.stacker.getPosition() == setpoint)
     	{
     		return true;
@@ -62,6 +63,7 @@ private void set ()
     	{
     		setSuccessful = true;
     	}
+    	Robot.stacker.setSetpointState(setpoint);
     }
 
     protected abstract StackerPosition setSetpointState();

src/org/usfirst/frc/team3316/robot/subsystems/Stacker.java

@@ -22,7 +22,6 @@ public class Stacker extends Subsystem
 {	
 	private class StackerManager extends TimerTask
 	{
-
 		private StackerPosition currentState;
 		private StackerPosition setpointState;
 
@@ -67,6 +66,7 @@ else if (currentState == StackerPosition.Floor)
 					if (!Robot.stacker.getSwitchRatchetLeft() ||
 						!Robot.stacker.getSwitchRatchetRight())
 					{
+						setpointState = null;
 						return null;
 					}
 
@@ -99,6 +99,7 @@ else if (currentState == StackerPosition.Floor)
 					if (!Robot.stacker.getSwitchRatchetLeft() ||
 						!Robot.stacker.getSwitchRatchetRight())
 					{
+						setpointState = null;
 						return null;
 					}
 
@@ -146,6 +147,7 @@ else if (currentState == StackerPosition.Step)
 			return setpointState;
 		}
 
+
 		public void run ()
 		{
 			currentState = Robot.stacker.getPosition();
@@ -272,9 +274,19 @@ public boolean closeSolenoidGripper ()
     	return true;
     }
 
+    double[] prevHeights = new double[100];
+
     public double getHeight ()
     {
-    	return (1 / heightIR.getVoltage());
+    	double currentHeight = (1 / heightIR.getVoltage());
+    	double avg = currentHeight;
+    	for(int i=0; i<prevHeights.length-1; i++) {
+    		prevHeights[i] = prevHeights[i+1];
+    		avg += prevHeights[i];
+    	}
+    	prevHeights[prevHeights.length-1] = currentHeight;
+    	avg /= prevHeights.length;
+    	return avg;
     }
 
     public boolean getSwitchRatchetRight ()