Skip to content

Commit c58988e

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #10 from strykeforce/tag-servo
Tag Servo Subsystem Initial
2 parents 51bbc7e + e7586d0 commit c58988e

File tree

4 files changed

+319
-21
lines changed

4 files changed

+319
-21
lines changed

‎src/main/java/frc/robot/constants/TagServoingConstants.java‎

Lines changed: 40 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,44 @@
11
package frc.robot.constants;
22

3+
import edu.wpi.first.math.geometry.Translation2d;
4+
import edu.wpi.first.math.util.Units;
5+
36
public class TagServoingConstants {
4-
public static final double kAngleCloseEnough = 3.0;
7+
// Cameras
8+
public static final int kLeftServoCam = 0; // Score RIGHT coral
9+
public static final int kRightServoCam = 1; // Score LEFT coral
10+
11+
// Targets
12+
public static final double kHorizontalTarget = 800;
13+
public static final double kAreaTarget = 800;
14+
15+
public static final double[] kAngleTarget = {
16+
Units.degreesToRadians(0),
17+
Units.degreesToRadians(60),
18+
Units.degreesToRadians(120),
19+
Units.degreesToRadians(180),
20+
Units.degreesToRadians(240),
21+
Units.degreesToRadians(300)
22+
};
23+
24+
// Tags
25+
public static final int[] kBlueTargetTag = {18, 17, 22, 21, 20, 19};
26+
public static final int[] kRedTargetTag = {7, 8, 9, 10, 11, 6};
27+
28+
// Tag align
29+
public static final double kAreaCloseEnough = 0;
30+
public static final double kHorizontalCloseEnough = 0;
31+
public static final double kAngleCloseEnough = Units.degreesToRadians(3.0);
32+
33+
// Drive
34+
public static final double kInitialDriveRadius = 1.5;
35+
public static final double kStopXDriveRadius = 1.2; // Should be closer to reef than target pose
36+
public static final double kDriveCloseEnough = 0.3;
37+
38+
// Reef
39+
public static final Translation2d kBlueReefPose =
40+
new Translation2d(Units.inchesToMeters(223.5), Units.inchesToMeters(158.5));
41+
42+
public static final Translation2d kRedReefPose =
43+
kBlueReefPose.plus(new Translation2d(Units.inchesToMeters(337.39), 0));
544
}

‎src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -103,7 +103,7 @@ public Alliance getAllianceColor() {
103103
}
104104

105105
public ScoringLevel getAlgaeLevel() {
106-
return tagAlignSubsystem.getHexant() % 2 == 0 ? ScoringLevel.L3 : ScoringLevel.L2;
106+
return tagAlignSubsystem.computeHexant() % 2 == 0 ? ScoringLevel.L3 : ScoringLevel.L2;
107107
}
108108

109109
public boolean hasCoral() {
@@ -284,8 +284,8 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
284284
}
285285

286286
if (drive) {
287-
tagAlignSubsystem.setup(allianceColor, scoreSide);
288-
tagAlignSubsystem.start();
287+
tagAlignSubsystem.setup(allianceColor, scoreSide == ScoreSide.LEFT);
288+
tagAlignSubsystem.start(allianceColor, scoreSide == ScoreSide.LEFT);
289289
}
290290
}
291291

‎src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java‎

Lines changed: 265 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,31 +1,280 @@
11
package frc.robot.subsystems.tagAlign;
22

3+
import WallEye.Point;
4+
import WallEye.WallEyeTagResult;
5+
import edu.wpi.first.math.controller.ProfiledPIDController;
6+
import edu.wpi.first.math.geometry.Pose2d;
7+
import edu.wpi.first.math.geometry.Rotation2d;
8+
import edu.wpi.first.math.geometry.Translation2d;
9+
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
10+
import edu.wpi.first.math.util.Units;
311
import edu.wpi.first.wpilibj.DriverStation.Alliance;
4-
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
12+
import frc.robot.constants.TagServoingConstants;
13+
import frc.robot.subsystems.drive.DriveSubsystem;
14+
import frc.robot.subsystems.vision.VisionSubsystem;
15+
import java.util.Set;
16+
import net.jafama.FastMath;
17+
import org.littletonrobotics.junction.Logger;
18+
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
19+
import org.strykeforce.telemetry.measurable.Measure;
520

6-
public class TagAlignSubsystem {
21+
public class TagAlignSubsystem extends MeasurableSubsystem {
22+
private DriveSubsystem driveSubsystem;
23+
private VisionSubsystem visionSubsystem;
724

8-
public enum TagAlignStates {
9-
DONE
25+
private ProfiledPIDController driveX;
26+
private ProfiledPIDController driveY;
27+
private ProfiledPIDController driveOmega;
28+
29+
private ProfiledPIDController alignX;
30+
private ProfiledPIDController alignY;
31+
private ProfiledPIDController alignOmega;
32+
33+
private TagAlignStates curState = TagAlignStates.DONE;
34+
35+
// Set by start()
36+
private Pose2d targetPose;
37+
private int targetCamId;
38+
private int targetTagId;
39+
private int fieldRelHexant;
40+
41+
public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) {
42+
this.driveSubsystem = driveSubsystem;
43+
this.visionSubsystem = visionSubsystem;
44+
45+
// FIXME: need sane constants
46+
this.driveX = new ProfiledPIDController(0.0019, 0, 0, new Constraints(3.0, 3.0));
47+
this.driveY = new ProfiledPIDController(0.00001, 0, 0, new Constraints(2.0, 1.0));
48+
this.driveOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0));
49+
this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180));
50+
51+
this.alignX = new ProfiledPIDController(0.0019, 0, 0, new Constraints(3.0, 3.0));
52+
this.alignY = new ProfiledPIDController(0.00001, 0, 0, new Constraints(2.0, 1.0));
53+
this.alignOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0));
54+
this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180));
55+
56+
Logger.recordOutput("TagAlignSubsystem/TargetArea", -1);
57+
Logger.recordOutput("TagAlignSubsystem/TargetTag", -1);
58+
Logger.recordOutput("TagAlignSubsystem/TargetCenterX", -1);
59+
}
60+
61+
// FIXME: uncomment when implemented
62+
public int computeHexant(/* Alliance color */ ) {
63+
Translation2d reefT = /* color == Alliance.Blue ? TagServoingConstants.kBlueReefPose : */
64+
TagServoingConstants.kRedReefPose;
65+
double offset = Units.degreesToRadians(30);
66+
67+
return (((int)
68+
(FastMath.normalizeZeroTwoPi(
69+
driveSubsystem
70+
.getPoseMeters()
71+
.getTranslation()
72+
.minus(reefT)
73+
.getAngle()
74+
.getRadians()
75+
- offset)
76+
/ Units.degreesToRadians(60)
77+
+ offset))
78+
+ 3)
79+
% 6;
80+
}
81+
82+
// Red reef numbered like blue (red 0 is facing the same direction as blue 0)
83+
private int computeFieldRelHexant(/* Alliance color */ ) {
84+
return (computeHexant(/* color */ ) + /* color == Alliance.Blue ? 0 : */ 3) % 6;
85+
}
86+
87+
private Pose2d getTargetDrivePose(/* Alliance color */ ) {
88+
Translation2d reefT = /* color == Alliance.Blue ? TagServoingConstants.kBlueReefPose : */
89+
TagServoingConstants.kRedReefPose;
90+
91+
Translation2d offset =
92+
new Translation2d(
93+
TagServoingConstants.kInitialDriveRadius,
94+
Rotation2d.fromDegrees(computeFieldRelHexant() * 60));
95+
96+
return new Pose2d(reefT.plus(offset), Rotation2d.fromDegrees(computeFieldRelHexant() * 60));
97+
}
98+
99+
private double getCurRadius() {
100+
Translation2d reefT = /* color == Alliance.Blue ? TagServoingConstants.kBlueReefPose : */
101+
TagServoingConstants.kRedReefPose;
102+
return driveSubsystem.getPoseMeters().getTranslation().minus(reefT).getNorm();
103+
}
104+
105+
public TagAlignStates getState() {
106+
return curState;
107+
}
108+
109+
public void setup(Alliance alliance, boolean scoreLeft) {
110+
targetPose = getTargetDrivePose(/* color */ );
111+
112+
// Inverted, scoring left coral means aligning right camera
113+
targetCamId =
114+
!scoreLeft ? TagServoingConstants.kLeftServoCam : TagServoingConstants.kRightServoCam;
115+
targetTagId = /*
116+
* color == Alliance.Blue ? TagServoingConstants.kRedTargetTag[computeHexant()]
117+
* :
118+
*/ TagServoingConstants.kRedTargetTag[computeHexant()];
119+
120+
fieldRelHexant = computeFieldRelHexant();
121+
122+
Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId);
123+
124+
Pose2d current = driveSubsystem.getPoseMeters();
125+
126+
driveX.reset(current.getX());
127+
driveY.reset(current.getY());
128+
driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
129+
130+
alignX.reset(current.getX());
131+
alignY.reset(current.getY());
132+
alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
133+
}
134+
135+
// CALL setup() first!!
136+
// Do not use in internal state machine
137+
public double calculateAlignY() {
138+
WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId);
139+
140+
int tagIndex = -1;
141+
int[] tags = result.getTagIDs();
142+
143+
for (int i = 0; i < tags.length; i++) {
144+
if (tags[i] == targetTagId) {
145+
tagIndex = i;
146+
break;
147+
}
148+
}
149+
150+
if (tagIndex == -1) {
151+
// Target tag not found
152+
return 2767;
153+
}
154+
155+
Point center = result.getTagCenters().get(tagIndex);
156+
157+
Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x());
158+
159+
double vY = alignY.calculate(center.x(), TagServoingConstants.kHorizontalTarget);
160+
161+
return vY;
10162
}
11163

12-
public Object getState() {
13-
// TODO Auto-generated method stub
14-
throw new UnsupportedOperationException("Unimplemented method 'getState'");
164+
public void start(Alliance alliance, boolean scoreLeft) {
165+
setup(alliance, scoreLeft);
166+
167+
curState = TagAlignStates.DRIVE;
15168
}
16169

17-
public int getHexant() {
18-
// TODO Auto-generated method stub
19-
throw new UnsupportedOperationException("Unimplemented method 'getHexant'");
170+
public void terminate() {
171+
driveSubsystem.move(0, 0, 0, false);
172+
curState = TagAlignStates.DONE;
20173
}
21174

22-
public void setup(Alliance allianceColor, ScoreSide scoreSide) {
23-
// TODO Auto-generated method stub
24-
throw new UnsupportedOperationException("Unimplemented method 'setup'");
175+
@Override
176+
public void periodic() {
177+
Logger.recordOutput("TagAlignSubsystem/State", curState.toString());
178+
179+
switch (curState) {
180+
case DRIVE -> {
181+
Pose2d current = driveSubsystem.getPoseMeters();
182+
183+
double vX = driveX.calculate(current.getX(), targetPose.getX());
184+
double vY = driveY.calculate(current.getY(), targetPose.getY());
185+
double vOmega =
186+
driveX.calculate(
187+
current.getRotation().getRadians(), targetPose.getRotation().getRadians());
188+
189+
Translation2d tagRelVel =
190+
new Translation2d(vX, vY)
191+
.rotateBy(
192+
Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant]));
193+
194+
double tagRelX = tagRelVel.getX();
195+
double tagRelY = tagRelVel.getY();
196+
197+
if (getCurRadius() < TagServoingConstants.kStopXDriveRadius) {
198+
tagRelX = 0;
199+
}
200+
201+
if (Math.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough
202+
&& targetPose.minus(current).getTranslation().getNorm()
203+
< TagServoingConstants.kDriveCloseEnough) {
204+
alignX.reset(current.getX());
205+
alignY.reset(current.getY());
206+
alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
207+
208+
curState = TagAlignStates.TAG_ALIGN;
209+
break;
210+
}
211+
212+
Translation2d adjusted =
213+
new Translation2d(tagRelX, tagRelY)
214+
.rotateBy(
215+
Rotation2d.fromRadians(TagServoingConstants.kAngleTarget[fieldRelHexant]));
216+
217+
driveSubsystem.move(adjusted.getX(), adjusted.getY(), vOmega, true);
218+
}
219+
220+
case TAG_ALIGN -> {
221+
WallEyeTagResult result = (WallEyeTagResult) visionSubsystem.getLastResult(targetCamId);
222+
223+
int tagIndex = -1;
224+
int[] tags = result.getTagIDs();
225+
226+
for (int i = 0; i < tags.length; i++) {
227+
if (tags[i] == targetTagId) {
228+
tagIndex = i;
229+
break;
230+
}
231+
}
232+
233+
if (tagIndex == -1) {
234+
return;
235+
}
236+
237+
Point center = result.getTagCenters().get(tagIndex);
238+
double area = result.getTagAreas()[tagIndex];
239+
240+
Logger.recordOutput("TagAlignSubsystem/TargetArea", area);
241+
Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x());
242+
243+
double vX = alignX.calculate(area, TagServoingConstants.kAreaTarget);
244+
double vY = alignY.calculate(center.x(), TagServoingConstants.kHorizontalTarget);
245+
246+
if (TagServoingConstants.kAreaTarget - area < TagServoingConstants.kAngleCloseEnough
247+
|| area > TagServoingConstants.kAreaTarget) {
248+
vX = 0;
249+
250+
if (Math.abs(TagServoingConstants.kHorizontalTarget - center.x())
251+
< TagServoingConstants.kHorizontalCloseEnough) {
252+
253+
terminate();
254+
break;
255+
}
256+
}
257+
258+
double vOmega =
259+
alignOmega.calculate(
260+
driveSubsystem.getGyroRotation2d().getRadians(),
261+
targetPose.getRotation().getRadians());
262+
263+
driveSubsystem.move(vX, vY, vOmega, false);
264+
}
265+
266+
case DONE -> {}
267+
}
25268
}
26269

27-
public void start() {
28-
// TODO Auto-generated method stub
29-
throw new UnsupportedOperationException("Unimplemented method 'start'");
270+
@Override
271+
public Set<Measure> getMeasures() {
272+
return Set.of(new Measure("State", () -> curState.ordinal()));
273+
}
274+
275+
public enum TagAlignStates {
276+
DRIVE,
277+
TAG_ALIGN,
278+
DONE
30279
}
31280
}

‎src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,13 @@
11
package frc.robot.subsystems.vision;
22

3-
public class VisionSubsystem {}
3+
import WallEye.WallEyeTagResult;
4+
5+
public class VisionSubsystem {
6+
public VisionSubsystem() {}
7+
8+
// FIXME: see walleye-testing branch from crescendo for correct implementation but
9+
// FIXME: USE WalleyeTagResult INSTEAD OF WalleyeResult!
10+
public WallEyeTagResult getLastResult(int index) {
11+
return null;
12+
}
13+
}

0 commit comments

Comments
 (0)