challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.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  * this is an auton strategy to cross the baseline and put a gear on a peg
  12  * requires predetermined starting point anywhere except between 109.1767 inches
  13  * and 205.7286 inches from the right side of the arena this program chooses
  14  * which peg to go for based on the starting point
  15  */
  16 public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup
  17
  18 {
  19
  20   private static final double LENGTH_OF_PEG = 10.5;
  21   private static final double PEG_EXTENDED = 136.5664;
  22   private static final int WIDTH_OF_ARENA = 324;
  23   private static final double MOTOR_VALUE_TURN = 0.5;// this is probably not
  24                                                      // correct
  25   private static final double MOTOR_VALUE_FORWARD = 0.5;// random
  26   double DISTANCE_FROM_BOILER_WALL = 105.7126; // This is the starting point
  27                                                // from the right side of the
  28                                                // arena in inches
  29
  30   public AutonGearThenBaselinePegCloseToBoiler() {
  31     requires(Robot.getDriveTrain());
  32     if (DISTANCE_FROM_BOILER_WALL > 0 && DISTANCE_FROM_BOILER_WALL < 109.1767) // right
  33                                                                                // peg
  34                                                                                // path
  35     {
  36       addSequential(
  37           new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 50.2168,
  38               MOTOR_VALUE_FORWARD)); // drive straight into extended line of peg
  39       addSequential(new TurnForAngle(60, Direction.LEFT, 5));// position
  40                                                              // robot
  41                                                              // for
  42                                                              // optimal
  43                                                              // gear
  44                                                              // placement
  45       addSequential(new DriveDistance(PEG_EXTENDED
  46           - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - LENGTH_OF_PEG,
  47           MOTOR_VALUE_FORWARD));// put gear on peg
  48       // does not include drift distance
  49     } else if (DISTANCE_FROM_BOILER_WALL > 205.7286
  50         && DISTANCE_FROM_BOILER_WALL < 324) // mirror of right peg path {
  51     {
  52       DISTANCE_FROM_BOILER_WALL = WIDTH_OF_ARENA - DISTANCE_FROM_BOILER_WALL; // Turning
  53                                                                               // DISTANCE_FROM_BOILER_WALL
  54                                                                               // into
  55                                                                               // starting
  56                                                                               // point
  57                                                                               // from
  58                                                                               // left
  59                                                                               // side
  60                                                                               // by
  61                                                                               // reversing
  62                                                                               // it
  63       addSequential(
  64           new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 50.2168,
  65               MOTOR_VALUE_FORWARD));
  66       addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
  67       addSequential(new DriveDistance(PEG_EXTENDED
  68           - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - LENGTH_OF_PEG,
  69           MOTOR_VALUE_FORWARD));
  70       // does not include drift distance
  71     } else {
  72       System.out.println(
  73           "DO NOT START HERE!!! MOVE CLOSER TO WALL OR LOSE ALL AUTON POINTS!!");
  74     }
  75   }
  76 }