Add command group for robot to go to hopper and shoot

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..ebad909
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team3501.robot.commandgroups;
+
+import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
+import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
+import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheel;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonHopperShoot extends CommandGroup {
+
+  public AutonHopperShoot() {
+
+    // Robot drives from center to front of airship
+    addSequential(new DriveDistance(92.3, 1));
+    // Robot turns towards hopper
+    addSequential(new TurnForAngle(90.0, Constants.Direction.RIGHT, 1.0));
+    // Robot drives near hopper
+    addSequential(new DriveDistance());
+    // Robot turns left towards hopper
+    addSequential(new TurnForAngle(90));
+    // Robot drives in front of hopper
+    addSequential(new DriveDistance());
+    // Robot turns to face hopper
+    addSequential(new TurnForAngle(90));
+    // Robot hits hopper switch
+    addSequential(new DriveDistance());
+    // Robot backs up from switch
+    addSequential(new DriveDistance());
+    // Robot turns towards the boiler
+    addSequential(new TurnForAngle(90));
+    // Robot drives to boiler
+    addSequential(new DriveDistance());
+    // Robot turns parallel to boiler
+    addSequential(new TurnForAngle(45));
+    // Shoot
+    addSequential(new RunFlyWheel());
+
+  }
+}