1 package org
.usfirst
.frc
.team3501
.robot
.commandgroups
;
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
;
8 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
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
16 public class AutonGearThenBaselinePegCloseToBoiler
extends CommandGroup
20 private static final double ROBOT_LENGTH
= 36.0;
21 private static final double ROBOT_WIDTH
= 40.0;
23 private static final double STRAIGHT_DIST_FROM_BOILER
= 76.8;
24 private static final double DISTANCE_TO_RETRIEVAL_PEG
= 114.3 + 17.625
25 - ((162 - (17.625 + 35.25) - 38.625) / Math
.sqrt(3));
26 private static final double LENGTH_OF_PEG
= 10.5;
27 private static final double PEG_EXTENDED_BOILER
= 117.518;
28 private static final double PEG_EXTENDED_RETRIEVAL_ZONE
= (DISTANCE_TO_RETRIEVAL_PEG
30 private static final double DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE
= 255.6765151902;
31 private static final double MOTOR_VALUE_TURN
= 0.5;// this is probably not
33 private static final double MOTOR_VALUE_FORWARD
= 0.5;// random
34 double distanceFromCorner
; // This is the starting point
35 // from the right side of the
36 // arena in inches (can change)
41 * which side to start on: "BLUE" for blue side and "RED" for red
43 * @param distanceFromcorner
44 * distance from the corner of the boiler
46 public AutonGearThenBaselinePegCloseToBoiler(String side
,
47 int distanceFromcorner
) {
48 requires(Robot
.getDriveTrain());
50 if (side
.equals("BOILER")) {
51 addSequential(new DriveDistance(
52 131.6 - (94.88 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)
55 // addSequential(new DriveDistance(
56 // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
57 // + STRAIGHT_DIST_FROM_BOILER - (ROBOT_LENGTH / 2.0),
58 // MOTOR_VALUE_FORWARD));
59 // position robot for optimal gear placement
60 addSequential(new TurnForAngle(60, Direction
.RIGHT
, 5));
62 addSequential(new DriveDistance(
63 2 * (94.88 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)
64 - ROBOT_LENGTH
/ 2 + 7,
66 // addSequential(new DriveDistance(PEG_EXTENDED_BOILER
67 // - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2) / Math.sqrt(3))
68 // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD));
70 // does not include drift distance
71 } else if (side
.equals("RETRIEVAL")) {
72 addSequential(new DriveDistance(
73 131.6 - (93.13 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)
76 addSequential(new TurnForAngle(60, Direction
.LEFT
, 5));
77 addSequential(new DriveDistance(
78 2 * (93.13 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)
81 // addSequential(new DriveDistance(
82 // ((distanceFromCorner + (ROBOT_WIDTH / 2.0)) / Math.sqrt(3))
83 // + DISTANCE_TO_RETRIEVAL_PEG - (ROBOT_LENGTH / 2.0),
84 // MOTOR_VALUE_FORWARD));
85 // addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN));
86 // addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
87 // - (((distanceFromCorner + (ROBOT_LENGTH / 2.0)) / Math.sqrt(3)) * 2)
88 // - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), MOTOR_VALUE_FORWARD));
89 // does not include drift distance