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

   1 package org.usfirst.frc.team3501.robot.commandgroups;
   2
   3 import org.usfirst.frc.team3501.robot.Constants.Direction;
   4 import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
   5 import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle;
   6
   7 import edu.wpi.first.wpilibj.command.CommandGroup;
   8
   9 /**
  10  *
  11  */
  12 public class AutonShoot extends CommandGroup {
  13
  14   private static final int ROBOT_WIDTH = 40; // inches
  15   private static final int ROBOT_LENGTH = 36; // inches
  16
  17   public AutonShoot() {
  18     addSequential(
  19         new DriveDistance(37.12 + (ROBOT_WIDTH / 2) - (ROBOT_LENGTH / 2), 5));
  20     addSequential(new TurnForAngle(135, Direction.LEFT, 10));
  21     addSequential(new DriveDistance(
  22         (37.12 + (ROBOT_WIDTH / 2)) * Math.sqrt(2) - 26.25 - (ROBOT_LENGTH / 2),
  23         10));
  24     addSequential(new Shoot(15 - timeSinceInitialized()));
  25   }
  26 }