FINALLY Finished Auton Command Group

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 5fbfe4043d6b58bca46b2daffc35783c2ae74251..58f5d0b19352bfdf1a6d49d11e3848e85febcd03 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -42,6 +42,7 @@ public class Robot extends IterativeRobot {
   @Override
   public void autonomousInit() {
     Scheduler.getInstance().add(new TimeDrive(1.5, 0.4));
+
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
index 5466e92eb3f12bcb76f7cb148e3fa5bc26707876..b674b35da8cd62c57fbc4b513dbe89cf4c80f218 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
@@ -8,25 +8,69 @@ import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 /**
- *
+ * this is an auton strategy to cross the baseline and put a gear on a peg
+ * requires predetermined starting point anywhere except between 109.1767 inches
+ * and 205.7286 inches from the right side of the arena this program chooses
+ * which peg to go for based on the starting point
  */
-public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup {
+public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup
 
-  private static final int MOTOR_VALUE_TURN = 60;
-  private static final int MOTOR_VALUE_FORWARD = 60;
-  private static final double DISTANCE_FROM_BOILER_WALL = 111.5676;
+{
 
-  public AutonGearThenBaselinePegCloseToBoiler() {
+  private static final double LENGTH_OF_PEG = 10.5;
+  private static final double PEG_EXTENDED = 136.5664;
+  private static final int WIDTH_OF_ARENA = 324;
+  private static final double MOTOR_VALUE_TURN = 0.5;// this is probably not
+                                                     // correct
+  private static final double MOTOR_VALUE_FORWARD = 0.5;// random
+  double DISTANCE_FROM_BOILER_WALL = 105.7126; // This is the starting point
+                                               // from the right side of the
+                                               // arena in inches
 
+  public AutonGearThenBaselinePegCloseToBoiler() {
     requires(Robot.getDriveTrain());
-    addSequential(
-        new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 52.4614,
-            MOTOR_VALUE_FORWARD));
-    addSequential(new TurnForAngle(60, Direction.LEFT, MOTOR_VALUE_TURN));
-    addSequential(new DriveDistance(
-        143.3272 - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - 10.5,
-        MOTOR_VALUE_FORWARD));
-    // does not include drift distance
-
+    if (DISTANCE_FROM_BOILER_WALL > 0 && DISTANCE_FROM_BOILER_WALL < 109.1767) // right
+                                                                               // peg
+                                                                               // path
+    {
+      addSequential(
+          new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 50.2168,
+              MOTOR_VALUE_FORWARD)); // drive straight into extended line of peg
+      addSequential(new TurnForAngle(60, Direction.LEFT, MOTOR_VALUE_TURN));// position
+                                                                            // robot
+                                                                            // for
+                                                                            // optimal
+                                                                            // gear
+                                                                            // placement
+      addSequential(new DriveDistance(PEG_EXTENDED
+          - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - LENGTH_OF_PEG,
+          MOTOR_VALUE_FORWARD));// put gear on peg
+      // does not include drift distance
+    } else if (DISTANCE_FROM_BOILER_WALL > 205.7286
+        && DISTANCE_FROM_BOILER_WALL < 324) // mirror of right peg path {
+    {
+      DISTANCE_FROM_BOILER_WALL = WIDTH_OF_ARENA - DISTANCE_FROM_BOILER_WALL; // Turning
+                                                                              // DISTANCE_FROM_BOILER_WALL
+                                                                              // into
+                                                                              // starting
+                                                                              // point
+                                                                              // from
+                                                                              // left
+                                                                              // side
+                                                                              // by
+                                                                              // reversing
+                                                                              // it
+      addSequential(
+          new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 50.2168,
+              MOTOR_VALUE_FORWARD));
+      addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
+      addSequential(new DriveDistance(PEG_EXTENDED
+          - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - LENGTH_OF_PEG,
+          MOTOR_VALUE_FORWARD));
+      // does not include drift distance
+    } else {
+      System.out.println(
+          "DO NOT START HERE!!! MOVE CLOSER TO WALL OR LOSE ALL AUTON POINTS!!");
+    }
   }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToRetrievalZone.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToRetrievalZone.java
deleted file mode 100644 (file)
index edb7d95..0000000
--- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToRetrievalZone.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package org.usfirst.frc.team3501.robot.commandgroups;
-
-import org.usfirst.frc.team3501.robot.Constants.Direction;
-import org.usfirst.frc.team3501.robot.Robot;
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
-import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- *
- */
-public class AutonGearThenBaselinePegCloseToRetrievalZone extends CommandGroup {
-
-  private static final double DISTANCE_FROM_RETRIEVAL_ZONE_WALL = 111.5676;
-  private static final int MOTOR_VALUE_TURN = 60;
-  private static final int MOTOR_VALUE_FORWARD = 60;
-
-  public AutonGearThenBaselinePegCloseToRetrievalZone() {
-
-    double x = DISTANCE_FROM_RETRIEVAL_ZONE_WALL;
-    requires(Robot.getDriveTrain());
-    addSequential(new DriveDistance(
-        DISTANCE_FROM_RETRIEVAL_ZONE_WALL / Math.sqrt(3) + 52.4614,
-        MOTOR_VALUE_FORWARD));
-    addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
-    addSequential(new DriveDistance(
-        143.3272 - 2 * DISTANCE_FROM_RETRIEVAL_ZONE_WALL / Math.sqrt(3) - 10.5,
-        MOTOR_VALUE_FORWARD));
-    // does not include drift distance
-
-  }
-}