X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommandgroups%2FAutonShoot.java;fp=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommandgroups%2FAutonShoot.java;h=c2af771f39c13f3288c2f861217aa9f091d07375;hp=0000000000000000000000000000000000000000;hb=b70398a7d5ac5f4ce64156d84227ccb4828c0615;hpb=7ba6bc91b8cf7205e05cdb974dc80fdd668ebe59

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonShoot.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonShoot.java
new file mode 100644
index 0000000..c2af771
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonShoot.java
@@ -0,0 +1,26 @@
+package org.usfirst.frc.team3501.robot.commandgroups;
+
+import org.usfirst.frc.team3501.robot.Constants.Direction;
+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 AutonShoot extends CommandGroup {
+
+  private static final int ROBOT_WIDTH = 40; // inches
+  private static final int ROBOT_LENGTH = 36; // inches
+
+  public AutonShoot() {
+    addSequential(
+        new DriveDistance(37.12 + (ROBOT_WIDTH / 2) - (ROBOT_LENGTH / 2), 5));
+    addSequential(new TurnForAngle(135, Direction.LEFT, 10));
+    addSequential(new DriveDistance(
+        (37.12 + (ROBOT_WIDTH / 2)) * Math.sqrt(2) - 26.25 - (ROBOT_LENGTH / 2),
+        10));
+    addSequential(new Shoot(15 - timeSinceInitialized()));
+  }
+}