Finished autoalignment code

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commandgroups / BaselineWallAlign.java

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/BaselineWallAlign.java b/src/org/usfirst/frc/team3501/robot/commandgroups/BaselineWallAlign.java
index b284b928ed1cf869e858f90de1a232a870d52a0c..39291ebb8997070079c3e665c4dab291bc0b453a 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/BaselineWallAlign.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/BaselineWallAlign.java
@@ -1,5 +1,6 @@
 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;
 
@@ -9,12 +10,20 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  *
  */
 public class BaselineWallAlign extends CommandGroup {
+  private static final String TEAM = "RED"; // set to either RED or BLUE team
 
   public BaselineWallAlign() {
-    addSequential(new DriveDistance(0, 0));
-    addSequential(new TurnForAngle(0, LEFT, 0));
-    addSequential(new DriveDistance(0, 0));
-    addSequential(new TurnForAngle(0, LEFT, 0));
+    addSequential(new DriveDistance(10, -0));
+
+    if (TEAM.equals("RED")) {
+      addSequential(new TurnForAngle(115.91, Direction.RIGHT, 0));
+      addSequential(new DriveDistance(126.35, 0));
+      addSequential(new TurnForAngle(90, Direction.LEFT, 0));
+    } else {
+      addSequential(new TurnForAngle(115.91, Direction.LEFT, 0));
+      addSequential(new DriveDistance(117.15, 0));
+      addSequential(new TurnForAngle(90, Direction.RIGHT, 0));
+    }
     addSequential(new PrepareToShoot());
 
   }