diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 01c9a4a..d3f1237 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -328,8 +328,15 @@ public void bindXboxDriver(int port) { drive) .ignoringDisable(true)); + // Point at target while A button is held + xboxDriver + .a() + .whileTrue( + DriveCommands.pointAtTarget( + drive, () -> vision.getTargetX(0), allianceSelector::fieldRotated)); + // Drive 1m forward while A button is held - xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1))); + // xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1))); // Align with pose, approaching in correct orientation from 1 m away // xboxDriver diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 047e30b..35f49fe 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -231,6 +231,46 @@ public static Command joystickDriveAtFixedOrientation( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + /** + * Field relative drive command using joystick for linear control and PID for angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, or controlling + * absolute rotation with a joystick. + */ + public static Command pointAtTarget( + Drive drive, Supplier yawErrorSupplier, BooleanSupplier fieldRotatedSupplier) { + + // Create PID controller + ProfiledPIDController angleController = + new ProfiledPIDController( + ANGLE_KP, + 0.0, + ANGLE_KD, + new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + + // Calculate angular speed + double omega = angleController.calculate(yawErrorSupplier.get().getRadians(), 0); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = new ChassisSpeeds(0, 0, omega); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + fieldRotatedSupplier.getAsBoolean() + ? drive.getRotation().plus(Rotation2d.kPi) + : drive.getRotation())); + }, + drive) + .withName("Point At Target") + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(yawErrorSupplier.get().getRadians())); + } + /** * Measures the velocity feedforward constants for the drive motors. * diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1db554a..bd8715b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -146,7 +146,7 @@ public class DriveConstants { new Slot0Configs() .withKP(300) .withKI(0) - .withKD(0.5) + .withKD(1.5) .withKS(0.1) .withKV(1.91) .withKA(0) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 119c6cc..71fb3be 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -67,12 +67,12 @@ public Vision(VisionConsumer consumer, Supplier poseSupplier, VisionIO.. } /** - * Returns the X angle to the best target, which can be used for simple servoing with vision. + * Returns the yaw angle to the best target, which can be used for simple servoing with vision. * * @param cameraIndex The index of the camera to use. */ public Rotation2d getTargetX(int cameraIndex) { - return inputs[cameraIndex].latestTargetObservation.tx(); + return inputs[cameraIndex].latestTargetObservation.yaw(); } @Override diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 52cd0c6..72c169a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -26,20 +26,20 @@ public class VisionConstants { public static AprilTagFields defaultAprilTagFieldLayout = AprilTagFields.k2025ReefscapeAndyMark; // Camera names, must match names configured on coprocessor - public static String cameraFrontRightName = "OV2311_TH_4"; - public static String cameraFrontLeftName = "OV2311_TH_3"; - public static String cameraBackRightName = "OV2311_TH_2"; - public static String cameraBackLeftName = "OV2311_TH_1"; + public static String cameraFrontRightName = "OV2311_TH_8"; + public static String cameraFrontLeftName = "OV2311_TH_5"; + public static String cameraBackRightName = "OV2311_TH_6"; + public static String cameraBackLeftName = "OV2311_TH_7"; // Robot to camera transforms public static Transform3d robotToFrontRightCamera = - new Transform3d(0.286, -0.332, 0.361, new Rotation3d(0, 0, 0.768)); + new Transform3d(0.248, -0.318, 0.513, new Rotation3d(0.0, 0.0, 0.0)); public static Transform3d robotToFrontLeftCamera = - new Transform3d(0.292, 0.316, 0.361, new Rotation3d(0, 0, 5.498)); + new Transform3d(0.222, 0.331, 0.513, new Rotation3d(0.0, 0, Math.PI / 2.0)); public static Transform3d robotToBackRightCamera = - new Transform3d(-0.259, -0.346, 0.628, new Rotation3d(0, 0, 4.311)); + new Transform3d(-0.375, -0.331, 0.513, new Rotation3d(0.0, 0.0, 3.0 * Math.PI / 2.0)); public static Transform3d robotToBackLeftCamera = - new Transform3d(-0.252, 0.341, 0.628, new Rotation3d(0, 0, 1.972)); + new Transform3d(-0.401, 0.318, 0.513, new Rotation3d(0.0, 0.0, Math.PI)); public static Distance minRobotWidth = Inches.of(36.875); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 1ebf25b..8a9c403 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -16,13 +16,19 @@ public interface VisionIO { public static class VisionIOInputs { public boolean connected = false; public TargetObservation latestTargetObservation = - new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, -1, -1); public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; } /** Represents the angle to a simple target, not used for pose estimation. */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + public static record TargetObservation( + Rotation2d yaw, + Rotation2d pitch, + Rotation2d skew, + double area, + float confidence, + int objectID) {} /** Represents a robot pose sample used for pose estimation. */ public static record PoseObservation( diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 7566750..165ed26 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -39,15 +39,22 @@ public void updateInputs(VisionIOInputs inputs) { // Read new camera observations Set tagIds = new HashSet<>(); List poseObservations = new LinkedList<>(); + for (var result : camera.getAllUnreadResults()) { // Update latest target observation if (result.hasTargets()) { + var bestTarget = result.getBestTarget(); inputs.latestTargetObservation = new TargetObservation( - Rotation2d.fromDegrees(result.getBestTarget().getYaw()), - Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + Rotation2d.fromDegrees(bestTarget.getYaw()), + Rotation2d.fromDegrees(bestTarget.getPitch()), + Rotation2d.fromDegrees(bestTarget.getSkew()), + bestTarget.getArea(), + bestTarget.getDetectedObjectConfidence(), + bestTarget.getDetectedObjectClassID()); } else { - inputs.latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); + inputs.latestTargetObservation = + new TargetObservation(Rotation2d.kZero, Rotation2d.kZero, Rotation2d.kZero, 0, -1, -1); } // Add pose observation