1 package org
.usfirst
.frc
.team3501
.robot
.commands
.auton
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.DriveDistance
;
5 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.DriveForTime
;
6 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.TurnForAngle
;
8 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
11 * This command group will be used in autonomous. Based on what position the
12 * robot is in, the robot will align with the goal. In the Software 2015-2016
13 * Google folder is a picture explaining each of the cases.
15 * dependency on sensors: lidars, encoders, gyro
17 * dependency on subsystems: drivetrain
19 * dependency on other commands: TurnForAngle(), DriveForDistance()
21 * pre-condition: robot is flush against a defense at the specified position in
22 * the opponent's courtyard
24 * post-condition: the robot is parallel to one of the three goals and the
25 * shooter is facing that goal
28 public class AlignToScore
extends CommandGroup
{
30 private final double DEFAULT_SPEED
= 0.5;
31 private final double maxTimeout
= 5;
34 // assuming that positive angle means turning right
35 // and negative angle means turning left
37 // constants for position 1: low bar
39 public double horizontalDistToGoal
;
41 public AlignToScore(int position
) {
42 if (Constants
.Auton
.isUsingTime
) {
44 addSequential(new DriveForTime(Constants
.Auton
.POS1_DIST1_TIME
,
45 Constants
.Auton
.POS1_DRIVE_MAXSPEED
));
46 addSequential(new TurnForAngle(Constants
.Auton
.POS1_TURN1
,
47 Constants
.Auton
.POS1_TURN_MAXSPEED
));
48 addSequential(new DriveDistance(Constants
.Auton
.POS1_DIST2_TIME
,
49 Constants
.Auton
.POS1_DRIVE_MAXSPEED
));
50 horizontalDistToGoal
= 0;
52 else if (position
== 2) {
53 addSequential(new DriveForTime(Constants
.Auton
.POS2_DIST1_TIME
,
54 Constants
.Auton
.POS2_DRIVE_MAXSPEED
));
55 addSequential(new TurnForAngle(Constants
.Auton
.POS2_TURN1
,
56 Constants
.Auton
.POS2_TURN_MAXSPEED
));
57 addSequential(new DriveDistance(Constants
.Auton
.POS2_DIST2_TIME
,
58 Constants
.Auton
.POS2_DRIVE_MAXSPEED
));
59 horizontalDistToGoal
= 0;
61 else if (position
== 3) {
62 addSequential(new DriveForTime(Constants
.Auton
.POS3_DIST1_TIME
,
63 Constants
.Auton
.POS3_DRIVE_MAXSPEED
));
64 addSequential(new TurnForAngle(Constants
.Auton
.POS3_TURN1
,
65 Constants
.Auton
.POS3_TURN_MAXSPEED
));
66 addSequential(new DriveDistance(Constants
.Auton
.POS3_DIST2_TIME
,
67 Constants
.Auton
.POS3_DRIVE_MAXSPEED
));
68 horizontalDistToGoal
= 0;
70 else if (position
== 4) {
71 addSequential(new DriveForTime(Constants
.Auton
.POS4_DIST1_TIME
,
72 Constants
.Auton
.POS4_DRIVE_MAXSPEED
));
73 addSequential(new TurnForAngle(Constants
.Auton
.POS4_TURN1
,
74 Constants
.Auton
.POS4_TURN_MAXSPEED
));
75 addSequential(new DriveDistance(Constants
.Auton
.POS4_DIST2_TIME
,
76 Constants
.Auton
.POS4_DRIVE_MAXSPEED
));
77 horizontalDistToGoal
= 0;
79 else if (position
== 5) {
80 addSequential(new DriveForTime(Constants
.Auton
.POS5_DIST1_TIME
,
81 Constants
.Auton
.POS5_DRIVE_MAXSPEED
));
82 addSequential(new TurnForAngle(Constants
.Auton
.POS5_TURN1
,
83 Constants
.Auton
.POS5_TURN_MAXSPEED
));
84 addSequential(new DriveDistance(Constants
.Auton
.POS5_DIST2_TIME
,
85 Constants
.Auton
.POS5_DRIVE_MAXSPEED
));
86 horizontalDistToGoal
= 0;
90 // position 1 is always the low bar
92 addSequential(new DriveDistance(Constants
.Auton
.POS1_DIST1
,
93 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
94 addSequential(new TurnForAngle(Constants
.Auton
.POS1_TURN1
,
95 Constants
.Auton
.TURN_MAX_TIMEOUT
));
96 addSequential(new DriveDistance(Constants
.Auton
.POS1_DIST2
,
97 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
98 } else if (position
== 2) {
100 addSequential(new DriveDistance(Constants
.Auton
.POS2_DIST1
,
101 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
102 addSequential(new TurnForAngle(Constants
.Auton
.POS2_TURN1
,
103 Constants
.Auton
.TURN_MAX_TIMEOUT
));
104 addSequential(new DriveDistance(Constants
.Auton
.POS2_DIST2
,
105 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
107 } else if (position
== 3) {
109 addSequential(new DriveDistance(Constants
.Auton
.POS3_DIST1
,
110 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
111 addSequential(new TurnForAngle(Constants
.Auton
.POS3_TURN1
, maxTimeout
));
112 addSequential(new DriveDistance(Constants
.Auton
.POS3_DIST2
,
113 Constants
.Auton
.TURN_MAX_TIMEOUT
));
114 addSequential(new TurnForAngle(Constants
.Auton
.POS3_TURN2
, maxTimeout
));
115 addSequential(new DriveDistance(Constants
.Auton
.POS3_DIST3
,
116 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
118 } else if (position
== 4) {
120 addSequential(new DriveDistance(Constants
.Auton
.POS4_DIST1
,
121 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
122 addSequential(new TurnForAngle(Constants
.Auton
.POS4_TURN1
, maxTimeout
));
123 addSequential(new DriveDistance(Constants
.Auton
.POS4_DIST2
,
124 Constants
.Auton
.TURN_MAX_TIMEOUT
));
125 addSequential(new TurnForAngle(Constants
.Auton
.POS4_TURN2
, maxTimeout
));
126 addSequential(new DriveDistance(Constants
.Auton
.POS4_DIST3
,
127 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
129 } else if (position
== 5) {
131 addSequential(new DriveDistance(Constants
.Auton
.POS5_DIST1
,
132 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
133 addSequential(new TurnForAngle(Constants
.Auton
.POS5_TURN1
, maxTimeout
));
134 addSequential(new DriveDistance(Constants
.Auton
.POS5_DIST2
,
135 Constants
.Auton
.TURN_MAX_TIMEOUT
));
136 addSequential(new TurnForAngle(Constants
.Auton
.POS5_TURN2
, maxTimeout
));
137 addSequential(new DriveDistance(Constants
.Auton
.POS5_DIST3
,
138 Constants
.Auton
.DRIVE_MAX_TIMEOUT
));
142 // following commented out method is calculations for path of robot in auton
143 // after passing through defense using two lidars
145 * public static double lidarCalculateAngleToTurn(int position,
146 * double horizontalDistToGoal) {
147 * double leftDist = Robot.driveTrain.getLeftLidarDistance();
148 * double rightDist = Robot.driveTrain.getRightLidarDistance();
150 * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2);
151 * double distToTower;
152 * // TODO: figure out if we do want to shoot into the side goal if we are
153 * // in position 1 or 2, or if we want to change that
154 * if (position == 1 || position == 2) {
156 * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
157 * - DIST_CASTLE_WALL_TO_SIDE_GOAL;
160 * // TODO: figure out if we do want to shoot into the font goal if we are
161 * // in position 3, 4, 5, or if we want to change that
164 * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2)
165 * - DIST_CASTLE_WALL_TO_SIDE_GOAL;
168 * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal);
170 * return angleToTurn;