add some of the constants for AlignToScore

[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 a7774a14ae3b57c93afdf62841227e47ce0c1262..0d286e38c5770a546a1e261cd56d5cd18c1b431f 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
@@ -29,28 +29,32 @@ public class AlignToScore extends CommandGroup {
 
   private final double DEFAULT_SPEED = 0.5;
 
+  // in inches
+  // assuming that positive angle means turning right
+  // and negative angle means turning left
+
   // constants for position 1: low bar
-  private final double POS1_DIST1 = 0;
-  private final double POS1_TURN1 = 0;
+  private final double POS1_DIST1 = 109;
+  private final double POS1_TURN1 = 60;
   private final double POS1_DIST2 = 0;
 
   // constants for position 2
-  private final double POS2_DIST1 = 0;
-  private final double POS2_TURN1 = 0;
+  private final double POS2_DIST1 = 140;
+  private final double POS2_TURN1 = 60;
   private final double POS2_DIST2 = 0;
 
   // constants for position 3
   private final double POS3_DIST1 = 0;
-  private final double POS3_TURN1 = 0;
-  private final double POS3_DIST2 = 0;
-  private final double POS3_TURN2 = 0;
+  private final double POS3_TURN1 = 90;
+  private final double POS3_DIST2 = 35.5;
+  private final double POS3_TURN2 = -90;
   private final double POS3_DIST3 = 0;
 
   // constants for position 4
   private final double POS4_DIST1 = 0;
-  private final double POS4_TURN1 = 0;
-  private final double POS4_DIST2 = 0;
-  private final double POS4_TURN2 = 0;
+  private final double POS4_TURN1 = -90;
+  private final double POS4_DIST2 = 18.5;
+  private final double POS4_TURN2 = 90;
   private final double POS4_DIST3 = 0;
 
   // constants for position 5
@@ -62,24 +66,22 @@ public class AlignToScore extends CommandGroup {
 
   public AlignToScore(int position) {
 
-    switch (position) {
+    if (position == 1) {
 
-    // position 1 is always the low bar
-    case 1:
+      // position 1 is always the low bar
 
       addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED));
       addSequential(new TurnForAngle(POS1_TURN1));
       addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED));
       horizontalDistToGoal = 0;
-
-    case 2:
+    } else if (position == 2) {
 
       addSequential(new DriveForDistance(POS2_DIST1, DEFAULT_SPEED));
       addSequential(new TurnForAngle(POS2_TURN1));
       addSequential(new DriveForDistance(POS2_DIST2, DEFAULT_SPEED));
       horizontalDistToGoal = 0;
 
-    case 3:
+    } else if (position == 3) {
 
       addSequential(new DriveForDistance(POS3_DIST1, DEFAULT_SPEED));
       addSequential(new TurnForAngle(POS3_TURN1));
@@ -88,7 +90,7 @@ public class AlignToScore extends CommandGroup {
       addSequential(new DriveForDistance(POS3_DIST3, DEFAULT_SPEED));
       horizontalDistToGoal = 0;
 
-    case 4:
+    } else if (position == 4) {
 
       addSequential(new DriveForDistance(POS4_DIST1, DEFAULT_SPEED));
       addSequential(new TurnForAngle(POS4_TURN1));
@@ -97,7 +99,7 @@ public class AlignToScore extends CommandGroup {
       addSequential(new DriveForDistance(POS4_DIST3, DEFAULT_SPEED));
       horizontalDistToGoal = 0;
 
-    case 5:
+    } else if (position == 5) {
 
       addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED));
       addSequential(new TurnForAngle(POS5_TURN1));