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
   8 import edu.wpi.first.wpilibj.Timer;
   9 import edu.wpi.first.wpilibj.command.CommandGroup;
  10 import edu.wpi.first.wpilibj.command.WaitCommand;
  11
  12 /**
  13  * // Robot starts in middle, goes to the hopper, then boiler,then shoots during
  14  * auton
  15  */
  16 public class AutonHopperShoot extends CommandGroup {
  17   // If red, direction is right; if blue, direction is left
  18   private static final Direction DIRECTION_TO_HOPPER = Constants.Direction.LEFT;
  19   // If red, direction is left; if blue, direction is right
  20   private static final Direction DIRECTION_TO_BOILER = Constants.Direction.RIGHT;
  21
  22   private Timer timer;
  23
  24   public AutonHopperShoot() {
  25     timer = new Timer();
  26     // Robot drives from center to front of airship
  27     addSequential(new DriveDistance(78.5, 2.7));
  28     // Robot turns towards hopper
  29     addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 2.5));
  30     // Robot drives into hopper switch
  31     addSequential(new DriveDistance(42.12, 5.25));
  32     addSequential(new WaitCommand(1));
  33     // Robot backs up from switch
  34     addSequential(new DriveDistance(-25.0, 2.9));
  35     // Robot turns towards the boiler
  36     addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 5.0));
  37     // Robot drives to boiler
  38     addSequential(new DriveDistance(90, 5.0));
  39     // Robot turns parallel to boiler
  40     addSequential(new TurnForAngle(45, DIRECTION_TO_BOILER, 5.0));
  41     // Shoot
  42     addSequential(new Shoot(15 - timeSinceInitialized()));
  43   }
  44
  45 }