Finished autoalignment code

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());
 
   }

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

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/KeyAllianceWallAlign.java b/src/org/usfirst/frc/team3501/robot/commandgroups/KeyAllianceWallAlign.java
index 508487cdf7ee9695b40a7c9c1003cfded5053c53..204f105e1145014a9955e5cbe5e563b0a44a2add 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/KeyAllianceWallAlign.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/KeyAllianceWallAlign.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,10 +10,18 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  *
  */
 public class KeyAllianceWallAlign extends CommandGroup {
+  private static final String TEAM = "RED"; // set to either RED or BLUE team
 
   public KeyAllianceWallAlign() {
-    addSequential(new TurnForAngle(0, LEFT, 0));
-    addSequential(new DriveDistance(0, 0));
+    if (TEAM.equals("RED")) {
+      addSequential(new TurnForAngle(45, Direction.RIGHT, 0));
+      addSequential(new DriveDistance(77.6, 0));
+      addSequential(new TurnForAngle(90, Direction.RIGHT, 0));
+    } else {
+      addSequential(new TurnForAngle(45, Direction.LEFT, 0));
+      addSequential(new DriveDistance(68.4, 0));
+      addSequential(new TurnForAngle(90, Direction.LEFT, 0));
+    }
     addSequential(new PrepareToShoot());
   }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/MiddlePegAlign.java b/src/org/usfirst/frc/team3501/robot/commandgroups/MiddlePegAlign.java
index 60061a8dc17dbc4e89a8baaaac9b97863a2a5fd5..df1c2e264ec537b2e5be037eb2bcf635341f07ad 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/MiddlePegAlign.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/MiddlePegAlign.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,11 +10,16 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  *
  */
 public class MiddlePegAlign extends CommandGroup {
+  private static final String TEAM = "RED"; // set to either RED or BLUE team
 
   public MiddlePegAlign() {
-    addSequential(new DriveDistance(0, 0));
-    addSequential(new TurnForAngle(0, LEFT, 0));
-    addSequential(new DriveDistance(0, 0));
+    addSequential(new DriveDistance(14.6, -0));
+    if (TEAM.equals("RED")) {
+      addSequential(new TurnForAngle(121.75, Direction.RIGHT, 0));
+    } else {
+      addSequential(new TurnForAngle(121.75, Direction.LEFT, 0));
+    }
+    addSequential(new DriveDistance(190.5, 0));
     addSequential(new PrepareToShoot());
   }
 }