challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java

   1 package org.usfirst.frc.team3501.robot.commandgroups;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4 import org.usfirst.frc.team3501.robot.Constants.Direction;
   5 import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
   6 import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
   7 import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
   8
   9 import edu.wpi.first.wpilibj.command.CommandGroup;
  10
  11 /**
  12  * // Robot starts in middle, goes to the hopper, then boiler,then shoots during
  13  * auton
  14  */
  15 public class AutonHopperShoot extends CommandGroup {
  16
  17   // If red, direction is right; if blue, direction is left
  18   private static final Direction DIRECTION_TO_HOPPER = Constants.Direction.RIGHT;
  19   // If red, direction is left; if blue, direction is right
  20   private static final Direction DIRECTION_TO_BOILER = Constants.Direction.LEFT;
  21
  22   public AutonHopperShoot() {
  23     // Robot drives from center to front of airship
  24     addSequential(new DriveDistance(94.75, 0.75));
  25     // Robot turns towards hopper
  26     addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 1.0));
  27     // Robot drives into hopper switch
  28     addSequential(new DriveDistance(44.0, 1));
  29     // Robot backs up from switch
  30     addSequential(new DriveDistance(5.0, -1.0));
  31     // Robot turns towards the boiler
  32     addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 1.0));
  33     // Robot drives to boiler
  34     addSequential(new DriveDistance(75.7, 0.75));
  35     // Robot turns parallel to boiler
  36     addSequential(new TurnForAngle(46.6, Constants.Direction.LEFT, 1.0));
  37     // Shoot
  38     addSequential(new RunFlyWheelContinuous());
  39
  40   }
  41 }