diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance1.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance1.java
new file mode 100644
index 0000000..2dc61df
--- /dev/null
+++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance1.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4079.RobotBuilderProject1.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc4079.RobotBuilderProject1.Robot;
+
+public class DriveDistance1 extends Command {
+  private double distance, threshold;
+  private double leftPos, rightPos;
+  private double error;
+  private double[] errors;
+  private int counter;
+  private double avg;
+  public DriveDistance1(double dist, double duration, double thresh) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(Robot.driveTrain);
+    distance = dist;
+    threshold = thresh;
+    error = 0;
+    errors = new double[10];
+    counter = 0;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    leftPos = Robot.driveTrain.getLeftDistance();
+    rightPos = Robot.driveTrain.getRightDistance();
+    Robot.driveTrain.configureForDrive();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.driveTrain.driveToTarget(leftPos+distance, rightPos-distance);
+    error = (leftPos + distance) - Robot.driveTrain.getLeftDistance();
+    error = errors[counter];
+    if (counter == errors.length - 1) {
+      counter = 0;
+    }
+    else counter++;
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    for (int i = 0; i < errors.length; i++) {
+      avg += errors[i];
+    }
+    avg /= errors.length;
+    return (avg < threshold);
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}
diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance2.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance2.java
new file mode 100644
index 0000000..d7f4f35
--- /dev/null
+++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance2.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4079.RobotBuilderProject1.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc4079.RobotBuilderProject1.Robot;
+
+public class DriveDistance2 extends Command {
+  private double distance, threshold;
+  private double leftPos, rightPos;
+  private double error;
+  private double[] errors;
+  private int counter;
+  public DriveDistance2(double dist, double duration, double thresh, int sampleNumber) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    requires(Robot.driveTrain);
+    distance = dist;
+    threshold = thresh;
+    error = 0;
+    errors = new double[sampleNumber];
+    counter = 0;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    leftPos = Robot.driveTrain.getLeftDistance();
+    rightPos = Robot.driveTrain.getRightDistance();
+    Robot.driveTrain.configureForDrive();
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.driveTrain.driveToTarget(leftPos+distance, rightPos-distance);
+    error = (leftPos + distance) - Robot.driveTrain.getLeftDistance();
+    error = errors[counter];
+    if (counter == errors.length - 1) {
+      counter = 0;
+    }
+    else counter++;
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    for (int i = 0; i < errors.length; i++) {
+      if (errors[i] > threshold) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}
diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java
index 67da712..8824277 100644
--- a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java
+++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java
@@ -52,7 +52,8 @@ protected void execute() {
     // Make this return true when this Command no longer needs to run execute()
     @Override
     protected boolean isFinished() {
-        return isTimedOut();
+        return Robot.elevator.limitSwitchUp.get();
+        
     }
 
     // Called once after isFinished returns true
diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java
index 3d93229..f47c31f 100644
--- a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java
+++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java
@@ -32,7 +32,7 @@ public class Elevator extends Subsystem {
 
     // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    // private final WPI_TalonSRX motor = RobotMap.elevatorMotor;
-    private final DigitalInput limitSwitchUp = RobotMap.elevatorLimitSwitchUp;
+    public final DigitalInput limitSwitchUp = RobotMap.elevatorLimitSwitchUp;
     private final DigitalInput limitSwitchDown = RobotMap.elevatorLimitSwitchDown;
 
     // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS