Skip to content

Commit

Permalink
Interpolated vision (#69)
Browse files Browse the repository at this point in the history 
* work on implement two limelight interpolate vision

* double limelight interpolated vision

* format with spotless

* Remove accidental committed file

* implement interpolated vision

---------

Co-authored-by: owenrobotics581 <[email protected]>
Co-authored-by: Jordan <[email protected]>
  • Loading branch information
@Owen756 @jordanjcoderman
3 people authored Oct 19, 2024
1 parent 973fa46 commit 95d9a96
Show file tree
Hide file tree
Showing 5 changed files with 94 additions and 84 deletions.
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@
import frc.robot.util.scheduling.LifecycleSubsystemManager;
import frc.robot.vision.Limelight;
import frc.robot.vision.VisionSubsystem;
import frc.robot.vision.interpolation.CameraDataset;
import frc.robot.vision.interpolation.InterpolatedVisionDataset;

public class Robot extends TimedRobot {
private Command autonomousCommand;
Expand All @@ -39,8 +41,8 @@ public class Robot extends TimedRobot {
new IntakeSubsystem(hardware.intakeMain, hardware.intakeCenteringMotor);
private final SwerveSubsystem swerve = new SwerveSubsystem();
private final ImuSubsystem imu = new ImuSubsystem(swerve.drivetrainPigeon);
private final Limelight leftLimelight = new Limelight("left");
private final Limelight rightLimelight = new Limelight("right");
private final Limelight leftLimelight = new Limelight("left", InterpolatedVisionDataset.HOME.leftSet);
private final Limelight rightLimelight = new Limelight("right", InterpolatedVisionDataset.HOME.rightSet);

private final VisionSubsystem vision = new VisionSubsystem(imu, leftLimelight, rightLimelight);
private final LocalizationSubsystem localization = new LocalizationSubsystem(imu, vision, swerve);
Expand Down
12 changes: 7 additions & 5 deletions src/main/java/frc/robot/vision/Limelight.java
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,23 @@
package frc.robot.vision;

import edu.wpi.first.math.geometry.Pose2d;
import frc.robot.vision.LimelightHelpers.LimelightResults;
import frc.robot.vision.interpolation.CameraDataset;
import frc.robot.vision.interpolation.InterpolatedVision;

import java.util.Optional;

import dev.doglog.DogLog;

public class Limelight {
private Optional<VisionResult> rawVisionResult;
private Optional<VisionResult> processedVisionResult;
private final String limelightTableName;
private final String name;
private CameraDataset interpolationData;
;

public Limelight(String name) {
public Limelight(String name, CameraDataset interpolationData) {
limelightTableName = "limelight-" + name;
this.name = name;
this.interpolationData = interpolationData;
}

public Optional<VisionResult> getInterpolatedVisionResult() {
Expand All @@ -25,7 +27,7 @@ public Optional<VisionResult> getInterpolatedVisionResult() {
return Optional.empty();
}

Pose2d interpolatedPose = InterpolatedVision.interpolatePose(rawResult.get().pose());
Pose2d interpolatedPose = InterpolatedVision.interpolatePose(rawResult.get().pose(), interpolationData);
return Optional.of(new VisionResult(interpolatedPose, rawResult.get().timestamp()));
}

Expand Down
7 changes: 7 additions & 0 deletions src/main/java/frc/robot/vision/interpolation/CameraDataset.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
package frc.robot.vision.interpolation;

import java.util.List;

/** A interpolated vision data set for a specific camera. */
public record CameraDataset(
List<VisionInterpolationData> red, List<VisionInterpolationData> blue) {}
11 changes: 2 additions & 9 deletions src/main/java/frc/robot/vision/interpolation/InterpolatedVision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,15 +5,8 @@
import frc.robot.fms.FmsSubsystem;

public class InterpolatedVision {
private static final InterpolatedVisionDataset usedSet =
RobotConfig.get().vision().interpolatedVisionSet();

/**
* @param visionInput - pose from the limelight
* @return a transformed pose that can be added to the pose estimator
*/
public static Pose2d interpolatePose(Pose2d visionInput) {
var usedDataPoints = FmsSubsystem.isRedAlliance() ? usedSet.redSet : usedSet.blueSet;
public static Pose2d interpolatePose(Pose2d visionInput, CameraDataset dataset) {
var usedDataPoints = FmsSubsystem.isRedAlliance() ? dataset.red() : dataset.blue();

return new Pose2d(
InterpolationUtil.interpolateTranslation(usedDataPoints, visionInput.getTranslation()),
Expand Down
142 changes: 74 additions & 68 deletions src/main/java/frc/robot/vision/interpolation/InterpolatedVisionDataset.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,79 +3,85 @@
import edu.wpi.first.math.geometry.Translation2d;
import java.util.List;

/** A interpolated vision data set for each field, for all cameras. */
public enum InterpolatedVisionDataset {
// ALL POINTS ARE NOT TESTED OR VALIDATED
HOME(
List.of(
new VisionInterpolationData(
new Translation2d(15.2245, 5.522), new Translation2d(15.194, 5.634), "SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(13.0745, 5.522),
new Translation2d(13.125, 5.722),
"PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(11.059, 6.842),
new Translation2d(11.16, 6.845),
"WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(13.799, 4.202),
new Translation2d(13.905, 4.361),
"FRONT_PODIUM_MIDDLE")),
List.of()),
new CameraDataset(
List.of(
new VisionInterpolationData(
new Translation2d(15.2245, 5.522), new Translation2d(15.194, 5.634), "SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(13.0745, 5.522),
new Translation2d(13.125, 5.722),
"PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(11.059, 6.842),
new Translation2d(11.16, 6.845),
"WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(13.799, 4.202),
new Translation2d(13.905, 4.361),
"FRONT_PODIUM_MIDDLE")),
List.of()),
new CameraDataset(
List.of(
new VisionInterpolationData(
new Translation2d(15.2245, 5.522), new Translation2d(15.194, 5.634), "SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(13.0745, 5.522),
new Translation2d(13.125, 5.722),
"PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(11.059, 6.842),
new Translation2d(11.16, 6.845),
"WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(13.799, 4.202),
new Translation2d(13.905, 4.361),
"FRONT_PODIUM_MIDDLE")),
List.of())),
BELLARMINE(
List.of(
new VisionInterpolationData(
new Translation2d(15.2245, 5.522), new Translation2d(15.125, 5.581), "SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(13.0745, 5.522),
new Translation2d(13.103, 5.560),
"PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(11.059, 6.842),
new Translation2d(11.18, 6.932),
"WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(13.799, 4.202),
new Translation2d(13.67, 4.106),
"FRONT_PODIUM_MIDDLE")),
List.of()),
CHEZY_CHAMPS(
List.of(
new VisionInterpolationData(
new Translation2d(15.2245, 5.522), new Translation2d(15.215, 5.553), "RED_SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(13.0745, 5.522),
new Translation2d(13.351, 5.572),
"RED_PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(11.059, 6.842),
new Translation2d(11.42, 6.958),
"RED_WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(13.799, 4.202),
new Translation2d(13.929, 4.152),
"RED_FRONT_PODIUM_MIDDLE")),
List.of(
new VisionInterpolationData(
new Translation2d(1.315, 5.522), new Translation2d(1.34, 5.51), "BLUE_SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(3.465, 5.522),
new Translation2d(3.295, 5.35),
"BLUE_PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(5.481, 6.842),
new Translation2d(5.256, 6.751),
"BLUE_WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(2.741, 4.202),
new Translation2d(2.630, 3.974),
"BLUE_FRONT_PODIUM_MIDDLE")));
new CameraDataset(
List.of(
new VisionInterpolationData(
new Translation2d(15.2245, 5.522), new Translation2d(15.125, 5.581), "SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(13.0745, 5.522),
new Translation2d(13.103, 5.560),
"PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(11.059, 6.842),
new Translation2d(11.18, 6.932),
"WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(13.799, 4.202),
new Translation2d(13.67, 4.106),
"FRONT_PODIUM_MIDDLE")),
List.of()),
new CameraDataset(
List.of(
new VisionInterpolationData(
new Translation2d(15.2245, 5.522), new Translation2d(15.125, 5.581), "SUBWOOFER"),
new VisionInterpolationData(
new Translation2d(13.0745, 5.522),
new Translation2d(13.103, 5.560),
"PODIUM_SPEAKER_INTERSECTION"),
new VisionInterpolationData(
new Translation2d(11.059, 6.842),
new Translation2d(11.18, 6.932),
"WING_LINE_MIDDLE"),
new VisionInterpolationData(
new Translation2d(13.799, 4.202),
new Translation2d(13.67, 4.106),
"FRONT_PODIUM_MIDDLE")),
List.of()));

public final List<VisionInterpolationData> redSet;
public final List<VisionInterpolationData> blueSet;
public final CameraDataset leftSet;
public final CameraDataset rightSet;

InterpolatedVisionDataset(List<VisionInterpolationData> red, List<VisionInterpolationData> blue) {
this.redSet = red;
this.blueSet = blue;
InterpolatedVisionDataset(CameraDataset left, CameraDataset right) {
this.leftSet = left;
this.rightSet = right;
}
}

0 comments on commit 95d9a96

Please sign in to comment.