From: Meryem Esa <meresa14@gmail.com>
Date: Sun, 14 Feb 2016 01:59:40 +0000 (-0800)
Subject: change position 5 to shoot in middle goal
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=1598df61294ce7a8471984e92c8ac04cd4cb5755

change position 5 to shoot in middle goal
---

diff --git a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
index 0d286e38..e5550cb1 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
@@ -59,8 +59,10 @@ public class AlignToScore extends CommandGroup {
 
   // constants for position 5
   private final double POS5_DIST1 = 0;
-  private final double POS5_TURN1 = 0;
-  private final double POS5_DIST2 = 0;
+  private final double POS5_TURN1 = -90;
+  private final double POS5_DIST2 = 72.5;
+  private final double POS5_TURN2 = 90;
+  private final double POS5_DIST3 = 0;
 
   public double horizontalDistToGoal;
 
@@ -104,7 +106,10 @@ public class AlignToScore extends CommandGroup {
       addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED));
       addSequential(new TurnForAngle(POS5_TURN1));
       addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS5_TURN2));
+      addSequential(new DriveForDistance(POS5_DIST3, DEFAULT_SPEED));
       horizontalDistToGoal = 0;
+
     }
   }