From 16f34fbc9db62d7867279ec0181d0cf9291bd853 Mon Sep 17 00:00:00 2001 From: Veselin Dikov Date: Sat, 30 Aug 2025 18:11:02 -0700 Subject: [PATCH 1/2] Adjusted physical angles of cameras and reflected this in Drive.java --- .../java/com/team2813/subsystems/Drive.java | 31 ++++++++++++------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 2dba229d..072fcba6 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -6,7 +6,6 @@ import static com.team2813.Constants.BACK_RIGHT_DRIVE_ID; import static com.team2813.Constants.BACK_RIGHT_ENCODER_ID; import static com.team2813.Constants.BACK_RIGHT_STEER_ID; -import static com.team2813.Constants.DriverConstants.DRIVER_CONTROLLER; import static com.team2813.Constants.FRONT_LEFT_DRIVE_ID; import static com.team2813.Constants.FRONT_LEFT_ENCODER_ID; import static com.team2813.Constants.FRONT_LEFT_STEER_ID; @@ -14,11 +13,22 @@ import static com.team2813.Constants.FRONT_RIGHT_ENCODER_ID; import static com.team2813.Constants.FRONT_RIGHT_STEER_ID; import static com.team2813.Constants.PIGEON_ID; +import static com.team2813.Constants.DriverConstants.DRIVER_CONTROLLER; import static com.team2813.lib2813.util.ControlUtils.deadband; import static edu.wpi.first.units.Units.Centimeters; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Rotations; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.stream.IntStream; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; + import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -42,6 +52,7 @@ import com.team2813.lib2813.preferences.PreferencesInjector; import com.team2813.sysid.SwerveSysidRequest; import com.team2813.vision.MultiPhotonPoseEstimator; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; @@ -68,14 +79,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.List; -import java.util.function.DoubleSupplier; -import java.util.stream.IntStream; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonTrackedTarget; /** This is the Drive. His name is Gary. Please be kind to him and say hi. Have a nice day! */ @@ -116,15 +119,21 @@ public class Drive extends SubsystemBase implements AutoCloseable { static final double FRONT_DIST = 0.4064; static final double LEFT_DIST = 0.3302; + // positive x - robot forward + // positive y - robot left + // positive z - robot up + // positive roll - tilt to the right + // positive pitch - look down + // positive yaw - turn left private static final Transform3d leftColorTransform = new Transform3d( new Translation3d(Centimeters.of(31), Centimeters.of(21), Centimeters.of(20)), - new Rotation3d(Degrees.of(0), Degrees.of(-30), Degrees.of(-30))); + new Rotation3d(Degrees.of(0), Degrees.of(-20), Degrees.of(-30))); private static final Transform3d rightColorTransform = new Transform3d( new Translation3d(Centimeters.of(29), Centimeters.of(-23), Centimeters.of(13)), - new Rotation3d(Degrees.of(0), Degrees.of(-20), Degrees.of(30))); + new Rotation3d(Degrees.of(0), Degrees.of(-20), Degrees.of(20))); /** * Configurable values for the {@code Drive} subsystem From 8b7fc66ad2ff55ab763619e0e846ec7633265fa8 Mon Sep 17 00:00:00 2001 From: Veselin Dikov Date: Sat, 30 Aug 2025 18:36:46 -0700 Subject: [PATCH 2/2] spotlessApply and some thoughts about merging poses --- .../java/com/team2813/subsystems/Drive.java | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/Drive.java b/src/main/java/com/team2813/subsystems/Drive.java index 072fcba6..062f19c8 100644 --- a/src/main/java/com/team2813/subsystems/Drive.java +++ b/src/main/java/com/team2813/subsystems/Drive.java @@ -6,6 +6,7 @@ import static com.team2813.Constants.BACK_RIGHT_DRIVE_ID; import static com.team2813.Constants.BACK_RIGHT_ENCODER_ID; import static com.team2813.Constants.BACK_RIGHT_STEER_ID; +import static com.team2813.Constants.DriverConstants.DRIVER_CONTROLLER; import static com.team2813.Constants.FRONT_LEFT_DRIVE_ID; import static com.team2813.Constants.FRONT_LEFT_ENCODER_ID; import static com.team2813.Constants.FRONT_LEFT_STEER_ID; @@ -13,22 +14,11 @@ import static com.team2813.Constants.FRONT_RIGHT_ENCODER_ID; import static com.team2813.Constants.FRONT_RIGHT_STEER_ID; import static com.team2813.Constants.PIGEON_ID; -import static com.team2813.Constants.DriverConstants.DRIVER_CONTROLLER; import static com.team2813.lib2813.util.ControlUtils.deadband; import static edu.wpi.first.units.Units.Centimeters; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Rotations; -import java.util.List; -import java.util.function.DoubleSupplier; -import java.util.stream.IntStream; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonTrackedTarget; - import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -52,7 +42,6 @@ import com.team2813.lib2813.preferences.PreferencesInjector; import com.team2813.sysid.SwerveSysidRequest; import com.team2813.vision.MultiPhotonPoseEstimator; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; @@ -79,6 +68,14 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.stream.IntStream; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; /** This is the Drive. His name is Gary. Please be kind to him and say hi. Have a nice day! */ @@ -547,6 +544,10 @@ public void periodic() { photonPoseEstimator.addHeadingData(Timer.getTimestamp(), getRotation3d()); } photonPoseEstimator.update(this::handlePhotonPose); + // Thoughts about moving away from functional programing paradigm here to just simpler, + // procedural programming, e.g., + // List estimates = phonPoseEstimater.getEstimates(); + // handleVisionPose(estimates); Pose2d drivePose = getPose(); currentPosePublisher.set(drivePose); photonPoseEstimator.setDrivePose(drivePose);