X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommandgroups%2FAutonMiddleGear.java;h=913bbd7e746c6b9b39d9dd4fc9be58a49b99564f;hp=bd718b341c6038983a6b9652ea7c64869e108eff;hb=c726d04ade36dead56cd4b2f1b91a341ce90bdb6;hpb=7cd65a82e59374ba6cae30c6f67e358717f7ee48

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java
index bd718b3..913bbd7 100644
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java
@@ -6,6 +6,7 @@ import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
 import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
 
 /**
  *
@@ -18,8 +19,8 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  * again to cross the baseline.
  */
 public class AutonMiddleGear extends CommandGroup {
-  private static final double DISTANCE_TO_PEG = 91.3;
-  private static final double DISTANCE_TO_BACK_OUT = 29.75;
+  private static final double DISTANCE_TO_PEG = 91.3 - 32;
+  private static final double DISTANCE_TO_BACK_OUT = -29.75;
   private static final double THIRD_DISTANCE_TO_TRAVEL = 70;
   private static final double DISTANCE_TO_BASELINE = 50.5;
 
@@ -37,6 +38,7 @@ public class AutonMiddleGear extends CommandGroup {
    */
   public AutonMiddleGear(Direction direction) {
     addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut));
+    addSequential(new WaitCommand(3));
     addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut));
     addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut));
     addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut));