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
= 40.0;
21 private static final double ROBOT_WIDTH
= 36.0;
23 private static final double DISTANCE_TO_PEG_EXTENDED_BOILER
= 73.1657;
24 private static final double DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
= 114.3
25 + 17.625 - ((162 - (17.625 + 35.25) - 38.625) / Math
.sqrt(3));
26 private static final double FIRST_LIMIT_LENGTH
= 75.649;
27 private static final double SECOND_LIMIT_LENGTH
= 162.8289106736;
28 private static final double LENGTH_OF_PEG
= 10.5;
29 private static final double PEG_EXTENDED_BOILER
= 117.518;
30 private static final double PEG_EXTENDED_RETRIEVAL_ZONE
= (DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
32 private static final double DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE
= 255.6765151902;
33 private static final double MOTOR_VALUE_TURN
= 0.5;// this is probably not
35 private static final double MOTOR_VALUE_FORWARD
= 0.5;// random
36 double distanceFromCorner
; // This is the starting point
37 // from the right side of the
38 // arena in inches (can change)
43 * which side to start on: "BLUE" for blue side and "RED" for red
45 * @param distanceFromBoilerCorner
46 * distance from the corner of the boiler
48 public AutonGearThenBaselinePegCloseToBoiler(String team
,
49 int distanceFromBoilerCorner
) {
51 this.distanceFromCorner
= distanceFromBoilerCorner
;
52 requires(Robot
.getDriveTrain());
53 if (distanceFromBoilerCorner
>= 0
54 && distanceFromBoilerCorner
< FIRST_LIMIT_LENGTH
) {
57 // drive straight into extended line of peg
60 ((distanceFromCorner
+ (ROBOT_WIDTH
/ 2.0)) / Math
.sqrt(3))
61 + DISTANCE_TO_PEG_EXTENDED_BOILER
- (ROBOT_LENGTH
/ 2.0),
62 MOTOR_VALUE_FORWARD
));
63 // position robot for optimal gear placement
64 addSequential(new TurnForAngle(60, Direction
.LEFT
, MOTOR_VALUE_TURN
));
66 addSequential(new DriveDistance(PEG_EXTENDED_BOILER
67 - (((distanceFromCorner
+ (ROBOT_WIDTH
/ 2.0)) * 2)
69 - LENGTH_OF_PEG
- (ROBOT_LENGTH
/ 2.0),
70 MOTOR_VALUE_FORWARD
));
72 // does not include drift distance
73 } else if (distanceFromBoilerCorner
> SECOND_LIMIT_LENGTH
74 && distanceFromBoilerCorner
<= DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE
) // mirror
80 this.distanceFromCorner
= DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE
81 - distanceFromBoilerCorner
;
82 // Turning DISTANCE_FROM_BOILER_WALL into starting point from left side by
86 ((distanceFromCorner
+ (ROBOT_WIDTH
/ 2.0)) / Math
.sqrt(3))
87 + DISTANCE_TO_PEG_EXTENDED_RETRIEVAL_ZONE
88 - (ROBOT_LENGTH
/ 2.0),
89 MOTOR_VALUE_FORWARD
));
90 addSequential(new TurnForAngle(60, Direction
.RIGHT
, MOTOR_VALUE_TURN
));
91 addSequential(new DriveDistance(PEG_EXTENDED_RETRIEVAL_ZONE
92 - (((distanceFromCorner
+ (ROBOT_LENGTH
/ 2.0)) / Math
.sqrt(3)) * 2)
93 - LENGTH_OF_PEG
- (ROBOT_LENGTH
/ 2.0),
94 MOTOR_VALUE_FORWARD
));
95 // does not include drift distance
98 "DO NOT START HERE!!! MOVE CLOSER TO A WALL OR LOSE ALL AUTON POINTS!!");