Skip to content

Vision Calib & V2.5 #17

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
pix-xl merged 4 commits into from
Mar 7, 2025
Merged

Vision Calib & V2.5 #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
721f3ba
vision experiment
pix-xl Mar 6, 2025
bdca454
Merge branch 'CrevolutionRoboticsProgramming:main' into main
pix-xl Mar 6, 2025
33fbead
Merge branch 'CrevolutionRoboticsProgramming:main' into main
pix-xl Mar 7, 2025
765a541
Vision V2.5
pix-xl Mar 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 10 additions & 5 deletions src/main/java/frc/robot/vision/VisionConfig.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,20 +12,25 @@
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Unit;

public class VisionConfig {

// Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard
public static final int TOTAL_CAMERAS = 4;
public static final String[] camNames = new String[] {"Center_Cam", "Left_Cam", "Right_Cam", "Drive_Cam"};
public static final String[] camNames = new String[] {"Left_Cam", "Right_Cam"}; //TODO: add center cam and drive cam

//Camera Positions
// TODO: config camera transforms
public static final Transform3d[] robotToCamTransforms = new Transform3d[] {
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d())
//left cam
new Transform3d(
new Translation3d(Units.inchesToMeters(-12.436), Units.inchesToMeters(11.677), Units.inchesToMeters(7.413)),
new Rotation3d(0,Units.degreesToRadians(15),Units.degreesToRadians(-20))),
//right cam
new Transform3d(
new Translation3d(Units.inchesToMeters(-12.436), Units.inchesToMeters(-11.677), Units.inchesToMeters(7.413)),
new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(20)))
};

// Creates field layout for AprilTags
Expand Down
15 changes: 15 additions & 0 deletions src/main/java/frc/robot/vision/VisionConsumer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
// package frc.robot.vision;

// import edu.wpi.first.math.Matrix;
// import edu.wpi.first.math.geometry.Pose2d;
// import edu.wpi.first.math.numbers.N1;
// import edu.wpi.first.math.numbers.N3;

// @FunctionalInterface
// public interface VisionConsumer {
// public void addVisionMeasurement(
// Pose2d visionRobotPoseM,
// double timestampSeconds,
// Matrix<N3,N1> visionMeasurementStdDevs
// );
// }
43 changes: 43 additions & 0 deletions src/main/java/frc/robot/vision/commands/VisionCommand.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
// package frc.robot.vision.commands;

// import static frc.robot.vision.VisionConfig.camNames;
// import static frc.robot.vision.VisionConfig.robotToCamTransforms;

// import java.nio.channels.NetworkChannel;

// import edu.wpi.first.math.geometry.Pose2d;
// import edu.wpi.first.networktables.NetworkTableInstance;
// import edu.wpi.first.networktables.StructPublisher;
// import edu.wpi.first.wpilibj2.command.Command;
// import frc.robot.vision.Vision;
// import frc.robot.vision.VisionConsumer;

// public class VisionCommand extends Command {
// private final Vision[] visions;
// private final VisionConsumer visionConsumer;

// public VisionCommand(VisionConsumer consumer){
// visions = new Vision[camNames.length];
// for (int i = 0; i < camNames.length; i++){
// visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
// }
// this.visionConsumer = consumer;
// }

// @Override
// public void execute(){
// for (int i = 0; i < visions.length; i++){
// var vision = visions[i];
// var visionEst = vision.getEstimatedGlobalPose();
// visionEst.ifPresent(est -> {
// var estStdDevs = vision.getEstimationStdDevs();
// visionConsumer.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
// });
// }
// }

// @Override
// public boolean runsWhenDisabled(){
// return true;
// }
// }