fix stuff

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / commandgroups / AutonGearThenBaselinePegCloseToBoiler.java

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
index 6af22a452c7bfa5bc65162c5967467b0b4953776..2fcb8f3a8ee943365dd4d82d558564ba64824c7b 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
@@ -17,17 +17,15 @@ public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup
 
 {
 
-  private static final double ROBOT_LENGTH = 40.0;
-  private static final double ROBOT_WIDTH = 36.0;
+  private static final double ROBOT_LENGTH = 36.0;
+  private static final double ROBOT_WIDTH = 40.0;
 
-  private static final double DISTANCE_TO_PEG_EXTENDED_BOILER = 73.1657;
-  private static final double DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE = 114.3
-      + 17.625 - ((162 - (17.625 + 35.25) - 38.625) / Math.sqrt(3));
-  private static final double FIRST_LIMIT_LENGTH = 75.649;
-  private static final double SECOND_LIMIT_LENGTH = 162.8289106736;
+  private static final double STRAIGHT_DIST_FROM_BOILER = 76.8;
+  private static final double DISTANCE_TO_RETRIEVAL_PEG = 114.3 + 17.625
+      - ((162 - (17.625 + 35.25) - 38.625) / Math.sqrt(3));
   private static final double LENGTH_OF_PEG = 10.5;
   private static final double PEG_EXTENDED_BOILER = 117.518;
-  private static final double PEG_EXTENDED_RETRIEVAL_ZONE = (DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
+  private static final double PEG_EXTENDED_RETRIEVAL_ZONE = (DISTANCE_TO_RETRIEVAL_PEG
       * 2) / Math.sqrt(3);
   private static final double DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE = 255.6765151902;
   private static final double MOTOR_VALUE_TURN = 0.5;// this is probably not
@@ -38,64 +36,57 @@ public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup
                              // arena in inches (can change)
 
   /***
-   * 
-   * @param team
+   *
+   * @param side
    *          which side to start on: "BLUE" for blue side and "RED" for red
    *          side
-   * @param distanceFromBoilerCorner
+   * @param distanceFromcorner
    *          distance from the corner of the boiler
    */
-  public AutonGearThenBaselinePegCloseToBoiler(String team,
-      int distanceFromBoilerCorner) {
-
-    this.distanceFromCorner = distanceFromBoilerCorner;
+  public AutonGearThenBaselinePegCloseToBoiler(String side,
+      int distanceFromcorner) {
     requires(Robot.getDriveTrain());
-    if (distanceFromBoilerCorner >= 0
-        && distanceFromBoilerCorner < FIRST_LIMIT_LENGTH) {
-      // boiler peg path
 
-      // drive straight into extended line of peg
-      addSequential(
-          new DriveDistance(
-              ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
-                  + DISTANCE_TO_PEG_EXTENDED_BOILER - (ROBOT_LENGTH / 2.0),
-              MOTOR_VALUE_FORWARD));
+    if (side.equals("BOILER")) {
+      addSequential(new DriveDistance(
+          131.6 - (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2,
+          5));
+      // addSequential(new DriveDistance(
+      // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
+      // + STRAIGHT_DIST_FROM_BOILER - (ROBOT_LENGTH / 2.0),
+      // MOTOR_VALUE_FORWARD));
       // position robot for optimal gear placement
-      addSequential(new TurnForAngle(60, Direction.LEFT, MOTOR_VALUE_TURN));
+      addSequential(new TurnForAngle(60, Direction.RIGHT, 5));
       // put gear on peg
-      addSequential(new DriveDistance(PEG_EXTENDED_BOILER
-          - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2)
-              / Math.sqrt(3))
-          - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0),
-          MOTOR_VALUE_FORWARD));
+      addSequential(new DriveDistance(
+          2 * (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2 + 7,
+          5));
+      // addSequential(new DriveDistance(PEG_EXTENDED_BOILER
+      // - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2) / Math.sqrt(3))
+      // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD));
 
       // does not include drift distance
-    } else if (distanceFromBoilerCorner > SECOND_LIMIT_LENGTH
-        && distanceFromBoilerCorner <= DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE) // mirror
-                                                                                   // of
-                                                                                   // right
-                                                                                   // peg
-                                                                                   // path
-    {
-      this.distanceFromCorner = DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE
-          - distanceFromBoilerCorner;
-      // Turning DISTANCE_FROM_BOILER_WALL into starting point from left side by
-      // reversing it
-      addSequential(
-          new DriveDistance(
-              ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
-                  + DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
-                  - (ROBOT_LENGTH / 2.0),
-              MOTOR_VALUE_FORWARD));
-      addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
-      addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
-          - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2)
-          - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0),
-          MOTOR_VALUE_FORWARD));
+    } else if (side.equals("RETRIEVAL")) {
+      addSequential(new DriveDistance(
+          131.6 - (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2,
+          5));
+      addSequential(new TurnForAngle(60, Direction.LEFT, 5));
+      addSequential(new DriveDistance(
+          2 * (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
+              - ROBOT_LENGTH / 2,
+          5));
+      // addSequential(new DriveDistance(
+      // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
+      // + DISTANCE_TO_RETRIEVAL_PEG - (ROBOT_LENGTH / 2.0),
+      // MOTOR_VALUE_FORWARD));
+      // addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
+      // addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
+      // - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2)
+      // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD));
       // does not include drift distance
-    } else {
-      System.out.println(
-          "DO NOT START HERE!!! MOVE CLOSER TO A WALL OR LOSE ALL AUTON POINTS!!");
     }
   }
 }