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.commands.driving.DriveDistance;
   5 import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
   6 import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheelContinuous;
   7
   8 import edu.wpi.first.wpilibj.command.CommandGroup;
   9
  10 /**
  11  * // Robot starts in middle, goes to the hopper, then boiler,then shoots during
  12  * auton
  13  */
  14 public class AutonHopperShoot extends CommandGroup {
  15
  16   private static final double DISTANCE_TO_HOPPER = 224.569677;
  17   private static final double DISTANCE_TO_TURNING_POSITION = 5.0;
  18
  19   public AutonHopperShoot() {
  20     // Robot drives from center to front of airship
  21     addSequential(new DriveDistance(DISTANCE_TO_TURNING_POSITION, 0.75));
  22     // Robot turns towards hopper
  23     addSequential(new TurnForAngle(45.0, Constants.Direction.RIGHT, 1.0));
  24     // Robot drives near hopper
  25     addSequential(new DriveDistance(DISTANCE_TO_HOPPER, 1));
  26     // Robot turns right towards hopper
  27     addSequential(new TurnForAngle(45.0, Constants.Direction.RIGHT, 1.0));
  28     // Robot drives into hopper switch
  29     addSequential(new DriveDistance(2.0, 1.0));
  30     // Robot backs up from switch
  31     addSequential(new DriveDistance(2.0, -1.0));
  32     // Robot turns towards the boiler
  33     addSequential(new TurnForAngle(90.0, Constants.Direction.RIGHT, 1.0));
  34     // Robot drives to boiler
  35     addSequential(new DriveDistance(123.3, 1.0));
  36     // Robot turns parallel to boiler
  37     addSequential(new TurnForAngle(45.0, Constants.Direction.LEFT, 1.0));
  38     // Shoot
  39     addSequential(new RunFlyWheelContinuous());
  40
  41   }
  42 }