change position 5 to shoot in middle goal

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / AlignToScore.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
index 0d286e38c5770a546a1e261cd56d5cd18c1b431f..e5550cb18a1e8512a8be83734cc4e4a7000fb956 100755 (executable)
--- 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;
+
     }
   }