From: Michael Wang <michaelwang9000@gmail.com>
Date: Sun, 15 Jan 2017 01:40:25 +0000 (-0800)
Subject: Add command group for robot to go to hopper and shoot
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=aa770db66b0d4fd330a8781602d10aea434d104d

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