finished Auton strat for peg close to boiler in any alliance

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..bdb9780
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java
@@ -0,0 +1,30 @@
+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 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() {
+  
+      
+      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
+      
+    }
+}