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 69da96f594dc2d6b61c860bd309fe1bca0d9e183..e5550cb18a1e8512a8be83734cc4e4a7000fb956 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/AlignToScore.java
@@ -9,8 +9,11 @@ import edu.wpi.first.wpilibj.command.CommandGroup;
  * robot is in, the robot will align with the goal. In the Software 2015-2016
  * Google folder is a picture explaining each of the cases.
  *
- * dependency on sensors: lidars, encoders, gyro dependency on subsystems:
- * drivetrain dependency on other commands: TurnForAngle(), DriveForDistance()
+ * dependency on sensors: lidars, encoders, gyro
+ *
+ * dependency on subsystems: drivetrain
+ *
+ * dependency on other commands: TurnForAngle(), DriveForDistance()
  *
  * pre-condition: robot is flush against a defense at the specified position in
  * the opponent's courtyard
@@ -26,57 +29,61 @@ 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;
 
   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));
@@ -85,7 +92,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));
@@ -94,16 +101,19 @@ 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));
       addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED));
+      addSequential(new TurnForAngle(POS5_TURN2));
+      addSequential(new DriveForDistance(POS5_DIST3, DEFAULT_SPEED));
       horizontalDistToGoal = 0;
+
     }
   }
 
-  public static double calculateAngleToTurn(int position,
+  public static double lidarCalculateAngleToTurn(int position,
       double horizontalDistToGoal) {
     double leftDist = Robot.driveTrain.getLeftLidarDistance();
     double rightDist = Robot.driveTrain.getRightLidarDistance();