-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* feat: notevision io classes * feat: add note vision subsystem * fix: correct target area units * fix: use rotation3ds for returning gamepiece positions * fix: make limelight name shorter
- Loading branch information
1 parent
377ceba
commit 0a41223
Showing
14 changed files
with
171 additions
and
45 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
2 changes: 1 addition & 1 deletion
2
...4/subsystems/vision/AprilTagVisionIO.java → ...ems/vision/apriltag/AprilTagVisionIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
2 changes: 1 addition & 1 deletion
2
...ems/vision/AprilTagVisionIOLimelight.java → ...n/apriltag/AprilTagVisionIOLimelight.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
2 changes: 1 addition & 1 deletion
2
...tems/vision/AprilTagVisionIOMegaTag2.java → ...on/apriltag/AprilTagVisionIOMegaTag2.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
48 changes: 48 additions & 0 deletions
48
src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,48 @@ | ||
| package org.team1540.robot2024.subsystems.vision.gamepiece; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import org.team1540.robot2024.util.vision.GamepieceDetection; | ||
|
|
||
| import static org.team1540.robot2024.Constants.Vision.Gamepiece.*; | ||
|
|
||
| public class NoteVision extends SubsystemBase { | ||
| private final NoteVisionIO io; | ||
| private final NoteVisionIOInputsAutoLogged inputs = new NoteVisionIOInputsAutoLogged(); | ||
|
|
||
| private static boolean hasInstance = false; | ||
|
|
||
| private NoteVision(NoteVisionIO io) { | ||
| if (hasInstance) throw new IllegalStateException("Instance of NoteVision already exists"); | ||
| this.io = io; | ||
| hasInstance = true; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| io.updateInputs(inputs); | ||
| } | ||
|
|
||
| public GamepieceDetection getLatestDetection() { | ||
| return new GamepieceDetection( | ||
| inputs.lastDetectionTimestampSecs, | ||
| inputs.targetRotation.rotateBy(CAMERA_POSE.getRotation().unaryMinus()), | ||
| inputs.targetArea, | ||
| inputs.targetClass); | ||
| } | ||
|
|
||
| public boolean hasDetection() { | ||
| return inputs.hasDetection; | ||
| } | ||
|
|
||
| public static NoteVision createReal() { | ||
| return new NoteVision(new NoteVisionIOLimelight(CAMERA_NAME)); | ||
| } | ||
|
|
||
| public static NoteVision createSim() { | ||
| return createDummy(); | ||
| } | ||
|
|
||
| public static NoteVision createDummy() { | ||
| return new NoteVision(new NoteVisionIO() {}); | ||
| } | ||
| } |
21 changes: 21 additions & 0 deletions
21
src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,21 @@ | ||
| package org.team1540.robot2024.subsystems.vision.gamepiece; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation3d; | ||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface NoteVisionIO { | ||
| @AutoLog | ||
| class NoteVisionIOInputs { | ||
| public boolean hasDetection = false; | ||
| public double lastDetectionTimestampSecs = 0.0; | ||
| public Rotation3d targetRotation = new Rotation3d(); // Camera-relative target rotation | ||
| public double targetArea = 0.0; | ||
| public String targetClass = ""; | ||
| } | ||
|
|
||
| default void updateInputs(NoteVisionIOInputs inputs) {} | ||
|
|
||
| default String getName() { | ||
| return ""; | ||
| } | ||
| } |
41 changes: 41 additions & 0 deletions
41
src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,41 @@ | ||
| package org.team1540.robot2024.subsystems.vision.gamepiece; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation3d; | ||
| import org.team1540.robot2024.util.vision.LimelightHelpers; | ||
|
|
||
| import static org.team1540.robot2024.Constants.Vision.Gamepiece.*; | ||
|
|
||
| public class NoteVisionIOLimelight implements NoteVisionIO { | ||
| private final String name; | ||
|
|
||
| public NoteVisionIOLimelight(String name) { | ||
| this.name = name; | ||
| LimelightHelpers.setCameraMode_Processor(name); | ||
| LimelightHelpers.setLEDMode_PipelineControl(name); | ||
| LimelightHelpers.setPipelineIndex(name, PIPELINE_INDEX); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(NoteVisionIOInputs inputs) { | ||
| inputs.hasDetection = LimelightHelpers.getTV(name); | ||
| if (inputs.hasDetection) { | ||
| inputs.lastDetectionTimestampSecs = | ||
| (LimelightHelpers.getLimelightNTTableEntry(name, "tv").getLastChange() / 1000000.0) | ||
| - ((LimelightHelpers.getLatency_Capture(name) + LimelightHelpers.getLatency_Pipeline(name)) | ||
| / 1000.0); | ||
| //TODO check rotation signs | ||
| inputs.targetRotation = | ||
| new Rotation3d( | ||
| 0, | ||
| -Math.toRadians(LimelightHelpers.getTY(name)), | ||
| Math.toRadians(LimelightHelpers.getTX(name))); | ||
| inputs.targetArea = LimelightHelpers.getTA(name); | ||
| inputs.targetClass = LimelightHelpers.getNeuralClassID(name); | ||
| } | ||
| } | ||
|
|
||
| @Override | ||
| public String getName() { | ||
| return name; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
9 changes: 9 additions & 0 deletions
9
src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,9 @@ | ||
| package org.team1540.robot2024.util.vision; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation3d; | ||
|
|
||
| public record GamepieceDetection( | ||
| double timestampSecs, | ||
| Rotation3d rotation, // Robot relative gamepiece rotation | ||
| double area, | ||
| String targetClass) {} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters