Commit | Line | Data |
---|---|---|
fb75626b KZ |
1 | package org.usfirst.frc.team3501.robot.commands.auton; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; | |
4 | import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; | |
5 | ||
6 | import edu.wpi.first.wpilibj.command.CommandGroup; | |
7 | ||
8 | /** | |
9 | * This command group will be used in autonomous. Based on what position the | |
10 | * robot is in, the robot will align with the goal. In the Software 2015-2016 | |
11 | * Google folder is a picture explaining each of the cases. | |
12 | * | |
13 | * dependency on sensors: lidars, encoders, gyro | |
14 | * | |
15 | * dependency on subsystems: drivetrain | |
16 | * | |
17 | * dependency on other commands: TurnForAngle(), DriveForDistance() | |
18 | * | |
19 | * pre-condition: robot is flush against a defense at the specified position in | |
20 | * the opponent's courtyard | |
21 | * | |
22 | * post-condition: the robot is parallel to one of the three goals and the | |
23 | * shooter is facing that goal | |
24 | * | |
25 | */ | |
26 | public class AlignToScore extends CommandGroup { | |
27 | private final static double CENTER_OF_MASS_TO_ROBOT_FRONT = 0; | |
28 | private final static double DIST_CASTLE_WALL_TO_SIDE_GOAL = 0; | |
29 | private final static double DIST_CASTLE_WALL_TO_FRONT_GOAL = 0; | |
30 | ||
31 | private final double DEFAULT_SPEED = 0.5; | |
32 | private final double maxTimeout = 5; | |
33 | ||
34 | // in inches | |
35 | // assuming that positive angle means turning right | |
36 | // and negative angle means turning left | |
37 | ||
38 | // constants for position 1: low bar | |
39 | private final double POS1_DIST1 = 109; | |
40 | private final double POS1_TURN1 = 60; | |
41 | private final double POS1_DIST2 = 0; | |
42 | ||
43 | // constants for position 2 | |
44 | private final double POS2_DIST1 = 140; | |
45 | private final double POS2_TURN1 = 60; | |
46 | private final double POS2_DIST2 = 0; | |
47 | ||
48 | // constants for position 3 | |
49 | private final double POS3_DIST1 = 0; | |
50 | private final double POS3_TURN1 = 90; | |
51 | private final double POS3_DIST2 = 35.5; | |
52 | private final double POS3_TURN2 = -90; | |
53 | private final double POS3_DIST3 = 0; | |
54 | ||
55 | // constants for position 4 | |
56 | private final double POS4_DIST1 = 0; | |
57 | private final double POS4_TURN1 = -90; | |
58 | private final double POS4_DIST2 = 18.5; | |
59 | private final double POS4_TURN2 = 90; | |
60 | private final double POS4_DIST3 = 0; | |
61 | ||
62 | // constants for position 5 | |
63 | private final double POS5_DIST1 = 0; | |
64 | private final double POS5_TURN1 = -90; | |
65 | private final double POS5_DIST2 = 72.5; | |
66 | private final double POS5_TURN2 = 90; | |
67 | private final double POS5_DIST3 = 0; | |
68 | ||
69 | public double horizontalDistToGoal; | |
70 | ||
71 | public AlignToScore(int position) { | |
72 | ||
73 | if (position == 1) { | |
74 | ||
75 | // position 1 is always the low bar | |
76 | ||
77 | addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED)); | |
78 | addSequential(new TurnForAngle(POS1_TURN1, maxTimeout)); | |
79 | addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED)); | |
80 | horizontalDistToGoal = 0; | |
81 | } else if (position == 2) { | |
82 | ||
83 | addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED)); | |
84 | addSequential(new TurnForAngle(POS2_TURN1, maxTimeout)); | |
85 | addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED)); | |
86 | horizontalDistToGoal = 0; | |
87 | ||
88 | } else if (position == 3) { | |
89 | ||
90 | addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED)); | |
91 | addSequential(new TurnForAngle(POS3_TURN1, maxTimeout)); | |
92 | addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED)); | |
93 | addSequential(new TurnForAngle(POS3_TURN2, maxTimeout)); | |
94 | addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED)); | |
95 | horizontalDistToGoal = 0; | |
96 | ||
97 | } else if (position == 4) { | |
98 | ||
99 | addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED)); | |
100 | addSequential(new TurnForAngle(POS4_TURN1, maxTimeout)); | |
101 | addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED)); | |
102 | addSequential(new TurnForAngle(POS4_TURN2, maxTimeout)); | |
103 | addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED)); | |
104 | horizontalDistToGoal = 0; | |
105 | ||
106 | } else if (position == 5) { | |
107 | ||
108 | addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED)); | |
109 | addSequential(new TurnForAngle(POS5_TURN1, maxTimeout)); | |
110 | addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED)); | |
111 | addSequential(new TurnForAngle(POS5_TURN2, maxTimeout)); | |
112 | addSequential(new DriveDistance(POS5_DIST3, DEFAULT_SPEED)); | |
113 | horizontalDistToGoal = 0; | |
114 | ||
115 | } | |
116 | } | |
117 | ||
118 | // following commented out method is calculations for path of robot in auton | |
119 | // after passing through defense using two lidars | |
120 | /* | |
121 | * public static double lidarCalculateAngleToTurn(int position, | |
122 | * double horizontalDistToGoal) { | |
123 | * double leftDist = Robot.driveTrain.getLeftLidarDistance(); | |
124 | * double rightDist = Robot.driveTrain.getRightLidarDistance(); | |
125 | * | |
126 | * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2); | |
127 | * double distToTower; | |
128 | * // TODO: figure out if we do want to shoot into the side goal if we are | |
129 | * // in position 1 or 2, or if we want to change that | |
130 | * if (position == 1 || position == 2) { | |
131 | * distToTower = Math | |
132 | * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) | |
133 | * - DIST_CASTLE_WALL_TO_SIDE_GOAL; | |
134 | * } | |
135 | * | |
136 | * // TODO: figure out if we do want to shoot into the font goal if we are | |
137 | * // in position 3, 4, 5, or if we want to change that | |
138 | * else { | |
139 | * distToTower = Math | |
140 | * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) | |
141 | * - DIST_CASTLE_WALL_TO_SIDE_GOAL; | |
142 | * } | |
143 | * | |
144 | * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal); | |
145 | * | |
146 | * return angleToTurn; | |
147 | * } | |
148 | */ | |
149 | } |