diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7298425f..edd74a0e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,6 +41,8 @@ import frc.robot.subsystems.swerve.GyroIOPigeon2; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem.AutoAimStates; +import frc.robot.subsystems.vision.NoteDetectionIOReal; +import frc.robot.subsystems.vision.NoteDetectionIOSim; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.autoaim.AutoAim; import java.util.function.Supplier; @@ -92,7 +94,10 @@ public void setYaw(Rotation2d yaw) {} : SwerveSubsystem.createSimCameras(), mode == RobotMode.REAL ? SwerveSubsystem.createTalonFXModules() - : SwerveSubsystem.createSimModules()); + : SwerveSubsystem.createSimModules(), + mode == RobotMode.REAL + ? new NoteDetectionIOReal(SwerveSubsystem.noteDetectionCamAConstants) + : new NoteDetectionIOSim(SwerveSubsystem.noteDetectionCamAConstants)); private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOReal()); private final FeederSubsystem feeder = new FeederSubsystem(new FeederIOReal()); private final ElevatorSubsystem elevator = diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 171eaa93..52cb3945 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -32,6 +32,7 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -65,6 +66,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.FieldConstants; import frc.robot.subsystems.swerve.Module.ModuleConstants; +import frc.robot.subsystems.vision.NoteDetectionIO; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.Vision.VisionConstants; import frc.robot.subsystems.vision.VisionHelper; @@ -73,6 +75,7 @@ import frc.robot.subsystems.vision.VisionIOSim; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.ShotData; +import frc.robot.utils.vision.NoteDetectionIOInputsLogged; import java.util.Arrays; import java.util.NoSuchElementException; import java.util.concurrent.locks.Lock; @@ -212,12 +215,21 @@ public class AutoAimStates { new Rotation3d(0, -0.490, Math.PI * 2 / 3)), // TODO check RIGHT_CAMERA_MATRIX_OPT, RIGHT_DIST_COEFFS_OPT); + public static final VisionConstants noteDetectionCamAConstants = + new VisionConstants( + "a", + new Transform3d(new Translation3d(), new Rotation3d()), // TODO + LEFT_CAMERA_MATRIX, + LEFT_DIST_COEFFS); private SwerveDriveOdometry odometry; private final SysIdRoutine moduleSteerRoutine; private final SysIdRoutine driveRoutine; + private final NoteDetectionIO noteDetectionIO; // TODO assuming there is only one for now + private final NoteDetectionIOInputsLogged noteDetectionInputs = new NoteDetectionIOInputsLogged(); - public SwerveSubsystem(GyroIO gyroIO, VisionIO[] visionIOs, ModuleIO[] moduleIOs) { + public SwerveSubsystem( + GyroIO gyroIO, VisionIO[] visionIOs, ModuleIO[] moduleIOs, NoteDetectionIO noteDetectionIO) { this.gyroIO = gyroIO; cameras = new Vision[visionIOs.length]; new AutoAim(); @@ -230,6 +242,7 @@ public SwerveSubsystem(GyroIO gyroIO, VisionIO[] visionIOs, ModuleIO[] moduleIOs for (int i = 0; i < visionIOs.length; i++) { cameras[i] = new Vision(visionIOs[i]); } + this.noteDetectionIO = noteDetectionIO; // mildly questionable VisionIOSim.pose = this::getPose3d; @@ -356,6 +369,8 @@ public void periodic() { for (var module : modules) { module.periodic(); } + noteDetectionIO.updateInputs(noteDetectionInputs); + Logger.processInputs("Note Detection", noteDetectionInputs); // Stop moving when disabled if (DriverStation.isDisabled()) { @@ -876,4 +891,36 @@ public Command runDriveCharacterizationCmd() { driveRoutine.dynamic(Direction.kReverse), this.runOnce(() -> SignalLogger.stop())); } + + // TODO idk if i did this right + public Command poseLockDriveCmd(DoubleSupplier x, DoubleSupplier y, DoubleSupplier theta) { + PIDController xController = new PIDController(-1, 0, 0); + PIDController yController = new PIDController(-1, 0, 0); + PIDController headingController = new PIDController(1.2, 0, 0.1); + headingController.enableContinuousInput(0, 2 * Math.PI); + return runVelocityFieldRelative( + () -> { + return new ChassisSpeeds( + xController.calculate(pose.getX(), x.getAsDouble()) * MAX_LINEAR_SPEED, + yController.calculate(pose.getY(), y.getAsDouble()) * MAX_LINEAR_SPEED, + headingController.calculate(pose.getRotation().getRadians(), theta.getAsDouble()) + * MAX_ANGULAR_SPEED); + }); + } + + public Command driveToPoseCmd(Supplier pose) { + return poseLockDriveCmd( + () -> pose.get().getX(), + () -> pose.get().getY(), + () -> pose.get().getRotation().getRadians()); + } + + public Command driveToNoteCmd() { + var note = VisionHelper.getBestTarget(getPose(), new PhotonPipelineResult(noteDetectionInputs.latency, noteDetectionInputs.targets), 0); //TODO change camerax and ppresult + if (note.isEmpty()) { + return Commands.none(); + } // TODO driver feedback? Must be proxied for duration + var pickup = note.get().transformBy(new Transform2d(new Translation2d(-1, 0), new Rotation2d())); + return driveToPoseCmd(() -> pickup); + } } diff --git a/src/main/java/frc/robot/subsystems/vision/NoteDetectionIO.java b/src/main/java/frc/robot/subsystems/vision/NoteDetectionIO.java new file mode 100644 index 00000000..8cfc449e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/NoteDetectionIO.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import frc.robot.subsystems.vision.Vision.VisionConstants; +import java.util.ArrayList; +import java.util.List; +import org.photonvision.targeting.PhotonTrackedTarget; + +/** Add your docs here. */ +public interface NoteDetectionIO { //TODO make vision superinterface/class? + + public static class NoteDetectionIOInputs { + public double timestamp = 0.0; + public double latency = 0.0; + public List targets = new ArrayList<>(); // TODO protobuf + public double numTargets = 0; + public VisionConstants constants; + public double yaw = 0.0; + public double pitch = 0.0; + public double distance = 0.0; + } + + public void updateInputs(NoteDetectionIOInputs inputs); + + public String getName(); +} diff --git a/src/main/java/frc/robot/subsystems/vision/NoteDetectionIOReal.java b/src/main/java/frc/robot/subsystems/vision/NoteDetectionIOReal.java new file mode 100644 index 00000000..323de598 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/NoteDetectionIOReal.java @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.vision.Vision.VisionConstants; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; + +/** Add your docs here. */ +public class NoteDetectionIOReal implements NoteDetectionIO { + public String cameraName; + public PhotonCamera camera; + public Matrix cameraMatrix; + public Matrix distCoeffs; + + private final VisionConstants constants; + + public NoteDetectionIOReal(VisionConstants constants) { + cameraName = constants.cameraName(); + camera = new PhotonCamera(cameraName); + cameraMatrix = constants.intrinsicsMatrix(); + distCoeffs = constants.distCoeffs(); + this.constants = constants; + } + + @Override + public void updateInputs(NoteDetectionIOInputs inputs) { + var result = camera.getLatestResult(); + inputs.timestamp = result.getTimestampSeconds(); + inputs.latency = result.getLatencyMillis(); + inputs.targets = result.targets; + inputs.constants = constants; + inputs.yaw = -Units.degreesToRadians(result.getBestTarget().getYaw()); // why is this negative + inputs.pitch = -Units.degreesToRadians(result.getBestTarget().getPitch()); + inputs.distance = + PhotonUtils.calculateDistanceToTargetMeters( + 0, 0, 0, Units.degreesToRadians(result.getBestTarget().getPitch())); // TODO fix + } + + @Override + public String getName() { + return cameraName; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/NoteDetectionIOSim.java b/src/main/java/frc/robot/subsystems/vision/NoteDetectionIOSim.java new file mode 100644 index 00000000..9be2733b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/NoteDetectionIOSim.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N5; +import frc.robot.subsystems.vision.Vision.VisionConstants; +import org.photonvision.PhotonCamera; + +/** Add your docs here. */ +public class NoteDetectionIOSim implements NoteDetectionIO { + public String cameraName; + public PhotonCamera camera; + public Matrix cameraMatrix; + public Matrix distCoeffs; + + private final VisionConstants constants; + + public NoteDetectionIOSim(VisionConstants constants) { + cameraName = constants.cameraName(); + camera = new PhotonCamera(cameraName); + cameraMatrix = constants.intrinsicsMatrix(); + distCoeffs = constants.distCoeffs(); + this.constants = constants; + } + + @Override + public void updateInputs(NoteDetectionIOInputs inputs) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'updateInputs'"); + } + + @Override + public String getName() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getName'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 75ed93c8..4b5bdd46 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N5; +import frc.robot.utils.vision.VisionIOInputsLogged; import java.util.Optional; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionHelper.java b/src/main/java/frc/robot/subsystems/vision/VisionHelper.java index cd0754cb..d72ef824 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionHelper.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionHelper.java @@ -8,14 +8,19 @@ import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.robot.Robot; @@ -27,6 +32,7 @@ import org.littletonrobotics.junction.LogTable; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.PhotonUtils; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.simulation.VisionSystemSim; @@ -437,4 +443,30 @@ public static Field2d getSimDebugField(VisionSystemSim visionSim) { if (!Robot.isSimulation()) return null; return visionSim.getDebugField(); } + //6995 + public static double getDistance(PhotonTrackedTarget target) { + return PhotonUtils.calculateDistanceToTargetMeters( + 0, 0, 0, Units.degreesToRadians(target.getPitch())); // TODO fix + } +public static Translation2d getNoteTargetOffset(PhotonTrackedTarget target, double cameraX) { + return new Translation2d(getDistance(target), Rotation2d.fromDegrees(-target.getYaw())) +.plus(new Translation2d(cameraX, 0)); +} + +public static List getTargets(Pose2d robotPose, PhotonPipelineResult result, double cameraX) { + if (result == null) { + return List.of(); + } + // TODO add sim logic + if (!result.hasTargets()) return List.of(); + var list = result.getTargets(); + list.removeIf((t)-> (t.getPitch() > 9)); + return list.stream().map((t)->robotPose.transformBy(new Transform2d(getNoteTargetOffset(t, cameraX), Rotation2d.fromDegrees(-t.getYaw())))).toList(); +} + +public static Optional getBestTarget(Pose2d robotPose, PhotonPipelineResult result, double cameraX) { + var targets = getTargets(robotPose, result, cameraX); + if (targets.size() == 0) {return Optional.empty();} + return Optional.of(targets.get(0)); +} } diff --git a/src/main/java/frc/robot/utils/vision/NoteDetectionIOInputsLogged.java b/src/main/java/frc/robot/utils/vision/NoteDetectionIOInputsLogged.java new file mode 100644 index 00000000..249b25c5 --- /dev/null +++ b/src/main/java/frc/robot/utils/vision/NoteDetectionIOInputsLogged.java @@ -0,0 +1,46 @@ +package frc.robot.utils.vision; + +import frc.robot.subsystems.vision.NoteDetectionIO; +import frc.robot.subsystems.vision.VisionHelper; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class NoteDetectionIOInputsLogged extends NoteDetectionIO.NoteDetectionIOInputs + implements LoggableInputs, Cloneable { + @Override + public void toLog(LogTable table) { + table.put("Timestamp", timestamp); + table.put("Latency", latency); + for (int i = 0; i < targets.size(); i++) { + VisionHelper.Logging.logPhotonTrackedTarget(targets.get(i), table, String.valueOf(i)); + } + table.put("NumTargets", targets.size()); + VisionHelper.Logging.logVisionConstants(constants, table); + table.put("Note Yaw", yaw); + table.put("Note Pitch", pitch); + table.put("Note Distance", distance); + } + + @Override + public void fromLog(LogTable table) { + timestamp = table.get("Timestamp", timestamp); + latency = table.get("Latency", latency); + for (int i = 0; i < table.get("NumTargets", numTargets); i++) { + this.targets.add(VisionHelper.Logging.getLoggedPhotonTrackedTarget(table, String.valueOf(i))); + } + constants = VisionHelper.Logging.getLoggedVisionConstants(table); + yaw = table.get("Note Yaw", yaw); + pitch = table.get("Note Pitch", pitch); + distance = table.get("Note Distance", distance); + } + + public NoteDetectionIOInputsLogged clone() { + NoteDetectionIOInputsLogged copy = new NoteDetectionIOInputsLogged(); + copy.timestamp = this.timestamp; + copy.latency = this.latency; + copy.yaw = this.yaw; + copy.pitch = this.pitch; + copy.distance = this.distance; + return copy; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java b/src/main/java/frc/robot/utils/vision/VisionIOInputsLogged.java similarity index 94% rename from src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java rename to src/main/java/frc/robot/utils/vision/VisionIOInputsLogged.java index 039833c2..34a62fe8 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java +++ b/src/main/java/frc/robot/utils/vision/VisionIOInputsLogged.java @@ -1,7 +1,9 @@ -package frc.robot.subsystems.vision; +package frc.robot.utils.vision; import edu.wpi.first.math.geometry.Pose3d; import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.vision.VisionHelper; +import frc.robot.subsystems.vision.VisionIO; import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.inputs.LoggableInputs; diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 46211fc1..8c565032 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.4", + "version": "v2024.2.8", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.4", + "version": "v2024.2.8", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.4", + "version": "v2024.2.8", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.4" + "version": "v2024.2.8" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.4" + "version": "v2024.2.8" } ] } \ No newline at end of file