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