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 bb3c3cb8c734edbf0c492effc43b7c1274536d62..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