Commit | Line | Data |
---|---|---|
fca066b6 CZ |
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 ROBOT_LENGTH = 40.0; | |
21 | private static final double ROBOT_WIDTH = 36.0; | |
22 | ||
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 | |
31 | * 2) / Math.sqrt(3); | |
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 | |
34 | // correct | |
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) | |
39 | ||
40 | /*** | |
41 | * | |
42 | * @param team | |
43 | * which side to start on: "BLUE" for blue side and "RED" for red | |
44 | * side | |
45 | * @param distanceFromBoilerCorner | |
46 | * distance from the corner of the boiler | |
47 | */ | |
48 | public AutonGearThenBaselinePegCloseToBoiler(String team, | |
49 | int distanceFromBoilerCorner) { | |
50 | ||
51 | this.distanceFromCorner = distanceFromBoilerCorner; | |
52 | requires(Robot.getDriveTrain()); | |
53 | if (distanceFromBoilerCorner >= 0 | |
54 | && distanceFromBoilerCorner < FIRST_LIMIT_LENGTH) { | |
55 | // boiler peg path | |
56 | ||
57 | // drive straight into extended line of peg | |
58 | addSequential( | |
59 | new DriveDistance( | |
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)); | |
65 | // put gear on peg | |
66 | addSequential(new DriveDistance(PEG_EXTENDED_BOILER | |
67 | - (((distanceFromCorner + (ROBOT_WIDTH / 2.0)) * 2) | |
68 | / Math.sqrt(3)) | |
69 | - LENGTH_OF_PEG - (ROBOT_LENGTH / 2.0), | |
70 | MOTOR_VALUE_FORWARD)); | |
71 | ||
72 | // does not include drift distance | |
73 | } else if (distanceFromBoilerCorner > SECOND_LIMIT_LENGTH | |
74 | && distanceFromBoilerCorner <= DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE) // mirror | |
75 | // of | |
76 | // right | |
77 | // peg | |
78 | // path | |
79 | { | |
80 | this.distanceFromCorner = DISTANCE_BETWEEN_BOILER_AND_RETRIEVAL_ZONE | |
81 | - distanceFromBoilerCorner; | |
82 | // Turning DISTANCE_FROM_BOILER_WALL into starting point from left side by | |
83 | // reversing it | |
84 | addSequential( | |
85 | new DriveDistance( | |
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 | |
96 | } else { | |
97 | System.out.println( | |
98 | "DO NOT START HERE!!! MOVE CLOSER TO A WALL OR LOSE ALL AUTON POINTS!!"); | |
99 | } | |
100 | } | |
101 | } |