diff --git a/src/main/java/org/frc5687/powerup/robot/Constants.java b/src/main/java/org/frc5687/powerup/robot/Constants.java
index 6b92e08c..7f769e9b 100644
--- a/src/main/java/org/frc5687/powerup/robot/Constants.java
+++ b/src/main/java/org/frc5687/powerup/robot/Constants.java
@@ -75,6 +75,11 @@ public class Lights {
         public static final double CLIMBER_HOLD = -0.89; // Party stobe
         public static final double CLIMBER_DOWN = PULSING_BLUE;
         public static final double TIME_WARNING = -.07;
+        public static final double TEST_RUNNING = 0;
+        public static final double TEST_WAITING = 0;
+
+        public static final double TEST_PASSED = SOLID_GREEN;
+        public static final double TEST_FAILED = SOLID_RED;
     }
     public class OI {
         public static final double RUMBLE_INTENSITY = 1;
@@ -392,6 +397,7 @@ public class HoldSpeeds {
 
     public class Arm {
         public static final double PDP_EXCESSIVE_CURRENT = 40.0;
+        public static final double MIN_AMPS = 3.5;
 
         public static final boolean MOTOR_INVERTED_PROTO = false;
         public static final boolean MOTOR_INVERTED_COMP = true;
diff --git a/src/main/java/org/frc5687/powerup/robot/OI.java b/src/main/java/org/frc5687/powerup/robot/OI.java
index 28ac7e35..1e6ecab9 100644
--- a/src/main/java/org/frc5687/powerup/robot/OI.java
+++ b/src/main/java/org/frc5687/powerup/robot/OI.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.frc5687.powerup.robot.commands.*;
+import org.frc5687.powerup.robot.commands.testing.*;
 import org.frc5687.powerup.robot.commands.actions.IntakeToDrive;
 import org.frc5687.powerup.robot.commands.actions.IntakeToFloor;
 import org.frc5687.powerup.robot.commands.actions.IntakeToScale;
@@ -44,7 +45,7 @@ public class ButtonNumbers {
 
     private JoystickButton carriageZeroEncoder;
 
-    //private JoystickButton servoToggle;
+    private JoystickButton selfTest;
 
     private JoystickButton driverArmUp;
     private JoystickButton driverArmDown;
@@ -70,14 +71,14 @@ public OI(Robot robot) {
         operatorArmToDriveButton = new JoystickButton(operatorGamepad, Gamepad.Buttons.X.getNumber());
         operatorArmToIntakeButton = new JoystickButton(operatorGamepad, Gamepad.Buttons.A.getNumber());
 
+
         climberUnwind = new JoystickButton(driverGamepad, Gamepad.Buttons.BACK.getNumber());
         climberWind = new JoystickButton(driverGamepad, Gamepad.Buttons.START.getNumber());
 
         intakeLeftIn = new JoystickButton(operatorGamepad, Gamepad.Buttons.LEFT_BUMPER.getNumber());
         intakeRightIn = new JoystickButton(operatorGamepad, Gamepad.Buttons.RIGHT_BUMPER.getNumber());
         carriageZeroEncoder = new JoystickButton(operatorGamepad, Gamepad.Buttons.BACK.getNumber());
-        //servoToggle = new JoystickButton(operatorGamepad, Gamepad.Buttons.START.getNumber());
-
+        selfTest = new JoystickButton(operatorGamepad, Gamepad.Buttons.START.getNumber());
         driverArmUp = new JoystickButton(driverGamepad, Gamepad.Buttons.RIGHT_BUMPER.getNumber());
         driverArmDown = new JoystickButton(driverGamepad, Gamepad.Buttons.RIGHT_STICK.getNumber());
 
@@ -91,6 +92,11 @@ public double getLeftSpeed() {
         return Helpers.applySensitivityFactor(speed, Constants.DriveTrain.SENSITIVITY);
     }
 
+
+    public boolean isStartPressed(){
+        return operatorGamepad.getRawButton(Gamepad.Buttons.START);
+    }
+
     public double getRightSpeed() {
         double speed = -getSpeedFromAxis(driverGamepad, ButtonNumbers.RIGHT_AXIS);
         speed = Helpers.applyDeadband(speed, Constants.DriveTrain.DEADBAND);
@@ -193,7 +199,7 @@ public void initializeButtons(Robot robot) {
         operatorArmToSwitchButton.whenPressed(new IntakeToSwitch(robot.getCarriage(), robot.getArm()));
         operatorArmToScaleButton.whenPressed(new IntakeToScale(robot.getCarriage(), robot.getArm()));
 
-        //servoToggle.whenPressed(new ServoToggle(robot.getIntake()));
+        selfTest.whenPressed(new SelfTestBootstrapper(robot));
 
         DriverStation.reportError("driverArmToIntakeButton " + (driverArmToIntakeButton ==null), false);
         DriverStation.reportError("driverArmToDriveButton " + (driverArmToDriveButton ==null), false);
diff --git a/src/main/java/org/frc5687/powerup/robot/Robot.java b/src/main/java/org/frc5687/powerup/robot/Robot.java
index c04adc0d..6afcad79 100644
--- a/src/main/java/org/frc5687/powerup/robot/Robot.java
+++ b/src/main/java/org/frc5687/powerup/robot/Robot.java
@@ -78,13 +78,13 @@ public void robotInit() {
         setPeriod(1 / Constants.CYCLES_PER_SECOND);
 
         try {
-            camera0 = CameraServer.getInstance().startAutomaticCapture(0);
+            // camera0 = CameraServer.getInstance().startAutomaticCapture(0);
         } catch (Exception e) {
             DriverStation.reportError(e.getMessage(), true);
         }
 
         try {
-            camera1 = CameraServer.getInstance().startAutomaticCapture(1);
+            // camera1 = CameraServer.getInstance().startAutomaticCapture(1);
         } catch (Exception e) {
             DriverStation.reportError(e.getMessage(), true);
         }
@@ -105,6 +105,7 @@ public void robotInit() {
     public Lights getLights() { return _lights; }
     public JeVoisProxy getJeVoisProxy() { return jeVoisProxy; }
     public LidarProxy getLidarProxy() { return lidarProxy; }
+    public OI getOI() { return oi; }
 
 
 
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/ArmMotorTest.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/ArmMotorTest.java
new file mode 100644
index 00000000..f8eef95b
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/ArmMotorTest.java
@@ -0,0 +1,83 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.frc5687.powerup.robot.Constants;
+import org.frc5687.powerup.robot.RobotMap;
+import org.frc5687.powerup.robot.subsystems.Arm;
+import org.frc5687.powerup.robot.subsystems.Carriage;
+import org.frc5687.powerup.robot.utils.PDP;
+import org.frc5687.powerup.robot.commands.MoveCarriageToSetpointPID;
+import edu.wpi.first.wpilibj.DriverStation;
+
+import static java.lang.Math.abs;
+
+
+public class ArmMotorTest extends Command {
+    private Arm arm;
+    private Carriage carriage;
+    private PDP pdp;
+    private long upMillis;
+    private long downMillis;
+    private long stopMillis;
+    private boolean finished;
+    private double amps;
+    private double bottomAngle;
+    private double topAngle;
+    private double startAngle;
+
+    public ArmMotorTest(Arm arm, Carriage carriage){
+     requires(arm);
+     requires(carriage);
+
+     this.arm = arm;
+     this.carriage = carriage;
+    }
+
+    @Override
+    protected void initialize() {
+        finished = false;
+        upMillis = System.currentTimeMillis() + 125;
+        stopMillis = System.currentTimeMillis() + 250;
+        downMillis = System.currentTimeMillis() + 375;
+        amps = 0;
+        topAngle = 0;
+        startAngle = arm.getAngle();
+    }
+
+    @Override
+    protected void execute() {
+        if (System.currentTimeMillis()< upMillis){
+            arm.drive(0.75);
+            amps = Math.max(amps, pdp.getCurrent(RobotMap.PDP.ARM_SP));
+            topAngle = Math.max(topAngle, arm.getAngle());
+
+        }
+        else if (System.currentTimeMillis()< stopMillis){
+            arm.drive(0);
+            bottomAngle = topAngle;
+        }
+        else {
+            arm.drive(-0.75);
+            amps = Math.max(amps, pdp.getCurrent(RobotMap.PDP.ARM_SP));
+            bottomAngle = Math.min(bottomAngle, arm.getAngle());
+        }
+    }
+
+    @Override
+    protected void end() {
+        if (amps < Constants.Arm.MIN_AMPS) {
+            DriverStation.reportError(amps + " amps drawn", false);
+        }
+        if (abs(startAngle - topAngle) < 20) {
+            DriverStation.reportError("angle change " + (startAngle - topAngle), false);
+        }
+        if (abs(topAngle - bottomAngle) > 20) {
+            DriverStation.reportError("angle change " + (bottomAngle - topAngle), false);
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return System.currentTimeMillis() > downMillis;
+    }
+}
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/ConfirmTest.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/ConfirmTest.java
new file mode 100644
index 00000000..b85364cf
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/ConfirmTest.java
@@ -0,0 +1,61 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import org.frc5687.powerup.robot.OI;
+/**
+ * Command to prompt the operator to confirm a result using the gamepad buttons.
+ */
+public class ConfirmTest extends Command {
+
+    private String _message;
+    private String _success;
+    private String _failure;
+    private boolean _clear;
+    private String _key;
+    private OI _oi;
+
+    public ConfirmTest(OI oi, String message, String success, String failure) {
+        _oi = oi;
+        _message = message;
+        _success = success;
+        _failure = failure;
+        _clear = false;
+    }
+
+    public ConfirmTest(OI oi,  String message, String key) {
+        _oi = oi;
+        _message = message;
+        _key = key;
+        _clear = false;
+    }
+
+    @Override
+    protected void initialize() {
+        DriverStation.reportError(_message, false);
+        SmartDashboard.putString("SelfTest/Message", _message);
+    }
+
+    @Override
+    protected void execute() {
+        if (!_clear) {
+            _clear = !_oi.isStartPressed();
+        }
+    }
+
+    @Override
+    protected boolean isFinished() {
+        if (_clear) {
+            if (_oi.isStartPressed()) {
+                return true;
+            }
+        }
+        return false;
+    }
+
+    @Override
+    protected void end() {
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/FullSelfTest.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/FullSelfTest.java
new file mode 100644
index 00000000..d609c792
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/FullSelfTest.java
@@ -0,0 +1,30 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import org.frc5687.powerup.robot.Robot;
+import org.frc5687.powerup.robot.subsystems.Intake;
+
+
+public class FullSelfTest extends CommandGroup {
+
+    public FullSelfTest(Robot robot) {
+
+
+        addSequential(new ConfirmTest(robot.getOI(), "Please ensure the robot is drydocked and press Start to continue.", "Test started.", "Test aborted."));
+        addSequential(new TestDriveTrain(robot.getDriveTrain(), robot.getPDP(), robot.getLights()));
+
+        addSequential(new ConfirmTest(robot.getOI(), "Please be sure that the carriage is clear and press Start to continue.", "Test started.", "Test aborted."));
+        addSequential(new TestCarriage(robot.getCarriage(), robot.getPDP(), robot.getLights()));
+
+        addSequential(new ConfirmTest(robot.getOI(), "Please be sure that the arm is clear and press Start to continue.", "Test started.", "Test aborted."));
+
+        addSequential(new ConfirmTest(robot.getOI(), "Please be sure that the intake is clear and press Start to continue.", "Test started.", "Test aborted."));
+        addSequential(new TestIntake(robot.getIntake(), robot.getPDP(), robot.getLights()));
+
+        addSequential(new ConfirmTest(robot.getOI(), "Tests complete.", "Test started.", "Test aborted."));
+
+
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/SelfTestBootstrapper.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/SelfTestBootstrapper.java
new file mode 100644
index 00000000..cfa9c810
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/SelfTestBootstrapper.java
@@ -0,0 +1,35 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.DriverStation;
+
+import org.frc5687.powerup.robot.OI;
+import org.frc5687.powerup.robot.Robot;
+import org.frc5687.powerup.robot.subsystems.Intake;
+
+public class SelfTestBootstrapper extends Command {
+    private Robot _robot;
+    private boolean done = false;
+
+    public SelfTestBootstrapper(Robot robot) {
+    _robot = robot;
+        }
+
+    @Override
+    protected void execute(){
+
+        if(!done && !DriverStation.getInstance().isFMSAttached()){
+            Scheduler.getInstance().add(new FullSelfTest(_robot));
+            done = true;
+        }
+
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return done;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestArm.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestArm.java
new file mode 100644
index 00000000..c17dd6f4
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestArm.java
@@ -0,0 +1,21 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.frc5687.powerup.robot.Constants;
+import org.frc5687.powerup.robot.Robot;
+import org.frc5687.powerup.robot.subsystems.Arm;
+import org.frc5687.powerup.robot.subsystems.Carriage;
+import org.frc5687.powerup.robot.commands.testing.ArmMotorTest;
+import org.frc5687.powerup.robot.commands.MoveArmToSetpointPID;
+
+public class TestArm extends CommandGroup{
+    public TestArm(Robot robot){
+        addSequential(new ArmMotorTest(robot.getArm(), robot.getCarriage()));
+        addSequential(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Encoder.ENCODER_START));
+        addSequential(new MoveArmToSetpointPID(robot.getArm(), Constants.Arm.Encoder.ENCODER_TOP));
+
+    }
+
+
+}
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriage.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriage.java
new file mode 100644
index 00000000..36b794d1
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriage.java
@@ -0,0 +1,161 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.frc5687.powerup.robot.Constants;
+import org.frc5687.powerup.robot.OI;
+import org.frc5687.powerup.robot.Robot;
+import org.frc5687.powerup.robot.RobotMap;
+import org.frc5687.powerup.robot.commands.auto.AutoIntake;
+import org.frc5687.powerup.robot.commands.auto.AutoZeroCarriage;
+import org.frc5687.powerup.robot.commands.auto.paths.FarLeftToRightScaleDeadPartOne;
+import org.frc5687.powerup.robot.subsystems.Carriage;
+import org.frc5687.powerup.robot.subsystems.Lights;
+import org.frc5687.powerup.robot.utils.PDP;
+
+public class TestCarriage extends Command {
+    private Carriage _carriage;
+    private PDP _pdp;
+    private Lights _lights;
+
+    private State _state = State.ZERO;
+    private long _endMillis;
+
+    private static long _maxZeroMillis = 1500;
+    private static long _maxBottomMillis = 2000;
+    private static long _maxMiddleMillis = 1500;
+
+    private final double _targetLiftingAmps = 3;
+
+    private boolean _pass = true;
+
+    private double _maxAmps = 0;
+
+    public TestCarriage(Carriage carriage, PDP pdp, Lights lights){
+        // Run the carriage up to the hall efect
+        requires(carriage);
+
+        _carriage = carriage;
+        _pdp = pdp;
+        _lights = lights;
+
+    }
+
+    @Override
+    protected void initialize() {
+        DriverStation.reportError("Starting carriage test", false);
+        _state = State.ZERO;
+        _maxAmps = 0;
+        _endMillis = System.currentTimeMillis() + _maxZeroMillis;
+        _carriage.zeroEncoder();
+    }
+
+    protected void execute() {
+
+        switch (_state) {
+            case ZERO:
+                _lights.setBoth(Constants.Lights.TEST_RUNNING, Constants.Lights.TEST_RUNNING);
+                _carriage.drive(Constants.Carriage.MAXIMUM_SPEED);
+                _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.CARRIAGE_SP));
+                if (_carriage.isAtTop()) {
+                    // At the top...
+                    _carriage.zeroEncoder();
+                    _carriage.drive(0);
+                    DriverStation.reportError("Carriage hall-effect detected.  Zeroing carriage.", false);
+                    _state = State.ZERODONE;
+                } else if (System.currentTimeMillis() > _endMillis) {
+                    DriverStation.reportError("Carriage hall-effect not detected within " + _maxZeroMillis + "ms.", false);
+                    _carriage.drive(0);
+                    _pass = false;
+                    _state = State.ZERODONE;
+                }
+                break;
+            case ZERODONE:
+                if (_maxAmps < _targetLiftingAmps) {
+                    _pass = false;
+                    SmartDashboard.putBoolean("SelfTest/Carriage/Amps/Passed", false);
+                    DriverStation.reportError("Target amperage not reached on carriage.  Expected " + _targetLiftingAmps + " but measured " + _maxAmps + ".", false);
+                } else {
+                    SmartDashboard.putBoolean("SelfTest/Carriage/Amps/Passed", true);
+                    DriverStation.reportError("Amp draw passed on carriage.  Expected " + _targetLiftingAmps + " and measured  " + _maxAmps + ".", false);
+                }
+                _state = _pass ? State.BOTTOM : State.DONE;
+                break;
+            case BOTTOM:
+                _carriage.setSetpoint(Constants.Carriage.ENCODER_BOTTOM_PROTO);
+                _carriage.enable();
+                _endMillis = System.currentTimeMillis() + _maxBottomMillis;
+                _state = State.BOTTOMWAIT;
+                break;
+            case BOTTOMWAIT:
+                if (_carriage.onTarget()) {
+                    DriverStation.reportError("Carriage reached bottom.", false);
+                    _carriage.disable();
+                    _carriage.drive(0);
+                    _pass = false;
+                    _state = State.MIDDLE;
+                } else if (System.currentTimeMillis() > _endMillis) {
+                    DriverStation.reportError("Carriage didn't reach bottom within " + _maxBottomMillis + "ms.", false);
+                    _carriage.disable();
+                    _carriage.drive(0);
+                    _pass = false;
+                    _state = State.DONE;
+                }
+                break;
+            case MIDDLE:
+                _carriage.setSetpoint(Constants.Carriage.ENCODER_MIDDLE_PROTO);
+                _carriage.enable();
+                _endMillis = System.currentTimeMillis() + _maxMiddleMillis;
+                _state = State.MIDDLEWAIT;
+                break;
+            case MIDDLEWAIT:
+                if (_carriage.onTarget()) {
+                    DriverStation.reportError("Carriage reached middle.", false);
+                    _carriage.disable();
+                    _carriage.drive(0);
+                    _pass = false;
+                    _state = State.WAIT;
+                } else if (System.currentTimeMillis() > _endMillis) {
+                    DriverStation.reportError("Carriage didn't reach middle within " + _maxMiddleMillis + "ms.", false);
+                    _carriage.disable();
+                    _carriage.drive(0);
+                    _pass = false;
+                    _state = State.DONE;
+                }
+                break;
+            case WAIT:
+                _carriage.disable();
+                _carriage.drive(0);
+                if (System.currentTimeMillis() > _endMillis) {
+                    _state = State.DONE;
+                }
+                break;
+        }
+
+
+    }
+
+    @Override
+    protected void end() {
+        _carriage.drive(0, true);
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return _state == State.DONE;
+    }
+
+    private enum State {
+        ZERO,
+        ZERODONE,
+        BOTTOM,
+        BOTTOMWAIT,
+        MIDDLE,
+        MIDDLEWAIT,
+        WAIT,
+        DONE
+    }
+
+}
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriageMotor.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriageMotor.java
new file mode 100644
index 00000000..ce40358d
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriageMotor.java
@@ -0,0 +1,37 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.frc5687.powerup.robot.subsystems.Carriage;
+
+
+public class TestCarriageMotor extends Command {
+    private Carriage carriage;
+    private long upMillis;
+    public long downMillis;
+    public TestCarriageMotor(Carriage carriage) {
+        requires(carriage);
+    }
+
+    @Override
+    protected void initialize() {
+        upMillis = System.currentTimeMillis() + 250;
+        downMillis = System.currentTimeMillis() + 500;
+    }
+
+    @Override
+    protected void execute() {
+        if(System.currentTimeMillis() < upMillis){
+            carriage.drive(1);
+        }
+        else {
+            carriage.drive(-1);
+        }
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return System.currentTimeMillis()> downMillis;
+    }
+}
+
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/TestCarriageSpeed.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriageSpeed.java
similarity index 94%
rename from src/main/java/org/frc5687/powerup/robot/commands/TestCarriageSpeed.java
rename to src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriageSpeed.java
index b103daa8..476a008f 100644
--- a/src/main/java/org/frc5687/powerup/robot/commands/TestCarriageSpeed.java
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestCarriageSpeed.java
@@ -1,4 +1,4 @@
-package org.frc5687.powerup.robot.commands;
+package org.frc5687.powerup.robot.commands.testing;
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.command.Command;
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestDriveTrain.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestDriveTrain.java
new file mode 100644
index 00000000..f0f332a4
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestDriveTrain.java
@@ -0,0 +1,174 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.frc5687.powerup.robot.Constants;
+import org.frc5687.powerup.robot.RobotMap;
+import org.frc5687.powerup.robot.subsystems.DriveTrain;
+import org.frc5687.powerup.robot.subsystems.Lights;
+import org.frc5687.powerup.robot.utils.PDP;
+
+/**
+ * Created by Ben Bernard on 4/20/2018.
+ */
+public class TestDriveTrain extends Command {
+    private static double kTOLERANCE = 0.25;
+    private double _runSpeed;
+    private int _runMillis;
+    private double _targetAmps;
+    private int _targetTicks;
+
+    private State _state = State.RIGHTFRONT;
+    private long _endMillis;
+
+    private double _maxAmps = 0;
+
+    private DriveTrain _driveTrain;
+    private PDP _pdp;
+    private Lights _lights;
+
+    boolean pass=true;
+
+
+    public TestDriveTrain(DriveTrain driveTrain, PDP pdp, Lights lights) {
+        requires(driveTrain);
+
+        _driveTrain = driveTrain;
+        _pdp = pdp;
+        _lights = lights;
+
+        _runSpeed = 1.0;
+        _runMillis = 500;
+        _targetAmps = 3;
+        _targetTicks = 16000;
+
+
+    }
+
+    @Override
+    protected void initialize() {
+        DriverStation.reportError("Starting drivetrain test", false);
+        _state = State.RIGHTFRONT;
+        _maxAmps = 0;
+        _endMillis = System.currentTimeMillis() + _runMillis;
+        _driveTrain.resetDriveEncoders();
+    }
+
+
+    protected void execute() {
+
+        switch (_state) {
+            case RIGHTFRONT:
+                _lights.setBoth(Constants.Lights.TEST_RUNNING, Constants.Lights.TEST_RUNNING);
+                _driveTrain.setPower(0, _runSpeed);
+                _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.RIGHT_FRONT_SRX));
+                if (System.currentTimeMillis() > _endMillis) {
+                    _driveTrain.setPower(0, 0);
+                    _state = State.RIGHTFRONTDONE;
+                }
+                break;
+            case RIGHTFRONTDONE:
+                _driveTrain.setPower(0, 0);
+                report("right front", _driveTrain.getRightTicks());
+                _state = State.RIGHTREAR;
+                break;
+            case RIGHTREAR:
+                _driveTrain.setPower(0, _runSpeed);
+                _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.RIGHT_REAR_SPX));
+                if (System.currentTimeMillis() > _endMillis) {
+                    _driveTrain.setPower(0, 0);
+                    _state = State.RIGHTREARDONE;
+                }
+                break;
+            case RIGHTREARDONE:
+                _driveTrain.setPower(0, 0);
+                report("right rear", _driveTrain.getRightTicks());
+                _state = State.LEFTFRONT;
+                break;
+
+            case LEFTFRONT:
+                _driveTrain.setPower(_runSpeed, 0);
+                _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.LEFT_FRONT_SRX));
+                if (System.currentTimeMillis() > _endMillis) {
+                    _driveTrain.setPower(_runSpeed, 0);
+                    _state = State.LEFTFRONTDONE;
+                }
+                break;
+            case LEFTFRONTDONE:
+                _driveTrain.setPower(0, 0);
+                report("left front", _driveTrain.getLeftTicks());
+                _state = State.LEFTREAR;
+                break;
+            case LEFTREAR:
+                _driveTrain.setPower(_runSpeed, 0);
+                _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.LEFT_REAR_SPX));
+                if (System.currentTimeMillis() > _endMillis) {
+                    _driveTrain.setPower(0, 0);
+                    _state = State.LEFTREARDONE;
+                }
+                break;
+            case LEFTREARDONE:
+                _driveTrain.setPower(0, 0);
+                report("left rear", _driveTrain.getLeftTicks());
+                _state = State.WAIT;
+                break;
+            case WAIT:
+                _driveTrain.setPower(0, 0);
+                if (System.currentTimeMillis() > _endMillis) {
+                    _state = State.DONE;
+                }
+                break;
+        }
+
+    }
+    private void report(String side, long ticks) {
+        if (_maxAmps < _targetAmps) {
+            pass = false;
+            SmartDashboard.putBoolean("SelfTest/Drivetrain/" + side + "/Amps/Passed", false);
+            DriverStation.reportError("Target amperage not reached on " + side  + ".  Expected " + _targetAmps + " but measured " + _maxAmps + ".", false);
+        } else {
+            SmartDashboard.putBoolean("SelfTest/Drivetrain/" + side + "/Amps/Passed", true);
+            DriverStation.reportError("Amp draw passed on " + side  + ".  Expected " + _targetAmps + " and measured  " + _maxAmps + ".", false);
+        }
+        SmartDashboard.putNumber("SelfTest/Drivetrain/" + side + "/Amps/Measured", _maxAmps);
+        if (ticks < _targetTicks) {
+            pass = false;
+            SmartDashboard.putBoolean("SelfTest/Drivetrain/" + side + "/Ticks/Passed", false);
+            DriverStation.reportError("Target ticks not reached on " + side + ".  Expected " + _targetTicks + " but measured " + ticks + ".", false);
+        } else {
+            SmartDashboard.putBoolean("SelfTest/Drivetrain/" + side + "/Ticks/Passed", true);
+            DriverStation.reportError("Target ticks reached on " + side + ".  Expected " + _targetTicks + " and measured " + ticks + ".", false);
+        }
+        _lights.setBoth(pass ? Constants.Lights.TEST_PASSED : Constants.Lights.TEST_FAILED);
+
+        SmartDashboard.putNumber("SelfTest/Drivetrain/" + side + "/Ticks/Measured", ticks);
+        _driveTrain.resetDriveEncoders();
+        _maxAmps = 0;
+        _endMillis = System.currentTimeMillis() + _runMillis;
+    }
+    @Override
+    protected void end() {
+        _driveTrain.setPower(0, 0);
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return _state == State.DONE;
+    }
+
+
+        private enum State {
+            RIGHTFRONT,
+            RIGHTFRONTDONE,
+            RIGHTREAR,
+            RIGHTREARDONE,
+            LEFTFRONT,
+            LEFTFRONTDONE,
+            LEFTREAR,
+            LEFTREARDONE,
+            WAIT,
+            DONE
+        }
+
+}
diff --git a/src/main/java/org/frc5687/powerup/robot/commands/testing/TestIntake.java b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestIntake.java
new file mode 100644
index 00000000..e5ed1c5d
--- /dev/null
+++ b/src/main/java/org/frc5687/powerup/robot/commands/testing/TestIntake.java
@@ -0,0 +1,132 @@
+package org.frc5687.powerup.robot.commands.testing;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.frc5687.powerup.robot.Constants;
+import org.frc5687.powerup.robot.RobotMap;
+import org.frc5687.powerup.robot.commands.auto.paths.FarLeftToRightScaleDeadPartOne;
+import org.frc5687.powerup.robot.subsystems.Carriage;
+import org.frc5687.powerup.robot.subsystems.Intake;
+import org.frc5687.powerup.robot.subsystems.Lights;
+import org.frc5687.powerup.robot.utils.PDP;
+
+public class TestIntake extends Command {
+    private Intake _intake;
+    private PDP _pdp;
+    private Lights _lights;
+
+    private State _state = State.LEFT;
+    private long _endMillis;
+
+    private static long _runMillis = 500;
+
+    private final double _targetAmps = 3;
+
+    private boolean _pass = true;
+
+    private double _maxAmps = 0;
+
+    public TestIntake(Intake intake, PDP pdp, Lights lights){
+        // Run the carriage up to the hall efect
+        requires(intake);
+
+        _intake = intake;
+        _pdp = pdp;
+        _lights = lights;
+
+    }
+
+    @Override
+    protected void initialize() {
+        DriverStation.reportError("Starting intake test", false);
+        _state = State.LEFT;
+        _maxAmps = 0;
+        _endMillis = System.currentTimeMillis() + _runMillis;
+    }
+
+    protected void execute() {
+
+        switch (_state) {
+            case LEFT:
+                _lights.setBoth(Constants.Lights.TEST_RUNNING, Constants.Lights.TEST_RUNNING);
+                _intake.drive(Constants.Intake.INTAKE_SPEED, 0);
+                _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.INTAKE_LEFT_SP));
+                if (System.currentTimeMillis() > _endMillis) {
+                    _intake.drive(0, 0);
+                    _state = State.LEFTDONE;
+                }
+                break;
+            case LEFTDONE:
+                if (_maxAmps < _targetAmps) {
+                    _pass = false;
+                    SmartDashboard.putBoolean("SelfTest/Intake/Left/Amps/Passed", false);
+                    DriverStation.reportError("Target amperage not reached on left intake.  Expected " + _targetAmps + " but measured " + _maxAmps + ".", false);
+                } else {
+                    _pass = true;
+                    SmartDashboard.putBoolean("SelfTest/Intake/Left/Amps/Passed", true);
+                    DriverStation.reportError("Amp draw passed on left intake.  Expected " + _targetAmps + " and measured  " + _maxAmps + ".", false);
+                }
+                _maxAmps = 0;
+                _endMillis = System.currentTimeMillis() + _runMillis;
+                _state = State.RIGHT;
+                break;
+            case RIGHT:
+                _lights.setBoth(Constants.Lights.TEST_RUNNING, Constants.Lights.TEST_RUNNING);
+                _intake.drive(0, Constants.Intake.INTAKE_SPEED);
+                _maxAmps = Math.max(_maxAmps, _pdp.getCurrent(RobotMap.PDP.INTAKE_RIGHT_SP_PROTO));
+                if (System.currentTimeMillis() > _endMillis) {
+                    _intake.drive(0, 0);
+                    _state = State.RIGHTDONE;
+                }
+                break;
+            case RIGHTDONE:
+                if (_maxAmps < _targetAmps) {
+                    _pass = false;
+                    SmartDashboard.putBoolean("SelfTest/Intake/Right/Amps/Passed", false);
+                    DriverStation.reportError("Target amperage not reached on right intake.  Expected " + _targetAmps + " but measured " + _maxAmps + ".", false);
+                } else {
+                    _pass = true;
+                    SmartDashboard.putBoolean("SelfTest/Intake/Right/Amps/Passed", true);
+                    DriverStation.reportError("Amp draw passed on right intake.  Expected " + _targetAmps + " and measured  " + _maxAmps + ".", false);
+                }
+                _endMillis = System.currentTimeMillis() + _runMillis;
+                _state = State.WAIT;
+                break;
+            case WAIT:
+                if (!_intake.cubeIsSecured()) {
+                    SmartDashboard.putBoolean("SelfTest/Intake/CubeIR", false);
+                    DriverStation.reportError("Cube not detected .", false);
+                } else {
+                    SmartDashboard.putBoolean("SelfTest/Intake/CubeIR", true);
+                    DriverStation.reportError("Cube detected .", false);
+                }
+                _intake.drive(0,0 );
+                if (System.currentTimeMillis() > _endMillis) {
+                    _state = State.DONE;
+                }
+                break;
+        }
+    }
+
+    @Override
+    protected void end() {
+        _intake.drive(0, 0);
+    }
+
+    @Override
+    protected boolean isFinished() {
+        return _state == State.DONE;
+    }
+
+    private enum State {
+        LEFT,
+        LEFTDONE,
+        RIGHT,
+        RIGHTDONE,
+        WAIT,
+        DONE
+    }
+
+}
diff --git a/src/main/java/org/frc5687/powerup/robot/subsystems/Lights.java b/src/main/java/org/frc5687/powerup/robot/subsystems/Lights.java
index e9f152f3..b4a46101 100644
--- a/src/main/java/org/frc5687/powerup/robot/subsystems/Lights.java
+++ b/src/main/java/org/frc5687/powerup/robot/subsystems/Lights.java
@@ -50,6 +50,11 @@ public void setBoth(double leftVal, double rightVal) {
         setRight(rightVal);
     }
 
+    public void setBoth(double val) {
+        setLeft(val);
+        setRight(val);
+    }
+
     public void setColors() {
         Intake intake = _robot.getIntake();
         Climber climber = _robot.getClimber();