code review changes

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

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 (file)
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()));
+  }
+}