add AutonGear commandgroup

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
new file mode 100644 (file)
index 0000000..b674b35
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
@@ -0,0 +1,76 @@
+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;
+
+/**
+ * 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
+
+{
+
+  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());
+    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!!");
+    }
+  }
+}