diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1ebb197..9e33d5d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -432,14 +432,19 @@ public static class Cameras { // Robot to camera transforms // (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead) - // Example Camera are mounted on the frame perimeter, 18" up from the floor, centered - // side-to-side + // Example Cameras are mounted in the back corners, 18" up from the floor, facing sideways public static Transform3d robotToCamera0 = new Transform3d( - Inches.of(13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, 0.0)); + Inches.of(-13.0), + Inches.of(13.0), + Inches.of(12.0), + new Rotation3d(0.0, 0.0, Math.PI / 2)); public static Transform3d robotToCamera1 = new Transform3d( - Inches.of(-13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, Math.PI)); + Inches.of(-13.0), + Inches.of(-13.0), + Inches.of(12.0), + new Rotation3d(0.0, 0.0, -Math.PI / 2)); // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) @@ -456,7 +461,7 @@ public static class SimCameras { public static final SimCameraProperties kSimCamera1Props = new SimCameraProperties() { { - setCalibration(1280, 800, Rotation2d.fromDegrees(90)); + setCalibration(1280, 800, Rotation2d.fromDegrees(120)); setCalibError(0.25, 0.08); setFPS(30); setAvgLatencyMs(20); @@ -467,7 +472,7 @@ public static class SimCameras { public static final SimCameraProperties kSimCamera2Props = new SimCameraProperties() { { - setCalibration(1280, 800, Rotation2d.fromDegrees(90)); + setCalibration(1280, 800, Rotation2d.fromDegrees(120)); setCalibError(0.25, 0.08); setFPS(30); setAvgLatencyMs(20); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c8099d9..ce35545 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,7 +62,8 @@ import frc.robot.util.GetJoystickValue; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; -import frc.robot.util.RBSIEnum; +import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIPowerMonitor; import java.util.Set; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -379,24 +380,27 @@ private void configureBindings() { m_drivebase::stop, m_drivebase)); - // Double-press the A button on Joystick3 to run the CameraSweepEvaluator - // Use WPILib's built-in double-press binding - joystick3 - .button(1) - .multiPress(2, 0.2) - .onTrue( - Commands.runOnce( - () -> { - try { - sweep.runFullSweep( - Filesystem.getOperatingDirectory() - .toPath() - .resolve("camera_sweep.csv") - .toString()); - } catch (Exception e) { - e.printStackTrace(); - } - })); + if (Constants.getMode() == Mode.SIM) { + // IN SIMULATION ONLY: + // Double-press the A button on Joystick3 to run the CameraSweepEvaluator + // Use WPILib's built-in double-press binding + joystick3 + .button(1) + .multiPress(2, 0.2) + .onTrue( + Commands.runOnce( + () -> { + try { + sweep.runFullSweep( + Filesystem.getOperatingDirectory() + .toPath() + .resolve("camera_sweep.csv") + .toString()); + } catch (Exception e) { + e.printStackTrace(); + } + })); + } } /** @@ -467,7 +471,7 @@ public Drive getDrivebase() { *

NOTE: These are currently only accessible with Constants.AutoType.PATHPLANNER */ private void definesysIdRoutines() { - if (Constants.getAutoType() == RBSIEnum.AutoType.PATHPLANNER) { + if (Constants.getAutoType() == AutoType.PATHPLANNER) { // Drivebase characterization autoChooserPathPlanner.addOption( "Drive Wheel Radius Characterization", diff --git a/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java deleted file mode 100644 index aa907ee..0000000 --- a/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright (c) 2026 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// 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.geometry.*; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; -import frc.robot.subsystems.drive.Drive; -import java.util.List; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.simulation.VisionTargetSim; -import org.photonvision.targeting.PhotonPipelineResult; - -public class CameraPlacementEvaluator { - - private final Drive driveSim; - private final VisionSystemSim visionSim; - - public CameraPlacementEvaluator(Drive driveSim, VisionSystemSim visionSim) { - this.driveSim = driveSim; - this.visionSim = visionSim; - } - - public double evaluateCameraPose( - PhotonCameraSim simCam, Transform3d robotToCamera, SimCameraProperties props) { - - // Add or adjust camera transform in vision sim - visionSim.adjustCamera(simCam, robotToCamera); - - // Test field poses - Pose2d[] testPoses = - new Pose2d[] { - new Pose2d(1.0, 1.0, new Rotation2d(0)), - new Pose2d(3.0, 1.0, new Rotation2d(Math.PI / 2)), - new Pose2d(5.0, 3.0, new Rotation2d(Math.PI)), - new Pose2d(2.0, 4.0, new Rotation2d(-Math.PI / 2)), - }; - - double totalScore = 0.0; - double latencyMillis = 0.0; - - for (Pose2d pose : testPoses) { - - driveSim.resetPose(pose); - - for (int i = 0; i < 5; i++) { - driveSim.simulationPeriodic(); - visionSim.update(driveSim.getPose()); - Timer.delay(Constants.loopPeriodSecs); - } - - var maybeCamPose3d = visionSim.getCameraPose(simCam); - if (maybeCamPose3d.isEmpty()) { - continue; - } - Pose3d camFieldPose = maybeCamPose3d.get(); - - List allTargets = (List) visionSim.getVisionTargets(); - - PhotonPipelineResult result = simCam.process(latencyMillis, camFieldPose, allTargets); - - int tagsSeen = result.targets.size(); - - Pose2d estimatedPose = driveSim.getPose(); - double poseError = pose.getTranslation().getDistance(estimatedPose.getTranslation()); - - double score = tagsSeen - poseError; - totalScore += score; - } - - return totalScore / testPoses.length; - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java index 5cb532d..05a0eb1 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java @@ -17,9 +17,15 @@ package frc.robot.subsystems.vision; +import static frc.robot.Constants.Cameras.robotToCamera0; +import static frc.robot.Constants.Cameras.robotToCamera1; + import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import java.io.FileWriter; import java.io.IOException; import java.util.ArrayList; @@ -45,6 +51,62 @@ public CameraSweepEvaluator( this.camSim2 = camSim2; } + private static Quaternion quatFromAxisAngle(double ax, double ay, double az, double angleRad) { + double half = 0.5 * angleRad; + double s = Math.sin(half); + double c = Math.cos(half); + + // normalize axis defensively + double n = Math.sqrt(ax * ax + ay * ay + az * az); + if (n < 1e-12) { + return new Quaternion(1.0, 0.0, 0.0, 0.0); + } + ax /= n; + ay /= n; + az /= n; + + return new Quaternion(c, ax * s, ay * s, az * s); + } + + // Hamilton product: q = a ⊗ b + private static Quaternion quatMul(Quaternion a, Quaternion b) { + double aw = a.getW(), ax = a.getX(), ay = a.getY(), az = a.getZ(); + double bw = b.getW(), bx = b.getX(), by = b.getY(), bz = b.getZ(); + + double w = aw * bw - ax * bx - ay * by - az * bz; + double x = aw * bx + ax * bw + ay * bz - az * by; + double y = aw * by - ax * bz + ay * bw + az * bx; + double z = aw * bz + ax * by - ay * bx + az * bw; + + // normalize to keep drift down + double n = Math.sqrt(w * w + x * x + y * y + z * z); + if (n < 1e-12) return new Quaternion(1.0, 0.0, 0.0, 0.0); + return new Quaternion(w / n, x / n, y / n, z / n); + } + + /** + * Compose base rotation with an "extra" camera yaw/pitch. + * + *

Conventions: yawDeg = rotation about +Z (up) pitchDeg = rotation about +Y + * + *

Order: extra = yaw ⊗ pitch (yaw first, then pitch, both in the camera/base frame) combined = + * base ⊗ extra + */ + private static Rotation3d composeCameraExtra(Rotation3d base, double yawDeg, double pitchDeg) { + double yaw = Units.degreesToRadians(yawDeg); + double pitch = Units.degreesToRadians(pitchDeg); + + Quaternion qBase = base.getQuaternion(); + + Quaternion qYaw = quatFromAxisAngle(0.0, 0.0, 1.0, yaw); + Quaternion qPitch = quatFromAxisAngle(0.0, 1.0, 0.0, pitch); + + Quaternion qExtra = quatMul(qYaw, qPitch); + Quaternion qCombined = quatMul(qBase, qExtra); + + return new Rotation3d(qCombined); + } + /** * Run a full sweep of candidate camera placements. * @@ -52,66 +114,120 @@ public CameraSweepEvaluator( */ public void runFullSweep(String outputCsvPath) throws IOException { // Example field bounds (meters) -- tune these for your field size - double[] fieldX = {0.0, 2.0, 4.0, 6.0, 8.0}; // 0–8m along field length - double[] fieldY = {0.0, 1.0, 2.0, 3.0, 4.0}; // 0–4m along field width + double[] fieldX = { + 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5, 8.0 + }; + double[] fieldY = {0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5}; double[] robotZ = {0.0}; // Usually floor height - double[] camYaw = {-30, 0, 30}; // Camera yaw - double[] camPitch = {-10, 0, 10}; // Camera pitch + + // NEW: Sweep robot yaw at each location (degrees) + double[] robotYawDeg = {-180, -135, -90, -45, 0, 45, 90, 135}; + + // Camera “extra” rotation sweep (applied on top of the mount rotation) + double[] camYawDeg = {-15, 0, 15}; + double[] camPitchDeg = {-10, 0, 10}; + + // Pull fixed mount transforms from Constants once (and log/write them) + final Transform3d rToC0 = robotToCamera0; + final Transform3d rToC1 = robotToCamera1; + + final Translation3d c0T = rToC0.getTranslation(); + final Translation3d c1T = rToC1.getTranslation(); + final Quaternion c0Q = rToC0.getRotation().getQuaternion(); + final Quaternion c1Q = rToC1.getRotation().getQuaternion(); try (FileWriter writer = new FileWriter(outputCsvPath)) { - writer.write("robotX,robotY,robotZ," + "cam1Yaw,cam1Pitch,cam2Yaw,cam2Pitch,score\n"); + // Header includes robot yaw and BOTH robot->camera transforms (translation + quaternion) + writer.write( + "robotX,robotY,robotZ,robotYawDeg," + + "cam1YawDeg,cam1PitchDeg,cam2YawDeg,cam2PitchDeg,score," + + "rToC0_tx,rToC0_ty,rToC0_tz,rToC0_qw,rToC0_qx,rToC0_qy,rToC0_qz," + + "rToC1_tx,rToC1_ty,rToC1_tz,rToC1_qw,rToC1_qx,rToC1_qy,rToC1_qz\n"); // Sweep robot over the field for (double rx : fieldX) { for (double ry : fieldY) { for (double rz : robotZ) { - Pose3d robotPose = - new Pose3d( - new Translation3d(rx, ry, rz), - new Rotation3d(0, 0, 0) // robot heading = 0 for simplicity - ); - - // Sweep camera rotations - for (double c1Yaw : camYaw) { - for (double c1Pitch : camPitch) { - Pose3d cam1Pose = - new Pose3d( - robotPose.getTranslation(), - new Rotation3d(Math.toRadians(c1Pitch), 0, Math.toRadians(c1Yaw))); - - for (double c2Yaw : camYaw) { - for (double c2Pitch : camPitch) { - Pose3d cam2Pose = - new Pose3d( - robotPose.getTranslation(), - new Rotation3d(Math.toRadians(c2Pitch), 0, Math.toRadians(c2Yaw))); - - // Get all vision targets - Set simTargets = visionSim.getVisionTargets(); - List targetList = new ArrayList<>(simTargets); - - // Simulate camera processing - PhotonPipelineResult res1 = camSim1.process(0, cam1Pose, targetList); - PhotonPipelineResult res2 = camSim2.process(0, cam2Pose, targetList); - - // Score - double score = res1.getTargets().size() + res2.getTargets().size(); - if (res1.getTargets().size() >= 2) score += 2.0; - if (res2.getTargets().size() >= 2) score += 2.0; - - // Penalize ambiguity safely with loops - for (var t : res1.getTargets()) score -= t.getPoseAmbiguity() * 2.0; - for (var t : res2.getTargets()) score -= t.getPoseAmbiguity() * 2.0; - - // Write CSV row - writer.write( - String.format( - "%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.1f,%.1f\n", - rx, ry, rz, c1Yaw, c1Pitch, c2Yaw, c2Pitch, score)); - - Logger.recordOutput("CameraSweep/Score", score); - Logger.recordOutput("CameraSweep/Cam1Pose", cam1Pose); - Logger.recordOutput("CameraSweep/Cam2Pose", cam2Pose); + + // NEW: Sweep robot yaw at each location + for (double rYaw : robotYawDeg) { + Pose3d robotPose = + new Pose3d( + new Translation3d(rx, ry, rz), + new Rotation3d(0.0, 0.0, Units.degreesToRadians(rYaw))); + + // Base camera poses from mount transforms (these rotate with robot yaw) + Pose3d cam1BasePose = robotPose.transformBy(rToC0); + Pose3d cam2BasePose = robotPose.transformBy(rToC1); + + // Sweep camera "extra" rotations + for (double c1Yaw : camYawDeg) { + for (double c1Pitch : camPitchDeg) { + + Rotation3d cam1Rot = + composeCameraExtra(cam1BasePose.getRotation(), c1Yaw, c1Pitch); + Pose3d cam1Pose = new Pose3d(cam1BasePose.getTranslation(), cam1Rot); + + for (double c2Yaw : camYawDeg) { + for (double c2Pitch : camPitchDeg) { + + Rotation3d cam2Rot = + composeCameraExtra(cam2BasePose.getRotation(), c2Yaw, c2Pitch); + Pose3d cam2Pose = new Pose3d(cam2BasePose.getTranslation(), cam2Rot); + + // Get all vision targets + Set simTargets = visionSim.getVisionTargets(); + List targetList = new ArrayList<>(simTargets); + + // Simulate camera processing + PhotonPipelineResult res1 = camSim1.process(0, cam1Pose, targetList); + PhotonPipelineResult res2 = camSim2.process(0, cam2Pose, targetList); + + // Score + double score = res1.getTargets().size() + res2.getTargets().size(); + if (res1.getTargets().size() >= 2) score += 2.0; + if (res2.getTargets().size() >= 2) score += 2.0; + + // Penalize ambiguity safely with loops + for (var t : res1.getTargets()) score -= t.getPoseAmbiguity() * 2.0; + for (var t : res2.getTargets()) score -= t.getPoseAmbiguity() * 2.0; + + // Write CSV row (note: we repeat mount transforms each row for + // “self-contained” CSV) + writer.write( + String.format( + "%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.1f,%.1f,%.3f," + + "%.6f,%.6f,%.6f,%.16g,%.16g,%.16g,%.16g," + + "%.6f,%.6f,%.6f,%.16g,%.16g,%.16g,%.16g\n", + rx, + ry, + rz, + rYaw, + c1Yaw, + c1Pitch, + c2Yaw, + c2Pitch, + score, + c0T.getX(), + c0T.getY(), + c0T.getZ(), + c0Q.getW(), + c0Q.getX(), + c0Q.getY(), + c0Q.getZ(), + c1T.getX(), + c1T.getY(), + c1T.getZ(), + c1Q.getW(), + c1Q.getX(), + c1Q.getY(), + c1Q.getZ())); + + Logger.recordOutput("CameraSweep/Score", score); + Logger.recordOutput("CameraSweep/RobotPose", robotPose); + Logger.recordOutput("CameraSweep/Cam1Pose", cam1Pose); + Logger.recordOutput("CameraSweep/Cam2Pose", cam2Pose); + } } } }