challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/commandgroups/AutonSideGear.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.Robot;
   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.command.CommandGroup;
   9
  10 /**
  11  *
  12  */
  13 public class AutonSideGear extends CommandGroup {
  14
  15   private static final double ROBOT_LENGTH = 36.0;
  16   private static final double ROBOT_WIDTH = 40.0;
  17
  18   private static final double STRAIGHT_DIST_FROM_BOILER = 76.8;
  19   private static final double DISTANCE_TO_RETRIEVAL_PEG = 114.3 + 17.625
  20       - ((162 - (17.625 + 35.25) - 38.625) / Math.sqrt(3));
  21   private static final double LENGTH_OF_PEG = 10.5;
  22   private static final double PEG_EXTENDED_BOILER = 117.518;
  23   private static final double PEG_EXTENDED_RETRIEVAL_ZONE = (DISTANCE_TO_RETRIEVAL_PEG
  24       * 2) / Math.sqrt(3);
  25   private static final double DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE = 255.6765151902;
  26   private static final double MOTOR_VALUE_TURN = 0.5;// this is probably not
  27                                                      // correct
  28   private static final double MOTOR_VALUE_FORWARD = 0.5;// random
  29   double distanceFromCorner;
  30
  31   /**
  32    * this is an auton strategy to cross the baseline and put a gear on a peg
  33    * requires predetermined starting point anywhere except between 109.1767
  34    * inches and 205.7286 inches from the right side of the arena this program
  35    * chooses which peg to go for based on the starting point
  36    */
  37   public AutonSideGear(String team, String side) {
  38     requires(Robot.getDriveTrain());
  39
  40     if (side.equals("BOILER")) {
  41       addSequential(new DriveDistance(
  42           131.6 - (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
  43               - ROBOT_LENGTH / 2,
  44           5));
  45
  46       if (team.equals("RED"))
  47         addSequential(new TurnForAngle(60, Direction.LEFT, 5));
  48       else if (team.equals("BLUE"))
  49         addSequential(new TurnForAngle(60, Direction.RIGHT, 5));
  50
  51       addSequential(new DriveDistance(
  52           2 * (94.88 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
  53               - ROBOT_LENGTH / 2 + 7,
  54           5));
  55     } else if (side.equals("RETRIEVAL")) {
  56       addSequential(new DriveDistance(
  57           131.6 - (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
  58               - ROBOT_LENGTH / 2,
  59           5));
  60
  61       if (team.equals("RED"))
  62         addSequential(new TurnForAngle(60, Direction.RIGHT, 5));
  63       else if (team.equals("BLUE"))
  64         addSequential(new TurnForAngle(60, Direction.LEFT, 5));
  65
  66       addSequential(new DriveDistance(
  67           2 * (93.13 - ROBOT_WIDTH / 2 - distanceFromCorner) / Math.sqrt(3)
  68               - ROBOT_LENGTH / 2,
  69           5));
  70     }
  71   }
  72 }