From: Trevor <tr89on@gmail.com>
Date: Sun, 5 Feb 2017 00:32:57 +0000 (-0800)
Subject: Finished autoalignment code
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=refs%2Fheads%2FSamhitaTrevor%2FAutonomousShootAlign;p=3501%2F2017steamworks

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 b284b92..39291eb 100644
--- 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 c053f54..1e626eb 100644
--- 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 508487c..204f105 100644
--- 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 60061a8..df1c2e2 100644
--- 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());
   }
 }