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
;
13 public class AutonSideGear
extends CommandGroup
{
15 private static final double ROBOT_LENGTH
= 36.0;
16 private static final double ROBOT_WIDTH
= 40.0;
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
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
28 private static final double MOTOR_VALUE_FORWARD
= 0.5;// random
29 double distanceFromCorner
;
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
37 public AutonSideGear(String side
) {
38 requires(Robot
.getDriveTrain());
40 if (side
.equals("BOILER")) {
41 addSequential(new DriveDistance(
42 131.6 - (94.88 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)
45 addSequential(new TurnForAngle(60, Direction
.RIGHT
, 5));
46 addSequential(new DriveDistance(
47 2 * (94.88 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)
48 - ROBOT_LENGTH
/ 2 + 7,
50 } else if (side
.equals("RETRIEVAL")) {
51 addSequential(new DriveDistance(
52 131.6 - (93.13 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)
55 addSequential(new TurnForAngle(60, Direction
.LEFT
, 5));
56 addSequential(new DriveDistance(
57 2 * (93.13 - ROBOT_WIDTH
/ 2 - distanceFromCorner
) / Math
.sqrt(3)