Skip to content

Commit 8324a52

Browse files
caparomulaDylanTaylor29nlaverdure
authored
Sector code (#118)
* Added function to map pose to reef sector * Added code to get left/right pole pose for a reef face * spotless * Refactoring * A little cleanup * Reword some documentation * Fix inconsistent units for reef radius * Replace Transform2d, Pose2d, Rotation2d allocations with kZero, kPi constants where possible * Clarify logic of Reef.getSector() * reconfigured driver buttons to what they will actually be during competition * Publish nearet left/right coral scoring pose * Add some smart dashboard debugging values * Send command scheduler to smart dashboard * Fix sector index issue * Undpo bogus index fix * Change dashboard names * A few tweaks * Flip angle by 180% for red reef * create geometry objects in native units of meters or abstract Distance where possible * Adjust reef position for stem gym, tweak constants, fix rotation * spotless * Finalize merge conflict --------- Co-authored-by: DylanTaylor29 <[email protected]> Co-authored-by: nlaverdure <[email protected]>
1 parent 3c3640f commit 8324a52

File tree

13 files changed

+293
-98
lines changed

13 files changed

+293
-98
lines changed

‎src/main/java/frc/game/FeederStation.java

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
package frc.game;
2+
3+
import static edu.wpi.first.units.Units.Degrees;
4+
import static edu.wpi.first.units.Units.Inches;
5+
6+
import edu.wpi.first.math.geometry.Pose2d;
7+
import edu.wpi.first.math.geometry.Rotation2d;
8+
import edu.wpi.first.math.geometry.Transform2d;
9+
10+
public enum FeederStation {
11+
blueRight(new Pose2d(Inches.of(33.51), Inches.of(25.80), new Rotation2d(Degrees.of(54 + 180)))),
12+
blueLeft(new Pose2d(Inches.of(33.51), Inches.of(291.20), new Rotation2d(Degrees.of(306 + 180)))),
13+
redRight(new Pose2d(Inches.of(657.37), Inches.of(291.20), new Rotation2d(Degrees.of(234 + 180)))),
14+
redLeft(new Pose2d(Inches.of(657.37), Inches.of(25.8), new Rotation2d(Degrees.of(126 + 180))));
15+
16+
private Pose2d pose;
17+
18+
FeederStation(Pose2d pose) {
19+
final Transform2d feederStationOffset =
20+
new Transform2d(Inches.of(-18), Inches.of(0), Rotation2d.kZero);
21+
this.pose = pose.plus(feederStationOffset);
22+
}
23+
24+
public Pose2d getPose() {
25+
return pose;
26+
}
27+
}

‎src/main/java/frc/util/Gamepiece.java renamed to ‎src/main/java/frc/game/Gamepiece.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
package frc.util;
1+
package frc.game;
22

33
import edu.wpi.first.wpilibj.util.Color;
44
import frc.robot.Constants.LedConstants;

‎src/main/java/frc/game/Reef.java

Lines changed: 176 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,176 @@
1+
package frc.game;
2+
3+
import static edu.wpi.first.units.Units.Degrees;
4+
import static edu.wpi.first.units.Units.Inches;
5+
import static edu.wpi.first.units.Units.Meters;
6+
7+
import edu.wpi.first.math.geometry.Pose2d;
8+
import edu.wpi.first.math.geometry.Rotation2d;
9+
import edu.wpi.first.math.geometry.Transform2d;
10+
import edu.wpi.first.math.geometry.Translation2d;
11+
import edu.wpi.first.units.measure.Distance;
12+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
13+
14+
/**
15+
* Enumerates the Reefs on the field.
16+
*
17+
* <p>Provides methods for finding the nearest reef to a pose, and for finding the nearest reef face
18+
* to a pose.
19+
*/
20+
public enum Reef {
21+
// TODO remove reef ceter offset
22+
Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5 + 4.5), Rotation2d.kZero)),
23+
Red(new Pose2d(Inches.of(514.125), Inches.of(158.5), Rotation2d.kPi));
24+
25+
/** The radius of the reef hexagon */
26+
public static final Distance radius = Inches.of(50.25);
27+
28+
/** The spacing between the left and right pipe centers. */
29+
public static final Distance pipeSpacing = Inches.of(13); // GM 5.3
30+
31+
/**
32+
* Gets the nearest reef to the given pose.
33+
*
34+
* <p>The nearest reef is determined by the distance between the given pose and the center of each
35+
* reef.
36+
*
37+
* @param atPose the pose for which to find the nearest reef
38+
* @return the nearest reef
39+
*/
40+
public static Reef getNearestReef(Pose2d atPose) {
41+
var deltaRed = atPose.getTranslation().minus(Red.getCenterPose().getTranslation()).getNorm();
42+
var deltaBlue = atPose.getTranslation().minus(Blue.getCenterPose().getTranslation()).getNorm();
43+
return deltaRed < deltaBlue ? Red : Blue;
44+
}
45+
46+
private Pose2d centerPose;
47+
48+
Reef(Pose2d centerPose) {
49+
this.centerPose = centerPose;
50+
}
51+
52+
/**
53+
* Get the center of this reef.
54+
*
55+
* @return the pose for the center of this reef
56+
*/
57+
public Pose2d getCenterPose() {
58+
return centerPose;
59+
}
60+
61+
/**
62+
* Get the sector of this reef into which the specified pose falls. The reefs are divided into 6
63+
* sectors, each 60 degrees wide. The sectors are numbered 0 to 5, starting from the edge facing
64+
* the driver station, and proceding clockwise. So sector 0 has the range (-30, 30], relative to
65+
* driver station.
66+
*
67+
* <p>Note: this code works the same for Red as Blue alliance because the Red reef center pose has
68+
* a rotational component that sets the frame of reference for the pose subtraction. This is why a
69+
* pose in sector 1 of the Red reef does not appear to be in sector 4.
70+
*
71+
* @param atPose the pose for which we want to know the reef sector
72+
* @return the index of the reef sector
73+
*/
74+
public int getSector(Pose2d atPose) {
75+
var delta = centerPose.getTranslation().minus(atPose.getTranslation());
76+
int vectorAngle = (int) delta.getAngle().getDegrees();
77+
int rayAngle = (vectorAngle + 30 + 360 + (int) centerPose.getRotation().getDegrees()) % 360;
78+
SmartDashboard.putNumber("Sector delta X", delta.getX());
79+
SmartDashboard.putNumber("Sector delta Y", delta.getY());
80+
SmartDashboard.putNumber("Sector vector delta", vectorAngle);
81+
SmartDashboard.putNumber("Sector ray angle", rayAngle);
82+
return rayAngle / 60;
83+
}
84+
85+
/**
86+
* Get the face on this reef into whose sector the specified pose falls.
87+
*
88+
* @param atPose the pose for which we want to know the reef sector
89+
* @return the reef face
90+
*/
91+
public Face getNearestFace(Pose2d atPose) {
92+
var sector = getSector(atPose);
93+
return Face.values()[6 * ordinal() + sector];
94+
}
95+
96+
/**
97+
* Enumerates all of the Reef faces on the field. Each face is named for its alliance color and
98+
* the two letters corresponding to its coral pipes as desccribed in GM 5.3
99+
*
100+
* <p>Methods are provided to get poses along the face corresponding to the center, left coral
101+
* pipe, and right coral pipe.
102+
*/
103+
public enum Face {
104+
blueAB(Blue, 0),
105+
blueCD(Blue, 1),
106+
blueEF(Blue, 2),
107+
blueGH(Blue, 3),
108+
blueIJ(Blue, 4),
109+
blueKL(Blue, 5),
110+
redAB(Red, 0),
111+
redCD(Red, 1),
112+
redEF(Red, 2),
113+
redGH(Red, 3),
114+
redIJ(Red, 4),
115+
redKL(Red, 5);
116+
117+
private final Reef reef;
118+
private Pose2d centerPose;
119+
private Pose2d leftPipePose;
120+
private Pose2d rightPipePose;
121+
122+
Face(Reef reef, int index) {
123+
this.reef = reef;
124+
var rotation = new Rotation2d(Degrees.of(60.0)).times(index);
125+
var translation = new Translation2d(radius.in(Meters), rotation.plus(Rotation2d.kPi));
126+
var offset = new Transform2d(translation, rotation);
127+
centerPose = reef.getCenterPose().plus(offset);
128+
leftPipePose =
129+
centerPose.transformBy(
130+
new Transform2d(
131+
new Translation2d(Meters.of(0), pipeSpacing.div(2)), Rotation2d.kZero));
132+
rightPipePose =
133+
centerPose.transformBy(
134+
new Transform2d(
135+
new Translation2d(Meters.of(0), pipeSpacing.div(-2)), Rotation2d.kZero));
136+
137+
SmartDashboard.putString("ReefFace." + this.toString(), centerPose.toString());
138+
}
139+
140+
/**
141+
* Gets the reef this face belongs to.
142+
*
143+
* @return the reef this face belongs to
144+
*/
145+
public Reef getReef() {
146+
return reef;
147+
}
148+
149+
/**
150+
* Gets the center pose of this face.
151+
*
152+
* @return the center pose of this face
153+
*/
154+
public Pose2d getCenterPose() {
155+
return centerPose;
156+
}
157+
158+
/**
159+
* Gets the left pipe pose of this face.
160+
*
161+
* @return the left pipe pose of this face
162+
*/
163+
public Pose2d getLeftPipePose() {
164+
return leftPipePose;
165+
}
166+
167+
/**
168+
* Gets the right pipe pose of this face.
169+
*
170+
* @return the right pipe pose of this face
171+
*/
172+
public Pose2d getRightPipePose() {
173+
return rightPipePose;
174+
}
175+
}
176+
}

‎src/main/java/frc/lib/AutoOption.java

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
import edu.wpi.first.wpilibj.util.Color;
66
import edu.wpi.first.wpilibj2.command.Command;
77
import frc.robot.auto.AutoMode;
8-
import frc.util.Util;
98
import java.util.Optional;
109

1110
public class AutoOption {

‎src/main/java/frc/lib/AutoSelector.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -127,12 +127,12 @@ public void disabledPeriodic() {
127127
SmartDashboard.putString("SelectedAutonomousMode", ao.getName());
128128
ao.getInitialPose()
129129
.ifPresentOrElse(
130-
initialPosePublisher::set, () -> initialPosePublisher.set(new Pose2d()));
130+
initialPosePublisher::set, () -> initialPosePublisher.set(Pose2d.kZero));
131131
},
132132
() -> {
133133
SmartDashboard.putString(
134134
"SelectedAutonomousMode", "None; no auto mode assigned to this slot");
135-
initialPosePublisher.set(new Pose2d());
135+
initialPosePublisher.set(Pose2d.kZero);
136136
});
137137
}
138138

‎src/main/java/frc/util/Util.java renamed to ‎src/main/java/frc/lib/Util.java

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
package frc.util;
1+
package frc.lib;
22

33
import edu.wpi.first.wpilibj.DriverStation.Alliance;
44
import edu.wpi.first.wpilibj.util.Color;
@@ -15,6 +15,6 @@ public static Color allianceToColor(Alliance alliance) {
1515
}
1616

1717
public static boolean nearlyEqual(double a, double b) {
18-
return Math.abs(a - b) < 1e-6;
18+
return Math.abs(a - b) < Math.ulp(1);
1919
}
2020
}

‎src/main/java/frc/robot/Constants.java

Lines changed: 20 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66
import edu.wpi.first.math.VecBuilder;
77
import edu.wpi.first.math.geometry.Pose2d;
88
import edu.wpi.first.math.geometry.Rotation2d;
9-
import edu.wpi.first.math.geometry.Transform2d;
109
import edu.wpi.first.math.geometry.Translation2d;
1110
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
1211
import edu.wpi.first.math.numbers.N1;
@@ -18,6 +17,8 @@
1817
import edu.wpi.first.wpilibj.Filesystem;
1918
import edu.wpi.first.wpilibj.TimedRobot;
2019
import edu.wpi.first.wpilibj.util.Color;
20+
import frc.game.FeederStation;
21+
import frc.game.Reef;
2122

2223
public final class Constants {
2324

@@ -105,69 +106,25 @@ public static final class AbsoluteEncoders {
105106
new Translation2d(kWheelBase.times(-0.5), kTrackWidth.times(-0.5)) // rear right
106107
);
107108

108-
public static final Pose2d blueReefCenter =
109-
new Pose2d(Inches.of(176.75), Inches.of(158.5), new Rotation2d());
110-
111-
public static final Pose2d redReefCenter =
112-
new Pose2d(Inches.of(514.125), Inches.of(158.5), new Rotation2d(Math.PI));
113-
114-
private static double radius = Inches.of(50.25).in(Meters);
115-
private static Rotation2d increment = new Rotation2d(Degrees.of(60.0));
116-
117-
private static Pose2d blueAB = blueReefCenter.plus(offset(0));
118-
private static Pose2d blueCD = blueReefCenter.plus(offset(1));
119-
private static Pose2d blueEF = blueReefCenter.plus(offset(2));
120-
private static Pose2d blueGH = blueReefCenter.plus(offset(3));
121-
private static Pose2d blueIJ = blueReefCenter.plus(offset(4));
122-
private static Pose2d blueKL = blueReefCenter.plus(offset(5));
123-
124-
private static Pose2d redAB = redReefCenter.plus(offset(0));
125-
private static Pose2d redCD = redReefCenter.plus(offset(1));
126-
private static Pose2d redEF = redReefCenter.plus(offset(2));
127-
private static Pose2d redGH = redReefCenter.plus(offset(3));
128-
private static Pose2d redIJ = redReefCenter.plus(offset(4));
129-
private static Pose2d redKL = redReefCenter.plus(offset(5));
130-
131-
private static Transform2d feederStationOffset =
132-
new Transform2d(Inches.of(-18), Inches.of(0), new Rotation2d(0));
133-
private static Pose2d blueRightFeeder =
134-
new Pose2d(Inches.of(33.51), Inches.of(25.80), new Rotation2d(Degrees.of(54 + 180)))
135-
.plus(feederStationOffset);
136-
private static Pose2d blueLeftFeeder =
137-
new Pose2d(Inches.of(33.51), Inches.of(291.20), new Rotation2d(Degrees.of(306 + 180)))
138-
.plus(feederStationOffset);
139-
private static Pose2d redRightFeeder =
140-
new Pose2d(Inches.of(657.37), Inches.of(291.20), new Rotation2d(Degrees.of(234 + 180)))
141-
.plus(feederStationOffset);
142-
private static Pose2d redLeftFeeder =
143-
new Pose2d(Inches.of(657.37), Inches.of(25.8), new Rotation2d(Degrees.of(126 + 180)))
144-
.plus(feederStationOffset);
145-
146-
private static Transform2d offset(double multiplier) {
147-
Rotation2d rotation = increment.times(multiplier);
148-
Translation2d translation = new Translation2d(radius, rotation.plus(new Rotation2d(Math.PI)));
149-
return new Transform2d(translation, rotation);
150-
}
151-
152109
public static final Pose2d[] kReefTargetPoses = {
153110
new Pose2d(1.0, 3.0, Rotation2d.fromDegrees(0.0)),
154111
new Pose2d(1.0, 5.0, Rotation2d.fromDegrees(0.0)),
155-
blueAB,
156-
blueCD,
157-
blueEF,
158-
blueGH,
159-
blueIJ,
160-
blueKL,
161-
redAB,
162-
redCD,
163-
redEF,
164-
redGH,
165-
redIJ,
166-
redKL,
167-
blueRightFeeder,
168-
blueLeftFeeder,
169-
redRightFeeder,
170-
redLeftFeeder
112+
Reef.Face.blueAB.getCenterPose(),
113+
Reef.Face.blueCD.getCenterPose(),
114+
Reef.Face.blueEF.getCenterPose(),
115+
Reef.Face.blueGH.getCenterPose(),
116+
Reef.Face.blueIJ.getCenterPose(),
117+
Reef.Face.blueKL.getCenterPose(),
118+
Reef.Face.redAB.getCenterPose(),
119+
Reef.Face.redCD.getCenterPose(),
120+
Reef.Face.redEF.getCenterPose(),
121+
Reef.Face.redGH.getCenterPose(),
122+
Reef.Face.redIJ.getCenterPose(),
123+
Reef.Face.redKL.getCenterPose(),
124+
FeederStation.blueRight.getPose(),
125+
FeederStation.blueLeft.getPose(),
126+
FeederStation.redRight.getPose(),
127+
FeederStation.redLeft.getPose()
171128
};
172129
}
173130

@@ -186,10 +143,10 @@ public static final class TurningControllerGains {
186143
}
187144

188145
public static final class DriveToPoseControllerGains {
189-
public static final double kTraP = 1.0;
146+
public static final double kTraP = 2.0;
190147
public static final double kTraI = 0.0;
191148
public static final double kTraD = 0.0;
192-
public static final double kRotP = 1.5;
149+
public static final double kRotP = 3.0;
193150
public static final double kRotI = 0.0;
194151
public static final double kRotD = 0.0;
195152
}

‎src/main/java/frc/robot/LEDs/LEDs.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,9 @@
1414
import edu.wpi.first.wpilibj.util.Color;
1515
import edu.wpi.first.wpilibj2.command.Command;
1616
import edu.wpi.first.wpilibj2.command.SubsystemBase;
17+
import frc.game.Gamepiece;
1718
import frc.lib.AutoOption;
1819
import frc.robot.Constants.LedConstants;
19-
import frc.util.Gamepiece;
2020
import java.util.Arrays;
2121
import java.util.Optional;
2222
import java.util.function.BooleanSupplier;

0 commit comments

Comments
 (0)