change switch statements in Robot.java to if else chain

[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 bb3c3cb8c734edbf0c492effc43b7c1274536d62..e5550cb18a1e8512a8be83734cc4e4a7000fb956 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
@@ -29,34 +29,40 @@ 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
   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;
 
@@ -100,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;
+
     }
   }