From aa770db66b0d4fd330a8781602d10aea434d104d Mon Sep 17 00:00:00 2001
From: Michael Wang <michaelwang9000@gmail.com>
Date: Sat, 14 Jan 2017 17:40:25 -0800
Subject: [PATCH] Add command group for robot to go to hopper and shoot

---
 .../robot/commandgroups/AutonHopperShoot.java | 43 +++++++++++++++++++
 1 file changed, 43 insertions(+)
 create mode 100644 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
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());
+
+  }
+}
-- 
2.30.2