package storm2014.commands;

import edu.wpi.first.wpilibj.command.Command;
import storm2014.Robot;
import storm2014.utilities.Debouncer;
/**
 * Sets the arm position to the desired mode.
 */
public class SetArmPosition extends Command {
    private final int _mode;
    private int _currMode;
    private final Debouncer _wait = new Debouncer(1);
    
    public SetArmPosition(int mode) {
        requires(Robot.intake);
        _mode = mode;
    }
    
    private void _nextMode() {
        _currMode = Math.min(_mode,Robot.intake.getMode()+1);
        Robot.intake.setMode(_mode);
        _wait.reset();
    }

    protected void initialize() {
        _nextMode();
    }

    protected void execute() {
        if(_wait.check(true)) {
            _nextMode();
        }
    }

    protected boolean isFinished() {
        return _currMode >= _mode;
    }

    protected void end() {}
    protected void interrupted() {}
}

@@ -282,24 +282,46 @@
  _currMode;%0A
+    private int _dir;%0A
     private 
@@ -358,17 +358,19 @@
 bouncer(
-1
+0.5
 );%0A    %0A
@@ -462,24 +462,152 @@
  mode;%0A    %7D
+%0A%0A    private double signum(double x) %7B%0A        return x %3E 0 ?  1 :%0A               x %3C 0 ? -1 :%0A                        0;%0A    %7D
 %0A    %0A    pr
@@ -639,17 +639,20 @@
         
-_
+int 
 currMode
@@ -658,23 +658,8 @@
 e = 
-Math.min(_mode,
 Robo
@@ -680,13 +680,95 @@
 de()
-+1);%0A
+;%0A        if(_dir*(_mode-currMode) %3E 0) %7B%0A            _currMode = currMode + _dir;%0A    
     
@@ -797,14 +797,28 @@
 de(_
-mode);
+currMode);%0A        %7D
 %0A   
@@ -874,24 +874,82 @@
 tialize() %7B%0A
+        _dir = (int)signum(_mode-Robot.intake.getMode());%0A
         _nex
@@ -1126,25 +1126,47 @@
 rn _
-currMode %3E= _mode
+dir*(_mode-Robot.intake.getMode()) %3C= 0
 ;%0A  

Fix SetArmPosition only liking positive