code review changes

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

diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java
new file mode 100644 (file)
index 0000000..eb412d2
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java
@@ -0,0 +1,45 @@
+package org.usfirst.frc.team3501.robot.commandgroups;
+
+import org.usfirst.frc.team3501.robot.Constants;
+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.Timer;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ * // Robot starts in middle, goes to the hopper, then boiler,then shoots during
+ * auton
+ */
+public class AutonHopperShoot extends CommandGroup {
+  // If red, direction is right; if blue, direction is left
+  private static final Direction DIRECTION_TO_HOPPER = Constants.Direction.LEFT;
+  // If red, direction is left; if blue, direction is right
+  private static final Direction DIRECTION_TO_BOILER = Constants.Direction.RIGHT;
+
+  private Timer timer;
+
+  public AutonHopperShoot() {
+    timer = new Timer();
+    // Robot drives from center to front of airship
+    addSequential(new DriveDistance(78.5, 2.7));
+    // Robot turns towards hopper
+    addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 2.5));
+    // Robot drives into hopper switch
+    addSequential(new DriveDistance(42.12, 5.25));
+    addSequential(new WaitCommand(1));
+    // Robot backs up from switch
+    addSequential(new DriveDistance(-25.0, 2.9));
+    // Robot turns towards the boiler
+    addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 5.0));
+    // Robot drives to boiler
+    addSequential(new DriveDistance(90, 5.0));
+    // Robot turns parallel to boiler
+    addSequential(new TurnForAngle(45, DIRECTION_TO_BOILER, 5.0));
+    // Shoot
+    addSequential(new Shoot(15 - timeSinceInitialized()));
+  }
+
+}