X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommandgroups%2FAutonMiddleGear.java;h=4880baf28f19e1c136521117d1114e49513c15c8;hb=9ca89e45fa84b2ec93bc6adf60c7dde1e0a7defb;hp=bd718b341c6038983a6b9652ea7c64869e108eff;hpb=e6e87119464c6c595e53fa3aeb1213544cf1fd23;p=3501%2F2017steamworks

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java
index bd718b3..4880baf 100644
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonMiddleGear.java
@@ -1,9 +1,7 @@
 
 package org.usfirst.frc.team3501.robot.commandgroups;
 
-import org.usfirst.frc.team3501.robot.Constants.Direction;
 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;
 
@@ -18,8 +16,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;
 
@@ -35,19 +33,14 @@ public class AutonMiddleGear extends CommandGroup {
    *          direction to turn after placing gear on peg in order to cross the
    *          baseline. Only Direction.LEFT and Direction.RIGHT will be accepted
    */
-  public AutonMiddleGear(Direction direction) {
+  public AutonMiddleGear() {
     addSequential(new DriveDistance(DISTANCE_TO_PEG, maxTimeOut));
-    addSequential(new DriveDistance(DISTANCE_TO_BACK_OUT, maxTimeOut));
-    addSequential(new TurnForAngle(ANGLE_TO_TURN, direction, maxTimeOut));
-    addSequential(new DriveDistance(THIRD_DISTANCE_TO_TRAVEL, maxTimeOut));
-    addSequential(
-        new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut));
-    addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut));
-  }
-
-  private Direction oppositeOf(Direction direction) {
-    if (direction == Direction.LEFT)
-      return Direction.RIGHT;
-    return Direction.LEFT;
+    // 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));
+    // addSequential(
+    // new TurnForAngle(ANGLE_TO_TURN, oppositeOf(direction), maxTimeOut));
+    // addSequential(new DriveDistance(DISTANCE_TO_BASELINE, maxTimeOut));
   }
 }