-
Notifications
You must be signed in to change notification settings - Fork 12
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #17 from acuaroo/main
Added vision to odometry
- Loading branch information
Showing
6 changed files
with
171 additions
and
41 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,20 @@ | ||
| package frc.robot.constants; | ||
|
|
||
| import edu.wpi.first.math.trajectory.TrapezoidProfile; | ||
|
|
||
| public class VisionConstants { | ||
| public static final double alignXP = 0.9; | ||
| public static final double alignXI = 0; | ||
| public static final double alignXD = 0; | ||
| public static final TrapezoidProfile.Constraints alignXConstraints = new TrapezoidProfile.Constraints(0.75, 0.5); | ||
|
|
||
| public static final double alignZP = 0.6; | ||
| public static final double alignZI = 0; | ||
| public static final double alignZD = 0; | ||
| public static final TrapezoidProfile.Constraints alignZConstraints = new TrapezoidProfile.Constraints(0.75, 0.5); | ||
|
|
||
| public static final double alignRotationP = 0.6; | ||
| public static final double alignRotationI = 0; | ||
| public static final double alignRotationD = 0; | ||
| public static final TrapezoidProfile.Constraints alignRotationConstraints = new TrapezoidProfile.Constraints(0.75, 0.5); | ||
| } |
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,70 @@ | ||
| package frc.robot.utils; | ||
|
|
||
| import edu.wpi.first.math.geometry.*; | ||
|
|
||
| import edu.wpi.first.networktables.NetworkTableInstance; | ||
|
|
||
| import edu.wpi.first.wpilibj.DriverStation; | ||
|
|
||
| public class VisionUtils { | ||
| /** | ||
| * Gets the requested entry from as well as optionally adding the alliance. Used internally | ||
| * @param pose The pose you want as defined by https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data | ||
| * @param useAlliance Whether to add on the alliance | ||
| * @return The pose from the limelight | ||
| */ | ||
| private static Pose3d getPose(String pose, boolean useAlliance) { | ||
| String suffix = (useAlliance && DriverStation.getAlliance().isPresent()) ? | ||
| ((DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) ? "_wpiblue" : "_wpired") : ""; | ||
|
|
||
| double[] returnedPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry(pose + suffix).getDoubleArray(new double[0]); | ||
| if (returnedPose.length == 0) return new Pose3d(); | ||
|
|
||
| return new Pose3d( | ||
| new Translation3d(returnedPose[0], returnedPose[1], returnedPose[2]), | ||
| new Rotation3d(0.0, 0.0, Math.toRadians(returnedPose[5])) | ||
| ); | ||
| } | ||
|
|
||
| /** | ||
| * Gets the bots position relative to the field. Used by odometry | ||
| * @return The bots position | ||
| * */ | ||
| public static Pose3d getBotPoseFieldSpace() { | ||
| return getPose("botpose", true); | ||
| } | ||
|
|
||
| /** | ||
| * Gets the bots position relative to the target. Used by AlignCommand | ||
| * @return The bots position | ||
| * */ | ||
| public static Pose3d getBotPoseTargetSpace() { | ||
| return getPose("botpose_targetspace", false); | ||
| } | ||
|
|
||
| /** | ||
| * Gets the distance from the first visible AprilTag | ||
| * @return The distance (in meters) from the AprilTag | ||
| */ | ||
| public static double getDistanceFromTag() { | ||
| Pose3d returnedPose = getBotPoseTargetSpace(); | ||
|
|
||
| return -returnedPose.getZ(); | ||
| } | ||
|
|
||
| /** | ||
| * Gets the latency from the pipeline | ||
| * @return The latency | ||
| */ | ||
| public static double getLatencyPipeline() { | ||
| return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tl").getDouble(0.0); | ||
| } | ||
|
|
||
| /** | ||
| * Gets the latency from capturing | ||
| * @return The latency | ||
| */ | ||
| public static double getLatencyCapture() { | ||
| return NetworkTableInstance.getDefault().getTable("limelight").getEntry("cl").getDouble(0.0); | ||
| } | ||
| } |
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