From d85513a73fb1d9208d8e0ede0d51ad4be5d86b78 Mon Sep 17 00:00:00 2001 From: Cameron Myhre Date: Sun, 17 Mar 2024 11:52:57 -0500 Subject: [PATCH 1/8] feat: Add alliance enum value This commit should make it easier to implement alliance depended / specific features. Will be expanded upon later. --- .../java/frc/utils/Autonomous/Alliance.java | 68 +++++++++++++++++++ .../utils/Autonomous/AutonomousOption.java | 44 ++++++++---- 2 files changed, 98 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/utils/Autonomous/Alliance.java diff --git a/src/main/java/frc/utils/Autonomous/Alliance.java b/src/main/java/frc/utils/Autonomous/Alliance.java new file mode 100644 index 0000000..b868e09 --- /dev/null +++ b/src/main/java/frc/utils/Autonomous/Alliance.java @@ -0,0 +1,68 @@ +package frc.utils.Autonomous; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public enum Alliance { + + RED (new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)), + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)), + Rotation2d.fromDegrees(180)), + BLUE (new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)), + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)), + Rotation2d.fromDegrees(0)), + NOT_APPLICABLE (null, + null, + null); + + private final Pose2d AMP_POSITION; + private final Pose2d SPEAKER_POSITION; + private final Rotation2d ROTATIONAL_OFFSET; + + /** + * Setup all values relevant to this alliance. + * + * @param ampPosition The position of this alliance's amp. + * @param speakerPosition The position of this alliance's speaker. + * @param rotationalOffset A value required when driving the robot on red team, as we are using the field's + * coordinate system for the robot's orientation, which would cause our inputs to be + * reversed when driving. By setting this value to pi (180 degrees) wwe are able to flip + * these inputs back to normal. + */ + Alliance(Pose2d ampPosition, Pose2d speakerPosition, Rotation2d rotationalOffset) { + this.AMP_POSITION = ampPosition; + this.SPEAKER_POSITION = speakerPosition; + this.ROTATIONAL_OFFSET = rotationalOffset; + } + + /** + * Returns the position of this alliance's amp. + * + * @return The position of this alliance's amp. + */ + public Pose2d getAmpPosition() { + return this.AMP_POSITION; + } + + /** + * Returns the position of this alliance's speaker. + * + * @return The position of this alliance's speaker. + */ + public Pose2d getSpeakerPosition() { + return this.SPEAKER_POSITION; + } + + /** + * Returns this alliance's rotational offset. + * + * @return A value required when driving the robot on red team, as we are using the field's + * coordinate system for the robot's orientation, which would cause our inputs to be + * reversed when driving. By setting this value to pi (180 degrees) wwe are able to flip + * these inputs back to normal. + */ + public Rotation2d getRotationalOffset() { + return this.ROTATIONAL_OFFSET; + } +} diff --git a/src/main/java/frc/utils/Autonomous/AutonomousOption.java b/src/main/java/frc/utils/Autonomous/AutonomousOption.java index 342e0b0..c9675ae 100644 --- a/src/main/java/frc/utils/Autonomous/AutonomousOption.java +++ b/src/main/java/frc/utils/Autonomous/AutonomousOption.java @@ -10,25 +10,28 @@ public enum AutonomousOption { RED_1 (AutoConstants.RED_1_TRAJECTORY, AutoConstants.RED_1_STARTING_POSE, - new Rotation2d(Math.PI)), + Alliance.RED), RED_2 (AutoConstants.RED_2_TRAJECTORY, AutoConstants.RED_2_STARTING_POSE, - new Rotation2d(Math.PI)), + Alliance.RED), RED_3 (AutoConstants.RED_3_TRAJECTORY, AutoConstants.RED_3_STARTING_POSE, - new Rotation2d(Math.PI)), + Alliance.RED), BLUE_1 (AutoConstants.BLUE_1_TRAJECTORY, AutoConstants.BLUE_1_STARTING_POSE, - new Rotation2d(0)), + Alliance.BLUE), BLUE_2 (AutoConstants.BLUE_2_TRAJECTORY, AutoConstants.BLUE_2_STARTING_POSE, - new Rotation2d(0)), + Alliance.BLUE), BLUE_3 (AutoConstants.BLUE_3_TRAJECTORY, AutoConstants.BLUE_3_STARTING_POSE, - new Rotation2d(0)); + Alliance.BLUE); private final Pose2d STARTING_LOCATION; private final List TRAJECTORY; + private final Alliance ALLIANCE; + private final Pose2d AMP_POSITION; + private final Pose2d SPEAKER_POSITION; private final Rotation2d TELEOP_ROTATION_OFFSET; /** @@ -36,15 +39,16 @@ public enum AutonomousOption { * * @param trajectory A list of points that the robot will drive to during auto. * @param startingLocation The starting position of the robot. - * @param teleopRotationOffset A value required when driving the robot on red team, as we are using the field's - * coordinate system for the robot's orientation, which would cause our inputs to be - * reversed when driving. By setting this value to pi (180 degrees) wwe are able to flip - * these inputs back to normal. + * @param alliance */ - AutonomousOption(List trajectory, Pose2d startingLocation, Rotation2d teleopRotationOffset) { + AutonomousOption(List trajectory, Pose2d startingLocation, Alliance alliance) { this.TRAJECTORY = trajectory; this.STARTING_LOCATION = startingLocation; - this.TELEOP_ROTATION_OFFSET = teleopRotationOffset; + this.ALLIANCE = alliance; + + this.AMP_POSITION = ALLIANCE.getAmpPosition(); + this.SPEAKER_POSITION = ALLIANCE.getSpeakerPosition(); + this.TELEOP_ROTATION_OFFSET = ALLIANCE.getRotationalOffset(); } /** @@ -57,14 +61,26 @@ public Pose2d getStartingPosition() { } /** - * Returns this at authonomous path's trajectory, which is a list of {@Code Pose3d} that the robot will travel to. + * Returns this at autonomous path's trajectory, which is a list of {@Code Pose3d} that the robot will travel to. * - * @return This authonomous path's trajectory. + * @return This autonomous path's trajectory. */ public List getTrajectory() { return this.TRAJECTORY; } + public Alliance getAlliance() { + return this.ALLIANCE; + } + + public Pose2d getAmpPosition() { + return this.AMP_POSITION; + } + + public Pose2d getSpeakerPosition() { + return this.SPEAKER_POSITION; + } + /** * Returns a the value by which the robot's rotation will be offset by, so that our drive inputs won't be * reversed on red alliance. From 6aa55fd9c974aee4a618f41610d463df13af8565 Mon Sep 17 00:00:00 2001 From: Cameron Myhre Date: Tue, 19 Mar 2024 18:14:52 -0500 Subject: [PATCH 2/8] style: Fix comment grammar --- .vscode/settings.json | 5 ++++- .../java/frc/robot/subsystems/DriveSubsystem.java | 13 +++++++------ 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 337aac5..bfb90aa 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,8 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.debug.settings.onBuildFailureProceed": true + "java.debug.settings.onBuildFailureProceed": true, + "cSpell.words": [ + "Odometry" + ] } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index c93a516..87f0cc8 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -185,7 +185,7 @@ public void periodic() { SmartDashboard.putNumber("Robot Heading: ", robotPose.getRotation().getDegrees()); SmartDashboard.putNumber("IMU Heading: ", gyro.getAngle()); - // Output the current driver controlelr offset to check whether or not our code works. + // Output the current driver controller offset to check whether or not our code works. SmartDashboard.putNumber("Rotation Offset: ", driverRotationalOffset.getDegrees()); } @@ -196,7 +196,7 @@ public void periodic() { */ private void updateRobotPose() { - // Return if poseEstimator hasn't been intialize yet. + // Return if poseEstimator hasn't been initialize yet. if (poseEstimator == null) { return; } @@ -204,7 +204,7 @@ private void updateRobotPose() { // Retrieve the estimated position from the robot's vision system. Pose2d estimatedPose2d = visionMeasurementSupplier.get(); - // Add the robot's estimated vision measurments to the pose estimator if they are not null. + // Add the robot's estimated vision measurements to the pose estimator if they are not null. if (estimatedPose2d != null) { // Incorporate the robot's estimated vision measurements into this DriveSubsystem's poseEstimator. @@ -262,7 +262,7 @@ public void resetTrajectory() { } /** - * Configre the path the robot will follow and the rotational offset that will be used during teleop + * Configure the path the robot will follow and the rotational offset that will be used during teleop * by using the selected autonomous command. * * @param selectedAuto The selected autonomous that determines which path the robot will follow and @@ -304,7 +304,8 @@ public void followTrajectory() { } /** - * gets whether or not the trajcetory is finished + * gets whether or not the trajectory is finished + * * @return whether or not the trajectory is finished */ public boolean isTrajectoryFinished() { @@ -331,7 +332,7 @@ public boolean isTrajectoryFinished() { * the right joystick y * @param boostMode controls the robot's max speed * @param fieldRelative determines whether the robot is field centric - * @param rateLimit applys rate limiting to the robot + * @param rateLimit Applies rate limiting to the robot */ public void driveFromController(double leftX, double leftY, double rightX, double rightY, double boostMode,boolean fieldRelative, boolean rateLimit) { From 25995d036fc62532a45d4e5514add8c45011138a Mon Sep 17 00:00:00 2001 From: Cameron Myhre Date: Thu, 21 Mar 2024 16:55:03 -0500 Subject: [PATCH 3/8] style: Fix grammatical; errors --- src/main/java/frc/robot/Constants.java | 16 ++--- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 17 +++--- .../vision/ResetImuWithVisionCommand.java | 17 +++--- .../frc/robot/subsystems/DriveSubsystem.java | 26 ++++----- .../subsystems/cameras/LimelightCamera.java | 2 +- .../cameras/PhotonVisionCamera.java | 13 +++-- .../robot/subsystems/cameras/RobotVision.java | 58 +++++++++---------- .../utils/Autonomous/AutonomousOption.java | 15 +++++ .../utils/geometry/EstimatedRobotPose3d.java | 56 +++++++++++++++--- 10 files changed, 137 insertions(+), 87 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 757e069..525e736 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -162,21 +162,21 @@ public static final class VisionConstants { // Camera settings public static final String LIMELIGHT_NAME = "limelight"; - public static final String BLACK_AND_WHITE_CAMERA_NAME = "Arducam_OV9281_USB_Camera"; + public static final String ARDUCAM_OV9281_USB_CAMERA = "Arducam_OV9281_USB_Camera"; + public static final String ARDUCAM_OV2311_USB_CAMERA = "Arducam_OV2311_USB_Camera"; public static final String COLOR_CAMERA_NAME = "USB_2M_GS_camera"; - // Camera offsets (NOTE: These values are placeholders and thus subject to - // change). - public static final Transform3d FRONT_CAMERA_OFFSET = new Transform3d( + // Camera offsets (NOTE: These values are placeholders and thus subject to change). + public static final Transform3d RED_OUTTAKE_CAMERA_OFFSET = new Transform3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); - public static final Transform3d RIGHT_CAMERA_OFFSET = new Transform3d( + public static final Transform3d RED_INTAKE_CAMERA_OFFSET = new Transform3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); - public static final Transform3d LEFT_CAMERA_OFFSET = new Transform3d( + public static final Transform3d BLACK_OUTTAKE_CAMERA_OFFSET = new Transform3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); - public static final Transform3d BACK_CAMERA_OFFSET = new Transform3d( + public static final Transform3d BLACK_INTAKE_CAMERA_OFFSET = new Transform3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)); } @@ -207,7 +207,7 @@ public static final class AutoConstants { // Constraint for the motion profiled robot angle controller // Value is a float because xyz - public static final double TRAJECOTRY_THRESHOLD = 0.1; //the maximum distance away from a waypoint before the robot moves onto the next one (meters) + public static final double TRAJECTORY_THRESHOLD = 0.1; //the maximum distance away from a waypoint before the robot moves onto the next one (meters) public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f8fe823..a030d51 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,7 +37,7 @@ public void robotInit() { // Start the timer snapshotTimer.start(); - // Mak eit possible to view caeras on the driverstation. + // Mak eit possible to view cameras on the driver station. CameraServer.startAutomaticCapture(); // Make it possible to view the photonvision dashboard over the internet @@ -100,7 +100,7 @@ public void autonomousPeriodic() { public void teleopInit() { // Update the robot's settings so that they match what was configured on SmartDashboard. - robotContainer.configureDriverPrefferences(); + robotContainer.configureDriverPreferences(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 917b0d4..fd2e514 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,7 +33,7 @@ public class RobotContainer { // The robot's subsystems private final RobotVision robotVision = new RobotVision.Builder() - .addPhotonVisionCamera(VisionConstants.COLOR_CAMERA_NAME, VisionConstants.BACK_CAMERA_OFFSET, + .addPhotonVisionCamera(VisionConstants.COLOR_CAMERA_NAME, VisionConstants.BLACK_INTAKE_CAMERA_OFFSET, VisionConstants.APRIL_TAG_PIPELINE) .build(); private final DriveSubsystem driveSubsystem = new DriveSubsystem(true, robotVision::estimateRobotPose); @@ -102,7 +102,7 @@ public class RobotContainer { */ public RobotContainer() { - // Setup all the neccicery SmartDashboard elements + // Setup all the necessary SmartDashboard elements setupDashboard(); // Configure the button bindings @@ -110,13 +110,13 @@ public RobotContainer() { } /** - * Configures the robot so that the controlls match what was configured on SMartDashboard. (e.g + * Configures the robot so that the controls match what was configured on SMartDashboard. (e.g * whether the robot is being controlled with a flightstick or an xbox controller). */ - public void configureDriverPrefferences() { + public void configureDriverPreferences() { // Get the drive command for the selected controller.(Note that the - // getSelected() method returns the command assosiated with each option, which + // getSelected() method returns the command associated with each option, which // is set above). driveSubsystem.setDefaultCommand(controllerSelection.getSelected()); @@ -153,15 +153,14 @@ private void setupDashboard() { SmartDashboard.putBoolean("Run Auto", false); // Setup autonomous selection. - // Loop through all of the available auto options and add each of them as a - // seperate autonomous option + // Loop through all of the available auto options and add each of them as a separate autonomous option // in SmartDashboard. autonomousSelection.setDefaultOption("RED_1", AutonomousOption.RED_1); for (AutonomousOption autonomousOption : AutonomousOption.values()) { autonomousSelection.addOption(autonomousOption.toString(), autonomousOption); } - // Add all of the configured SmartDashboard elrments to the GUI. + // Add all of the configured SmartDashboard elements to the GUI. SmartDashboard.putData("Controller Selection", controllerSelection); SmartDashboard.putData("Autonomous Selection", autonomousSelection); } @@ -174,7 +173,7 @@ public void updateCommands() { } /** - * Takes a photongraph using all of the cameras. + * Takes a photo using all of the cameras. */ public void snapshot() { robotVision.snapshotAll(); diff --git a/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java b/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java index b18be53..43bb11b 100644 --- a/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java +++ b/src/main/java/frc/robot/commands/vision/ResetImuWithVisionCommand.java @@ -1,11 +1,8 @@ package frc.robot.commands.vision; -import org.photonvision.estimation.RotTrlTransform3d; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.cameras.RobotVision; @@ -33,14 +30,14 @@ public class ResetImuWithVisionCommand extends Command { private boolean isFinished = false; /** - * Creates a new {@code ResetImuWithVisionCommand} with acsess to the specified subsystems. + * Creates a new {@code ResetImuWithVisionCommand} with access to the specified subsystems. * * @param driveSubsystem The robot's drivetrain * @param robotVision The robot's camera manager */ public ResetImuWithVisionCommand(DriveSubsystem driveSubsystem, RobotVision robotVision) { - // Gain acsess to the robot's subsystems so that we are able to interact with them. + // Gain access to the robot's subsystems so that we are able to interact with them. this.driveSubsystem = driveSubsystem; this.robotVision = robotVision; @@ -51,18 +48,18 @@ public ResetImuWithVisionCommand(DriveSubsystem driveSubsystem, RobotVision robo @Override public void initialize() { - // Make it possible to run the comand again. If we don't set isFinished to false every tine - // this command is run, then the command will only sucsessfully run once, since once it completes + // Make it possible to run the command again. If we don't set isFinished to false every tine + // this command is run, then the command will only successfully run once, since once it completes isFinished = false; // Determine whether or not the driver wants to use vision to reset the robot's gyro. // This is done by checking if we pressed the back button within the last 750 milliseconds. if (timer.get() > 0.75 || previousHeading == -999999999) { System.out.println("------------------------------------------------"); - resetGyro(); // We don't want to use vision to rset the robot's gyro. + resetGyro(); // We don't want to use vision to reset the robot's gyro. } - // Reset the timer so that we can keep track of the time that has passed sicne this command + // Reset the timer so that we can keep track of the time that has passed since this command // started running. This helps us determine when we want to reset the robot's heading with // april tags as well as when we want to timer.reset(); @@ -145,7 +142,7 @@ private void restoreOrientation() { * ----------------------------------------------------------------------------------------------- * * Add the robot's previous heading and the rotational offset minus our current heading (How much - * we turrned since we started trying to reset the IMU with vision) together. Doing this is necessary, + * we turned since we started trying to reset the IMU with vision) together. Doing this is necessary, * as the robot may have rotated since when we last stored it's rotation. */ double updatedPreviousHeading = previousHeading + (driveSubsystem.getHeading() diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 87f0cc8..d50cf48 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -79,7 +79,7 @@ public class DriveSubsystem extends SubsystemBase { // other "prevTime" used for displaying motor outputs private double prevTimeForEncoder = WPIUtilJNI.now() * 1e-6; - // variables for storing past motor postions + // variables for storing past motor positions private double currentFrontLeftMeters = 0; private double currentFrontRightMeters = 0; private double currentBackLeftMeters = 0; @@ -100,15 +100,15 @@ public class DriveSubsystem extends SubsystemBase { // SwerveDrivePoseEstimator object to keep track of the robot's position on the field. private SwerveDrivePoseEstimator poseEstimator; - // Create a supplier to make it possible for this DriveSubsystem to gain acsess to the cameras' estimated position. + // Create a supplier to make it possible for this DriveSubsystem to gain access to the cameras' estimated position. private Supplier visionMeasurementSupplier; private Timer elapsedTime = new Timer(); /** - * Creates a new {@code DriveSubsystem} object with the specified paramaters. + * Creates a new {@code DriveSubsystem} object with the specified parameters. * * @param resetOrientation Whether or not the IMU will be reset. - * @param visionMeasurementSupplier A refference to a method that will provide the drivetrain with the + * @param visionMeasurementSupplier A reference to a method that will provide the drivetrain with the */ public DriveSubsystem(boolean resetOrientation, Supplier visionMeasurementSupplier) { @@ -117,13 +117,13 @@ public DriveSubsystem(boolean resetOrientation, Supplier visionMeasureme zeroHeading();; } - // Stert the time so that we are able to get time stamped vision measurments. + // Start the time so that we are able to get time stamped vision measurements. elapsedTime.start(); /* - * Initialze up the visionMeasurementSupplier so that this DriveSubsystem is able to get + * Initialize up the visionMeasurementSupplier so that this DriveSubsystem is able to get * the camera(s) estimate of the robot's position. This helps ensure the robot is able to - * reliable prefrom autonomous action, like autoscoring and auto note grabbing. + * reliable preform autonomous action, like autoscaling and auto note grabbing. */ this.visionMeasurementSupplier = visionMeasurementSupplier; @@ -293,7 +293,7 @@ public void setTrajectory(List trajectory) { */ public void followTrajectory() { if (waypoint < trajectory.size()) { - if ( (double) getPose().getTranslation().getDistance(trajectory.get(waypoint).getTranslation()) < AutoConstants.TRAJECOTRY_THRESHOLD) { + if ( (double) getPose().getTranslation().getDistance(trajectory.get(waypoint).getTranslation()) < AutoConstants.TRAJECTORY_THRESHOLD) { waypoint += 1; } else { driveToPosition(trajectory.get(waypoint)); @@ -413,7 +413,7 @@ public void driveFromController(double leftX, double leftY, double rightX, doubl } - // applys driving to the robot + // Applies driving to the robot drive(leftX, leftY, rotSpeedCommanded, boostMode, true, true); } @@ -609,18 +609,18 @@ public void resetOdometry(Pose2d pose) { } /** - * Sets the direciton that the robot is facing to the sepecified value. + * Sets the direction that the robot is facing to the specified value. * - * @param newYaw The direciton you want the robot to think it's facing + * @param newYaw The direction you want the robot to think it's facing */ public void setYaw(Rotation2d newYaw) { gyro.setYaw(newYaw.getDegrees()); } /** - * Sets the direciton that the robot is facing to the sepecified value. + * Sets the direction that the robot is facing to the specified value. * - * @param newYaw The direciton you want the robot to think it's facing + * @param newYaw The direction you want the robot to think it's facing */ public void setYaw(double newYaw) { gyro.setYaw(newYaw); diff --git a/src/main/java/frc/robot/subsystems/cameras/LimelightCamera.java b/src/main/java/frc/robot/subsystems/cameras/LimelightCamera.java index 2dc0551..a2c424d 100644 --- a/src/main/java/frc/robot/subsystems/cameras/LimelightCamera.java +++ b/src/main/java/frc/robot/subsystems/cameras/LimelightCamera.java @@ -203,7 +203,7 @@ public EstimatedRobotPose3d estimateRobotPose3d() { // Get an array containing the values of the robot's calculated position on the field. double[] botposeValues = getValue("botpose").getDoubleArray(new double[6]); - // Return an EstimatedRobotPose3d containing the caera's estimante of the robot's position on the field. + // Return an EstimatedRobotPose3d containing the camera's estimate of the robot's position on the field. return new EstimatedRobotPose3d( botposeValues[0], botposeValues[1], botposeValues[2], // Robot's position on the filed (x, y, z) in meters. botposeValues[3], botposeValues[4], botposeValues[5] // Robot's rotation (roll, pitch, yaw) in radians. diff --git a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java index 96c7cb1..55ad503 100644 --- a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java +++ b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java @@ -15,7 +15,7 @@ import frc.robot.Constants.VisionConstants; /** - * A camera attatched to a co-processer. + * A camera attached to a co-processor. * * @author Cameron Myhre * @version 1.0 @@ -42,8 +42,9 @@ public PhotonVisionCamera(String cameraName, Transform3d cameraOffset) { // Set the camera offset this.cameraOffset = cameraOffset; + this.cameraOffset.getRotation(); - // Initialize this camera's PhotonPoseEstimaotr so that we are able to estimate + // Initialize this camera's PhotonPoseEstimator so that we are able to estimate // the robot's position. photonPoseEstimator = new PhotonPoseEstimator(VisionConstants.APRIL_TAG_FIELD_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, cameraOffset); @@ -59,7 +60,7 @@ public void periodic() { /* * Output values so that I, Cameron, can figure out how photonvision's Object Detection code * works. This needs to be done because there isn't any documentation, and the people on the discord - * server effectibly told me to just figure it out. + * server effectively told me to just figure it out. */ if (result.hasTargets()) { @@ -87,10 +88,10 @@ public void takeOutputSnapshot() { } /** - * Estimates the robot's position on the filed using all of the visilbe + * Estimates the robot's position on the filed using all of the visible * AprilTags and returns the result. * - * @return An estimante of the robot's positon on the field. + * @return An estimate of the robot's position on the field. */ public Optional estimateRobotPose() { @@ -119,7 +120,7 @@ public void setPipeline(int pipeline) { */ public PhotonTrackedTarget getDistanceToTag(int id) { - // Make sure that this camera is on the ApilTag detection pipeline. If it isn't, + // Make sure that this camera is on the AprilTag detection pipeline. If it isn't, // then print an error and return null. if (camera.getPipelineIndex() != VisionConstants.APRIL_TAG_PIPELINE) { diff --git a/src/main/java/frc/robot/subsystems/cameras/RobotVision.java b/src/main/java/frc/robot/subsystems/cameras/RobotVision.java index 07d9a26..76791d8 100644 --- a/src/main/java/frc/robot/subsystems/cameras/RobotVision.java +++ b/src/main/java/frc/robot/subsystems/cameras/RobotVision.java @@ -43,7 +43,7 @@ public static class Builder { * Creates a new PhotonVisionCamera object and add it to this * RobotVision object's list of processed cameras. * - * @param cameraName The name of the camera you want to gain acsess to. + * @param cameraName The name of the camera you want to gain access to. * @param cameraOffset This camera's position relative to this robot's center. * @param pipeline The vision pipeline this camera will process images with. */ @@ -77,10 +77,10 @@ public Builder addPhotonVisionCamera(PhotonVisionCamera camera) { } /** - * Create a new LimelightCamera object with the desired paramaters and add it + * Create a new LimelightCamera object with the desired parameters and add it * to this RobotVision 's hashMap of processed limelight cameras. * - * @param networktableName The anme of the network table that this limelight is using. + * @param networktableName The name of the network table that this limelight is using. * @param ledMode The LEDMode you want this limelight to use. *
    *
  • 0: Use the LED Mode set in the current pipeline
    • @@ -104,7 +104,7 @@ public Builder addPhotonVisionCamera(PhotonVisionCamera camera) { */ public Builder addLimelightCamera(String networktableName, int ledMode, int camMode, int pipeline, int streamMode) { - // Create a new limelgith camera + // Create a new limelight camera LimelightCamera newLimelight = new LimelightCamera(networktableName, ledMode, camMode, pipeline, streamMode); // Add the limelight to the list of limelight camera. @@ -144,8 +144,8 @@ public RobotVision build() { public void periodic() { /* - * Try and estimate where the robot is on the field. Then, if we are able to sucsessfully - * estimate th erobot's position, dispay the results on SmartDashboard so that we can determine + * Try and estimate where the robot is on the field. Then, if we are able to successfully + * estimate the robot's position, display the results on SmartDashboard so that we can determine * how accurate our estimation is. */ Pose2d estimatedRobotPose = estimateRobotPose(); @@ -157,7 +157,7 @@ public void periodic() { } /** - * Estimates the robot's positon on the field using by using the position data gathered from + * Estimates the robot's position on the field using by using the position data gathered from * visible AprilTags and returns the value. If the robot can't see any AprilTags, then this * method will return null. * @@ -167,57 +167,57 @@ public Pose2d estimateRobotPose() { // Create a new ArrayList to store all of the estimated robot positions. EstimatedRobotPose3d estimatedRobotPosition = new EstimatedRobotPose3d(); - int numEstimatedPositons = 0; + int numEstimatedPositions = 0; // Loop through all of this RobotVision's PhotonVisionCameras and get their estimated robot position for (PhotonVisionCamera camera : photonVisionCameras) { - // Get the camera's estimante of the robot's positon on the field. If the camera was unable to estimate a - // positon, then skip over to the next camera. + // Get the camera's estimate of the robot's position on the field. If the camera was unable to estimate a + // position, then skip over to the next camera. Optional optionalEstimatedPose = camera.estimateRobotPose(); if (optionalEstimatedPose.isEmpty()) { continue; } // Get the EstimatedRobotPose from optionalEstimatedPosition since we know the value isn't null. - EstimatedRobotPose estimateddRobotPose = optionalEstimatedPose.get(); + EstimatedRobotPose estimatedRobotPose = optionalEstimatedPose.get(); // Get the estimated position and convert it to a EstimatedRobotPose3d. Then add it to estimatedRobotPosition - // so that we can average out each camera's estimated position and get a more accurate estimante. - EstimatedRobotPose3d estimateddRobotPose3d = new EstimatedRobotPose3d(estimateddRobotPose.estimatedPose); - estimatedRobotPosition.add(estimateddRobotPose3d); + // so that we can average out each camera's estimated position and get a more accurate estimate. + EstimatedRobotPose3d estimatedRobotPose3d = new EstimatedRobotPose3d(estimatedRobotPose.estimatedPose); + estimatedRobotPosition.add(estimatedRobotPose3d); - // Add 1 to the numEstimatedPositons so that we can keep track of the total number of positions used in - // the final estimante. This helps ensure our final position is as accurate as possible. - numEstimatedPositons++; + // Add 1 to the numEstimatedPositions so that we can keep track of the total number of positions used in + // the final estimate. This helps ensure our final position is as accurate as possible. + numEstimatedPositions++; } // Loop through all of this RobotVision's LimelightCameras and get their estimated robot position for (LimelightCamera camera : limelightCameras) { - // Get the camera's estimante of the robot's positon on the field. If the camera was unable to estimate a - // positon, then skip over to the next camera. + // Get the camera's estimate of the robot's position on the field. If the camera was unable to estimate a + // position, then skip over to the next camera. EstimatedRobotPose3d estimatedRobotPose3d = camera.estimateRobotPose3d(); if (estimatedRobotPose3d == null) { continue; } - // Aadd the camera's estimated position to estimatedRobotPosition so that we can average out each camera's - // estimated position and get a more accurate estimante. + // Add the camera's estimated position to estimatedRobotPosition so that we can average out each camera's + // estimated position and get a more accurate estimate. estimatedRobotPosition.add(estimatedRobotPose3d); - // Add 1 to the numEstimatedPositons so that we can keep track of the total number of positions used in - // the final estimante. This helps ensure our final position is as accurate as possible. - numEstimatedPositons++; + // Add 1 to the numEstimatedPositions so that we can keep track of the total number of positions used in + // the final estimate. This helps ensure our final position is as accurate as possible. + numEstimatedPositions++; } // Return null if we can't see any tags. - if (numEstimatedPositons == 0) { + if (numEstimatedPositions == 0) { return null; } // Average out all of the estimated values - estimatedRobotPosition.div(numEstimatedPositons); + estimatedRobotPosition.div(numEstimatedPositions); return estimatedRobotPosition.toPose3d().toPose2d(); } @@ -339,7 +339,7 @@ public void snapshotAll() { } /** - * Returns a {@code PhotonVisionCamera} object with the speicfied name. + * Returns a {@code PhotonVisionCamera} object with the specified name. * * @param cameraName The name of the {@code PhotonVisionCamera} you want to get. * @return A {@code PhotonVisionCamera} with the specified name. (Or null if no camera with the specified @@ -349,7 +349,7 @@ public PhotonVisionCamera getPhotonVisionCamera(String cameraName) { /* * Loop through all of this RobotVision's PhotonVisionCameras and attempt to find a camera with - * the specified name. If a camera with the specified anme can be found, then return it. Otherwise + * the specified name. If a camera with the specified name can be found, then return it. Otherwise * look through the limelight cameras and try to find the camera there. */ for (PhotonVisionCamera camera : photonVisionCameras) { @@ -366,7 +366,7 @@ public PhotonVisionCamera getPhotonVisionCamera(String cameraName) { } /** - * Returns a {@code LimelightCamera} object with the speicfied name. + * Returns a {@code LimelightCamera} object with the specified name. * * @param cameraName The name of the {@code LimelightCamera} you want to get. * @return A {@code LimelightCamera} with the specified name. (Or null if no camera with the specified diff --git a/src/main/java/frc/utils/Autonomous/AutonomousOption.java b/src/main/java/frc/utils/Autonomous/AutonomousOption.java index c9675ae..01936f6 100644 --- a/src/main/java/frc/utils/Autonomous/AutonomousOption.java +++ b/src/main/java/frc/utils/Autonomous/AutonomousOption.java @@ -69,14 +69,29 @@ public List getTrajectory() { return this.TRAJECTORY; } + /** + * Returns the alliance that this auto is associated with. + * + * @return The alliance that this auto is associated with. + */ public Alliance getAlliance() { return this.ALLIANCE; } + /** + * Returns the position of this auto's target amp. + * + * @return The position of this auto's target amp. + */ public Pose2d getAmpPosition() { return this.AMP_POSITION; } + /** + * Returns the position of this auto's target speaker. + * + * @return The position of this auto's target speaker. + */ public Pose2d getSpeakerPosition() { return this.SPEAKER_POSITION; } diff --git a/src/main/java/frc/utils/geometry/EstimatedRobotPose3d.java b/src/main/java/frc/utils/geometry/EstimatedRobotPose3d.java index 8b05a2e..6979b10 100644 --- a/src/main/java/frc/utils/geometry/EstimatedRobotPose3d.java +++ b/src/main/java/frc/utils/geometry/EstimatedRobotPose3d.java @@ -140,10 +140,10 @@ public Pose3d getDistanceTo(EstimatedRobotPose3d position) { /** * Returns a {@code Rotation3d} object specifying how much this robot would have to rotate - * to face the same direciton. + * to face the same direction. * * @param rotation The rotation you want to get the distance to. - * @return A {@code Pose3d} object specifying the amout that this robot would have to rotate + * @return A {@code Pose3d} object specifying the amount that this robot would have to rotate * in each */ public Pose3d getDistanceTo(Rotation3d rotation) { @@ -160,7 +160,7 @@ public Pose3d getDistanceTo(Rotation3d rotation) { /** * Returns the distance this robot would have to travel in order to reach the specified location. * - * @param position The positon you want to get the distance to. + * @param position The position you want to get the distance to. * @return The distance to the specified location as a {@code Pose3d} object. */ public Pose3d getDistanceTo(Translation3d position) { @@ -176,7 +176,7 @@ public Pose3d getDistanceTo(Translation3d position) { /** * Returns a {@code Pose3d} object specifying how much the robot will have to drive an rotate along - * each axis in order to reach the speicifed positon. + * each axis in order to reach the specified position. * * @param position The position you want to get the distance to. * @return The distance to the specified position as a {@code Pose3d} object. @@ -209,10 +209,10 @@ public Pose3d toPose3d() { } /** - * Adds the specified {@code EstimatedRobotPose3d}'s valoes to this + * Adds the specified {@code EstimatedRobotPose3d}'s values to this * {@code EstimatedRobotPose3d}'s values. * - * @param robotPose The {@code EstimatedRobotPose3d} who's valoes are to be added to this + * @param robotPose The {@code EstimatedRobotPose3d} who's values are to be added to this * {@code EstimatedRobotPose3d}'s values. */ public void add(EstimatedRobotPose3d robotPose) { @@ -225,10 +225,29 @@ public void add(EstimatedRobotPose3d robotPose) { } /** - * Subtracts the specified {@code EstimatedRobotPose3d}'s valoes from this + * Adds the specified {@code Pose3d}'s values to this + * {@code EstimatedRobotPose3d}'s values. + * + * @param robotPose The {@code Pose3d} who's values are to be added to this + * {@code EstimatedRobotPose3d}'s values. + */ + public void add(Pose3d pose3d) { + this.x += pose3d.getX(); + this.y += pose3d.getY(); + this.z += pose3d.getZ(); + + // Get the rotation from the specified Pose3d and add its values to this RobotPose3d's values. + Rotation3d rotation = pose3d.getRotation(); + this.roll += rotation.getX(); + this.pitch += rotation.getY(); + this.yaw += rotation.getZ(); + } + + /** + * Subtracts the specified {@code EstimatedRobotPose3d}'s values from this * {@code EstimatedRobotPose3d}'s values. * - * @param robotPose The {@code EstimatedRobotPose3d} who's valoes are to be subtracted from this + * @param robotPose The {@code EstimatedRobotPose3d} who's values are to be subtracted from this * {@code EstimatedRobotPose3d}'s values. */ public void minus(EstimatedRobotPose3d robotPose) { @@ -240,6 +259,25 @@ public void minus(EstimatedRobotPose3d robotPose) { this.yaw -= robotPose.getYaw(); } + /** + * Subtracts the specified {@code Pose3d}'s values from this + * {@code EstimatedRobotPose3d}'s values. + * + * @param robotPose The {@code EstimatedRobotPose3d} who's values are to be subtracted from this + * {@code EstimatedRobotPose3d}'s values. + */ + public void minus(Pose3d pose3d) { + this.x -= pose3d.getX(); + this.y -= pose3d.getY(); + this.z -= pose3d.getZ(); + + // Get the rotation from the specified Pose3d and add its values to this RobotPose3d's values. + Rotation3d rotation = pose3d.getRotation(); + this.roll -= rotation.getX(); + this.pitch -= rotation.getY(); + this.yaw -= rotation.getZ(); + } + /** * Multiplies all values of this {@code EstimatedRobotPose3d} by the specified multiplier. * @@ -273,7 +311,7 @@ public void div(double divisor) { /** * Sets this {@code EstimatedRobotPose3d}'s x coordinate to the specified value. * - * @param newX The new x coordiante of this {@code EstimatedRobotPose3d} in meters. + * @param newX The new x coordinate of this {@code EstimatedRobotPose3d} in meters. */ public void setX(double newX) { From cb1632b77f70b43fd209219c1c26089d010afa80 Mon Sep 17 00:00:00 2001 From: Cameron Myhre Date: Thu, 21 Mar 2024 17:01:22 -0500 Subject: [PATCH 4/8] feat: Add the ability to see detected object's position on the screen. --- .../cameras/PhotonVisionCamera.java | 19 +++++++++++++++++-- .../robot/subsystems/cameras/RobotVision.java | 1 - 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java index 55ad503..30bd85b 100644 --- a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java +++ b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java @@ -9,8 +9,10 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.VisionConstants; @@ -64,10 +66,23 @@ public void periodic() { */ if (result.hasTargets()) { - if(result.getTargets().isEmpty()) { - System.out.println("Work"); + List targets = result.getTargets(); + if(targets.isEmpty()) { return; } + + // Get the detects object's position on the screen and display it on SmartDashboard. + PhotonTrackedTarget target = targets.get(0); + List tagCorners = target.getDetectedCorners(); + SmartDashboard.putNumber("Bottom Left Corner X: ", tagCorners.get(0).x); + SmartDashboard.putNumber("Bottom Left Corner Y: ", tagCorners.get(0).x); + SmartDashboard.putNumber("Bottom Right Corner X: ", tagCorners.get(1).x); + SmartDashboard.putNumber("Bottom Right Corner Y: ", tagCorners.get(1).x); + SmartDashboard.putNumber("Top Right Corner X: ", tagCorners.get(2).x); + SmartDashboard.putNumber("Top Right Corner Y: ", tagCorners.get(2).x); + SmartDashboard.putNumber("Top Left Corner X: ", tagCorners.get(3).x); + SmartDashboard.putNumber("Top Left Corner Y: ", tagCorners.get(3).x); + } } diff --git a/src/main/java/frc/robot/subsystems/cameras/RobotVision.java b/src/main/java/frc/robot/subsystems/cameras/RobotVision.java index 76791d8..97f0d60 100644 --- a/src/main/java/frc/robot/subsystems/cameras/RobotVision.java +++ b/src/main/java/frc/robot/subsystems/cameras/RobotVision.java @@ -331,7 +331,6 @@ public void snapshotAll() { // Loop through all of then cameras and take a snapshot (photograph) using each camera. for (PhotonVisionCamera camera : photonVisionCameras) { camera.takeInputSnapshot(); - System.out.println("Snap!"); } for (LimelightCamera camera : limelightCameras) { camera.snapshot(); From edba1e358914b66d5d75ca6d09aaaa31580320b2 Mon Sep 17 00:00:00 2001 From: NotAppIicable Date: Fri, 22 Mar 2024 16:04:24 -0500 Subject: [PATCH 5/8] feat: Add various vision improvements --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../cameras/PhotonVisionCamera.java | 19 +++++++++++++------ vendordeps/REVLib.json | 10 +++++----- vendordeps/photonlib.json | 10 +++++----- 5 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a030d51..f60d27c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -38,7 +38,7 @@ public void robotInit() { snapshotTimer.start(); // Mak eit possible to view cameras on the driver station. - CameraServer.startAutomaticCapture(); + // CameraServer.startAutomaticCapture(); // Make it possible to view the photonvision dashboard over the internet PortForwarder.add(5800, "photonvision.local", 5800); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fd2e514..aa4c0fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,7 +34,7 @@ public class RobotContainer { // The robot's subsystems private final RobotVision robotVision = new RobotVision.Builder() .addPhotonVisionCamera(VisionConstants.COLOR_CAMERA_NAME, VisionConstants.BLACK_INTAKE_CAMERA_OFFSET, - VisionConstants.APRIL_TAG_PIPELINE) + VisionConstants.OBJECT_DETECTION_PIPELINE) .build(); private final DriveSubsystem driveSubsystem = new DriveSubsystem(true, robotVision::estimateRobotPose); diff --git a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java index 30bd85b..93c79d2 100644 --- a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java +++ b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java @@ -67,21 +67,28 @@ public void periodic() { if (result.hasTargets()) { List targets = result.getTargets(); - if(targets.isEmpty()) { + if(targets.isEmpty() || targets.get(0) == null) { return; } // Get the detects object's position on the screen and display it on SmartDashboard. PhotonTrackedTarget target = targets.get(0); - List tagCorners = target.getDetectedCorners(); + List tagCorners = target.getMinAreaRectCorners(); + if (tagCorners.isEmpty()) { + return; + } SmartDashboard.putNumber("Bottom Left Corner X: ", tagCorners.get(0).x); - SmartDashboard.putNumber("Bottom Left Corner Y: ", tagCorners.get(0).x); + SmartDashboard.putNumber("Bottom Left Corner Y: ", tagCorners.get(0).y); SmartDashboard.putNumber("Bottom Right Corner X: ", tagCorners.get(1).x); - SmartDashboard.putNumber("Bottom Right Corner Y: ", tagCorners.get(1).x); + SmartDashboard.putNumber("Bottom Right Corner Y: ", tagCorners.get(1).y); SmartDashboard.putNumber("Top Right Corner X: ", tagCorners.get(2).x); - SmartDashboard.putNumber("Top Right Corner Y: ", tagCorners.get(2).x); + SmartDashboard.putNumber("Top Right Corner Y: ", tagCorners.get(2).y); SmartDashboard.putNumber("Top Left Corner X: ", tagCorners.get(3).x); - SmartDashboard.putNumber("Top Left Corner Y: ", tagCorners.get(3).x); + SmartDashboard.putNumber("Top Left Corner Y: ", tagCorners.get(3).y); + + SmartDashboard.putNumber("Object Y: ",(tagCorners.get(3).y + tagCorners.get(1).y) / 2); + SmartDashboard.putNumber("Object X: ",(tagCorners.get(3).x + tagCorners.get(1).x) / 2); + } } diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf8..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 77641e4..0e80a16 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.9", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.9" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.9" + "version": "v2024.3.1" } ] } \ No newline at end of file From 353f02a483fca9f2f7d00fc774c10f437737c3c9 Mon Sep 17 00:00:00 2001 From: NotAppIicable Date: Tue, 26 Mar 2024 22:31:47 -0500 Subject: [PATCH 6/8] feat: Add complex auto path functunality This commit should allow for more complex autonomous routines to be preformed. Each pre-existing auto has its own associated complex autonomous routine which can be run by enabling the "Complex Auto" option in SmartDashboard. These --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 87 ++++++-- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 197 +++++++++++++++++- .../commands/DriveToPositionCommand.java | 36 ++++ .../commands/FollowTrajectoryCommand.java | 74 +++++++ .../commands/vision/AlignToNoteCommand.java | 0 .../frc/robot/subsystems/DriveSubsystem.java | 2 +- .../cameras/PhotonVisionCamera.java | 18 +- .../java/frc/utils/vision/DetectedObject.java | 119 +++++++++++ 10 files changed, 506 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveToPositionCommand.java create mode 100644 src/main/java/frc/robot/commands/FollowTrajectoryCommand.java create mode 100644 src/main/java/frc/robot/commands/vision/AlignToNoteCommand.java create mode 100644 src/main/java/frc/utils/vision/DetectedObject.java diff --git a/build.gradle b/build.gradle index c73a804..462fb1d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 525e736..12bcc3c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -36,7 +37,7 @@ public final class Constants { public static final class DriveConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds - public static final double MAX_SPEED_METERS_PER_SECOND = 2; // 2 // meters per second + public static final double MAX_SPEED_METERS_PER_SECOND = 2.0; // 2 // meters per second public static final double BOOST_MODE_MAX_SPEED_METERS_PER_SECOND = 4.8; // meters per second public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // radians per second @@ -211,39 +212,68 @@ public static final class AutoConstants { public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED); - public static final Pose2d BLUE_1_STARTING_POSE = new Pose2d(1.665, 6.636, Rotation2d.fromRadians((1.0/3.0) * Math.PI)); - public static final Pose2d BLUE_2_STARTING_POSE = new Pose2d(2.362, 5.484, Rotation2d.fromRadians(0)); - public static final Pose2d BLUE_3_STARTING_POSE = new Pose2d(1.689, 4.451, Rotation2d.fromRadians((5.0/3.0) * Math.PI)); - public static final Pose2d RED_1_STARTING_POSE = new Pose2d(16.238, 6.650, Rotation2d.fromRadians((2.0/3.0) * Math.PI )); - public static final Pose2d RED_2_STARTING_POSE = new Pose2d(15.481, 5.593, Rotation2d.fromRadians(Math.PI)); - public static final Pose2d RED_3_STARTING_POSE = new Pose2d(16.322, 4.367, Rotation2d.fromRadians((4.0/3.0) * Math.PI)); + public static final Pose2d BLUE_1_STARTING_POSE = new Pose2d(1.629, 6.457, Rotation2d.fromRadians(0)); + public static final Pose2d BLUE_2_STARTING_POSE = new Pose2d(1.278, 5.603, Rotation2d.fromRadians(0)); + public static final Pose2d BLUE_3_STARTING_POSE = new Pose2d(1.629, 4.749, Rotation2d.fromRadians(0)); + public static final Pose2d RED_1_STARTING_POSE = new Pose2d(14.884, 6.457, Rotation2d.fromRadians(Math.PI)); + public static final Pose2d RED_2_STARTING_POSE = new Pose2d(15.235, 5.603, Rotation2d.fromRadians(Math.PI)); + public static final Pose2d RED_3_STARTING_POSE = new Pose2d(14.884, 4.749, Rotation2d.fromRadians(Math.PI)); + // Old paths + public static final List RED_1_TRAJECTORY = List.of( + new Pose2d(15.036,7.671, RED_1_STARTING_POSE.getRotation()), + new Pose2d(15.072,7.671, RED_1_STARTING_POSE.getRotation()) + ); + public static final List RED_2_TRAJECTORY = List.of( + new Pose2d(12.429,5.713, RED_2_STARTING_POSE.getRotation()) + ); + public static final List RED_3_TRAJECTORY = List.of( + new Pose2d(14.940,1.580, RED_3_STARTING_POSE.getRotation()), + new Pose2d(13.475,1.195, RED_3_STARTING_POSE.getRotation()) + ); public static final List BLUE_1_TRAJECTORY = List.of( new Pose2d(2.926,7.563, BLUE_1_STARTING_POSE.getRotation()), new Pose2d(5.497,7.563, BLUE_1_STARTING_POSE.getRotation()) ); - public static final List BLUE_2_TRAJECTORY = List.of( new Pose2d(5.293,5.593, BLUE_2_STARTING_POSE.getRotation()) ); - public static final List BLUE_3_TRAJECTORY = List.of( new Pose2d(3.503,0.931, BLUE_3_STARTING_POSE.getRotation()), new Pose2d(4.440,0.931, BLUE_3_STARTING_POSE.getRotation()) ); - public static final List RED_1_TRAJECTORY = List.of( - new Pose2d(15.036,7.671, RED_1_STARTING_POSE.getRotation()), - new Pose2d(15.072,7.671, RED_1_STARTING_POSE.getRotation()) + // New Paths + public static final List BLUE_1_COMPLEX_POSITIONS = List.of( + new Pose2d(1.629, 7.807, Rotation2d.fromDegrees(0)), // Close to amp position + new Pose2d(7.717, 7.436, Rotation2d.fromDegrees(0)), // Topmost note + new Pose2d(7.717, 5.785, Rotation2d.fromDegrees(0)) // Second top note ); - - public static final List RED_2_TRAJECTORY = List.of( - new Pose2d(12.429,5.713, RED_2_STARTING_POSE.getRotation()) + public static final List BLUE_2_COMPLEX_POSITIONS = List.of( + new Pose2d(2.273, 4.088, Rotation2d.fromDegrees(0)), // Bottom Note + new Pose2d(2.273, 5.569, Rotation2d.fromDegrees(0)), // Middle Note + new Pose2d(2.273, 7.004, Rotation2d.fromDegrees(0)) // Top Note + ); + public static final List BLUE_3_COMPLEX_POSITIONS = List.of( + new Pose2d(4.726, 4.840, Rotation2d.fromDegrees(0)), // Position under triangle thingy + new Pose2d(7.717, 4.099, Rotation2d.fromDegrees(0)), // Center note in center of field. + new Pose2d(7.717, 2.425, Rotation2d.fromDegrees(0)) // Second to last note in center of field. ); - public static final List RED_3_TRAJECTORY = List.of( - new Pose2d(14.940,1.580, RED_3_STARTING_POSE.getRotation()), - new Pose2d(13.475,1.195, RED_3_STARTING_POSE.getRotation()) + public static final List RED_1_COMPLEX_POSITIONS = List.of( + new Pose2d(14.884, 7.807, Rotation2d.fromDegrees(0)), // Close to amp position + new Pose2d(8.796, 7.436, Rotation2d.fromDegrees(0)), // Topmost note + new Pose2d(8.796, 5.785, Rotation2d.fromDegrees(0)) // Second top note + ); + public static final List RED_2_COMPLEX_POSITIONS = List.of( + new Pose2d(14.24, 4.088, Rotation2d.fromDegrees(0)), // Bottom Note + new Pose2d(14.24, 5.569, Rotation2d.fromDegrees(0)), // Middle Note + new Pose2d(14.24, 7.004, Rotation2d.fromDegrees(0)) // Top Note + ); + public static final List RED_3_COMPLEX_POSITIONS = List.of( + new Pose2d(11.787, 4.840, Rotation2d.fromDegrees(0)), // Position under triangle thingy + new Pose2d(8.796, 4.099, Rotation2d.fromDegrees(0)), // Center note in center of field. + new Pose2d(8.796, 2.425, Rotation2d.fromDegrees(0)) // Second to last note in center of field. ); } @@ -254,6 +284,27 @@ public static final class NeoMotorConstants { public static final class FieldConstants { public static final Pose2d SPEAKER_POSE = new Pose2d(0, 0, new Rotation2d()); public static final double SPEAKER_HEIGHT = 81; + + // Speaker Scoring Locations + public static final Pose2d RED_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION = + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)); + public static final Pose2d RED_ALLIANCE_SPEAKER_LEFT_SCORING_LOCATION = + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)); + public static final Pose2d RED_ALLIANCE_SPEAKER_RIGHT_SCORING_LOCATION = + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)); + + public static final Pose2d BLUE_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION = + new Pose2d(new Translation2d(1.278, 5.603), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_ALLIANCE_SPEAKER_LEFT_SCORING_LOCATION = + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_ALLIANCE_SPEAKER_RIGHT_SCORING_LOCATION = + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)); + + // AMP Scoring locations + public static final Pose2d RED_AMP_SCORING_POSITION = + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_AMP_SCORING_POSITION = + new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0)); } public static final class IntakeConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f60d27c..ae793a3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; -import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aa4c0fb..15e5e45 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import java.util.List; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -14,10 +16,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import frc.robot.Constants.VisionConstants; +import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.OIConstants; +import frc.robot.commands.FollowTrajectoryCommand; import frc.robot.commands.vision.ResetImuWithVisionCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.cameras.RobotVision; @@ -33,8 +39,8 @@ public class RobotContainer { // The robot's subsystems private final RobotVision robotVision = new RobotVision.Builder() - .addPhotonVisionCamera(VisionConstants.COLOR_CAMERA_NAME, VisionConstants.BLACK_INTAKE_CAMERA_OFFSET, - VisionConstants.OBJECT_DETECTION_PIPELINE) + .addPhotonVisionCamera(VisionConstants.ARDUCAM_OV9281_USB_CAMERA, VisionConstants.RED_OUTTAKE_CAMERA_OFFSET, + VisionConstants.APRIL_TAG_PIPELINE) .build(); private final DriveSubsystem driveSubsystem = new DriveSubsystem(true, robotVision::estimateRobotPose); @@ -151,6 +157,7 @@ private void setupDashboard() { // Determines whether or not we want to run autonomous. SmartDashboard.putBoolean("Run Auto", false); + SmartDashboard.putBoolean("Complex Auto", false); // Setup autonomous selection. // Loop through all of the available auto options and add each of them as a separate autonomous option @@ -160,6 +167,8 @@ private void setupDashboard() { autonomousSelection.addOption(autonomousOption.toString(), autonomousOption); } + // Setup the ability to use complex autonomous paths + // Add all of the configured SmartDashboard elements to the GUI. SmartDashboard.putData("Controller Selection", controllerSelection); SmartDashboard.putData("Autonomous Selection", autonomousSelection); @@ -189,11 +198,28 @@ public Command getAutonomousCommand() { // Get the selected auto AutonomousOption selectedAuto = autonomousSelection.getSelected(); - // Start making the robot follow a trajectory - RunCommand autonomousCommand = new RunCommand( - () -> auto() - ); + // Setup the drivetrain to work with the selected auto. + driveSubsystem.setAutonomous(selectedAuto); + + /* + * Check whether or not the driver wants to run a complex autonomous path. If they do, then get + * the complex path for the selected auto. Otherwise, make the robot follow the basic trajectory associated + * with the selected auto. + */ + SequentialCommandGroup selectedAutonomousRoutine = null; + if (SmartDashboard.getBoolean("Complex Auto", false)) { + // Get complicated autonomous routine associated with the selected auto. + selectedAutonomousRoutine = getComplexPath(selectedAuto); + } else { + + // Follow the basic pre-planned path + selectedAutonomousRoutine = new SequentialCommandGroup( + new FollowTrajectoryCommand( + driveSubsystem, + selectedAuto.getTrajectory()) + ); + } // Configure the robot's settings so that it will be optimized for the selected command. driveSubsystem.setAutonomous(selectedAuto); @@ -201,11 +227,168 @@ public Command getAutonomousCommand() { if (SmartDashboard.getBoolean("Run Auto", false)) { // Run path following command, then stop at the end. - return autonomousCommand.andThen(() -> driveSubsystem.drive(0, 0, 0, 0, false, false)); + return selectedAutonomousRoutine; } return null; } + /** + * Returns the complex autonomous routine associated with the specified autonomous routine. + * + * @param autonomousOption The autonomous routine that was selected by the driver. + * @return A {@code SequentialCommandGroup} containing a more advanced autonomous routine. A few examples of complicated + * autonomous routines would be autos that score multiple notes or preform more actions than driving along a + * single preplanned path. + */ + private SequentialCommandGroup getComplexPath(AutonomousOption autonomousOption) { + + // Create a empty variable to store the autonomous command. + SequentialCommandGroup complexPath; + + // Create a complex autonomous command for each auto. See the + switch (autonomousOption) { + case BLUE_1: + complexPath = new SequentialCommandGroup( + // Shoot note + new FollowTrajectoryCommand(driveSubsystem, // Drive up to the amp and then grab the note closest to the wall near the amp. + AutoConstants.BLUE_1_COMPLEX_POSITIONS.subList(0, 1)), + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.BLUE_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp + AutoConstants.BLUE_1_STARTING_POSE)), // Drive to the original starting location + // Score note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.BLUE_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp + AutoConstants.BLUE_1_COMPLEX_POSITIONS.get(2))), // Drive to the lower of the two targeted notes + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.BLUE_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp + AutoConstants.BLUE_1_STARTING_POSE)) // Drive to the original starting location + // Score note + ); + break; + case BLUE_2: + complexPath = new SequentialCommandGroup( + new FollowTrajectoryCommand(driveSubsystem, + List.of(AutoConstants.BLUE_2_COMPLEX_POSITIONS.get(0))), // Drive to top note + // Grab note + new FollowTrajectoryCommand(driveSubsystem, + List.of(FieldConstants.BLUE_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION)), // Return to the speaker + // Shoot note + new FollowTrajectoryCommand(driveSubsystem, + List.of(AutoConstants.BLUE_2_COMPLEX_POSITIONS.get(1))), // Drive to center note + // Grab note + new FollowTrajectoryCommand(driveSubsystem, + List.of(FieldConstants.BLUE_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION)), // Return to the speaker + // Shoot note + new FollowTrajectoryCommand(driveSubsystem, + List.of(AutoConstants.BLUE_2_COMPLEX_POSITIONS.get(2))), // Drive to bottom note + // Grab note + new FollowTrajectoryCommand(driveSubsystem, + List.of(FieldConstants.BLUE_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION)) // Return to the speaker + // Shoot note + ); + break; + case BLUE_3: + complexPath = new SequentialCommandGroup( + // Score note + new FollowTrajectoryCommand(driveSubsystem, + AutoConstants.BLUE_3_COMPLEX_POSITIONS.subList(0, 1)), + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.BLUE_3_COMPLEX_POSITIONS.get(0), + AutoConstants.BLUE_3_STARTING_POSE + )), + // Score note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.BLUE_3_COMPLEX_POSITIONS.get(0), + AutoConstants.BLUE_3_COMPLEX_POSITIONS.get(2) + )), + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.BLUE_3_COMPLEX_POSITIONS.get(0), + AutoConstants.BLUE_3_STARTING_POSE + )) + // Score note + ); + break; + case RED_1: + complexPath = new SequentialCommandGroup( + // Shoot note + new FollowTrajectoryCommand(driveSubsystem, // Drive up to the amp and then grab the note closest to the wall near the amp. + AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 1)), + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp + AutoConstants.RED_1_STARTING_POSE)), // Drive to the original starting location + // Score note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp + AutoConstants.RED_1_COMPLEX_POSITIONS.get(2))), // Drive to the lower of the two targeted notes + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), // Drive back up near the amp + AutoConstants.RED_1_STARTING_POSE)) // Drive to the original starting location + // Score note + ); + case RED_2: + complexPath = new SequentialCommandGroup( + new FollowTrajectoryCommand(driveSubsystem, + List.of(AutoConstants.RED_1_COMPLEX_POSITIONS.get(0))), // Drive to top note + // Grab note + new FollowTrajectoryCommand(driveSubsystem, + List.of(FieldConstants.RED_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION)), // Return to the speaker + // Shoot note + new FollowTrajectoryCommand(driveSubsystem, + List.of(AutoConstants.RED_1_COMPLEX_POSITIONS.get(1))), // Drive to center note + // Grab note + new FollowTrajectoryCommand(driveSubsystem, + List.of(FieldConstants.RED_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION)), // Return to the speaker + // Shoot note + new FollowTrajectoryCommand(driveSubsystem, + List.of(AutoConstants.RED_1_COMPLEX_POSITIONS.get(2))), // Drive to bottom note + // Grab note + new FollowTrajectoryCommand(driveSubsystem, + List.of(FieldConstants.RED_ALLIANCE_SPEAKER_CENTER_SCORING_LOCATION)) // Return to the speaker + // Shoot note + ); + break; + case RED_3: + complexPath = new SequentialCommandGroup( + // Score note + new FollowTrajectoryCommand(driveSubsystem, + AutoConstants.RED_1_COMPLEX_POSITIONS.subList(0, 1)), + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), + AutoConstants.RED_1_STARTING_POSE + )), + // Score note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), + AutoConstants.RED_1_COMPLEX_POSITIONS.get(2) + )), + // Grab note + new FollowTrajectoryCommand(driveSubsystem, List.of( + AutoConstants.RED_1_COMPLEX_POSITIONS.get(0), + AutoConstants.RED_1_STARTING_POSE + )) + // Score note + ); + break; + default: + complexPath = new SequentialCommandGroup( + new RunCommand( + () -> auto(), + driveSubsystem) + ); + break; + } + + // Return the complex autonomous routine + return complexPath; + } + /** * Start making the robot follow the selected auto path. */ diff --git a/src/main/java/frc/robot/commands/DriveToPositionCommand.java b/src/main/java/frc/robot/commands/DriveToPositionCommand.java new file mode 100644 index 0000000..9cf9995 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToPositionCommand.java @@ -0,0 +1,36 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.DriveSubsystem; + +public class DriveToPositionCommand extends Command { + + private DriveSubsystem driveSubsystem; + private Pose2d targetPosition; + + public DriveToPositionCommand(DriveSubsystem driveSubsystem, Pose2d targetPosition) { + this.driveSubsystem = driveSubsystem; + this.targetPosition = targetPosition; + + addRequirements(driveSubsystem); + } + + public void execute() { + driveSubsystem.driveToPosition(targetPosition); + } + + + /** + * Stops all of the swerve modules located on the drivetrain. (All motor power is set to 0). + */ + @Override + public void end(boolean interrupted) { + driveSubsystem.setX(); + } + + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/FollowTrajectoryCommand.java b/src/main/java/frc/robot/commands/FollowTrajectoryCommand.java new file mode 100644 index 0000000..c67c3a7 --- /dev/null +++ b/src/main/java/frc/robot/commands/FollowTrajectoryCommand.java @@ -0,0 +1,74 @@ +package frc.robot.commands; + +import java.util.List; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.DriveSubsystem; + +/** + * This command tells the robot to drive to a list of positions on the field. + */ +public class FollowTrajectoryCommand extends Command { + + private DriveSubsystem driveSubsystem; + private List trajectory; + private boolean isFinished = false; + + /** + * Setup everything so that this command can access ad interact with the robot's drivetrain. + * + * @param driveSubsystem The robot's drivetrain. + * @param trajectory The path the robot will drive along. + */ + public FollowTrajectoryCommand(DriveSubsystem driveSubsystem, List trajectory) { + + // Gain access to the robot's drivetrain. + this.driveSubsystem = driveSubsystem; + + // Setup the path that the robot will follow. + this.trajectory = trajectory; + + // Make sure that 2 commands that involve driving don't happen at the same time. + addRequirements(driveSubsystem); + } + + /** + * Tell the robot what trajectory it will follow and then tell it to drive along that path. + */ + @Override + public void initialize() { + isFinished = false; + driveSubsystem.setTrajectory(this.trajectory); + } + + /** + * Follow the trajectory while the command is active. + */ + public void execute() { + driveSubsystem.followTrajectory(); + System.out.print("Working"); + if (driveSubsystem.isTrajectoryFinished()) { + isFinished = true; + } + } + + /** + * Stops all of the swerve modules located on the drivetrain. (All motor power is set to 0). + */ + @Override + public void end(boolean interrupted) { + driveSubsystem.setX(); + } + + /** + * Return whether or not the robot has finished. + */ + @Override + public boolean isFinished() { + if (isFinished) { + System.out.print("DOne"); + } + return isFinished; + } +} diff --git a/src/main/java/frc/robot/commands/vision/AlignToNoteCommand.java b/src/main/java/frc/robot/commands/vision/AlignToNoteCommand.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d50cf48..4f5b730 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -137,7 +137,7 @@ public DriveSubsystem(boolean resetOrientation, Supplier visionMeasureme frontRight.getPosition(), backLeft.getPosition(), backRight.getPosition()}, - new Pose2d(new Translation2d(8.945, 7.815), new Rotation2d(0, 0))); // Note: See how accurate this turns out to be. Change if need be. + new Pose2d(new Translation2d(0,0), new Rotation2d(0, 0))); // Note: See how accurate this turns out to be. Change if need be. // Configure alternative drive mode PID controllers. thetaController.enableContinuousInput(0, Math.PI * 2); diff --git a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java index 93c79d2..d80038a 100644 --- a/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java +++ b/src/main/java/frc/robot/subsystems/cameras/PhotonVisionCamera.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.VisionConstants; +import frc.utils.vision.DetectedObject; /** * A camera attached to a co-processor. @@ -77,6 +78,8 @@ public void periodic() { if (tagCorners.isEmpty()) { return; } + + // Get raw data SmartDashboard.putNumber("Bottom Left Corner X: ", tagCorners.get(0).x); SmartDashboard.putNumber("Bottom Left Corner Y: ", tagCorners.get(0).y); SmartDashboard.putNumber("Bottom Right Corner X: ", tagCorners.get(1).x); @@ -86,10 +89,14 @@ public void periodic() { SmartDashboard.putNumber("Top Left Corner X: ", tagCorners.get(3).x); SmartDashboard.putNumber("Top Left Corner Y: ", tagCorners.get(3).y); - SmartDashboard.putNumber("Object Y: ",(tagCorners.get(3).y + tagCorners.get(1).y) / 2); + // Calculate object's position on the screen relative to the top left corner. SmartDashboard.putNumber("Object X: ",(tagCorners.get(3).x + tagCorners.get(1).x) / 2); + SmartDashboard.putNumber("Object Y: ",(tagCorners.get(3).y + tagCorners.get(1).y) / 2); - + // Convert the detected object to a DetectedObject and output the calculated position values. + DetectedObject detectedObject = new DetectedObject(target); + SmartDashboard.putNumber("Center Origin Object X: ", detectedObject.getX()); + SmartDashboard.putNumber("Center Origin Object Y: ", detectedObject.getY()); } } @@ -121,6 +128,13 @@ public Optional estimateRobotPose() { return photonPoseEstimator.update(); } + public List gDetectedObjects() { + + // TODO: Add ability to get detected object's data. + + return null; + } + /** * This method sets the pipeline that this camera's stream will be processed * using. diff --git a/src/main/java/frc/utils/vision/DetectedObject.java b/src/main/java/frc/utils/vision/DetectedObject.java new file mode 100644 index 0000000..f3493f6 --- /dev/null +++ b/src/main/java/frc/utils/vision/DetectedObject.java @@ -0,0 +1,119 @@ +package frc.utils.vision; + +import java.util.List; + +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +public class DetectedObject { + + private double x, y; + private double area, percentOfScreen; + private double skew; + + /** + * Create a {@code DetectedObject} using values from a {@code PhotonTrackedTarget}. + * + * @param detectedObject The {@code PhotonTrackedTarget} who's values will be used to create + * this {@code DetectedObject} + */ + public DetectedObject(PhotonTrackedTarget detectedObject) { + + // Get the detected object's area and position on the screen. + double[] screenPosition = calculateScreenPosition(detectedObject); + this.x = screenPosition[0]; + this.y = screenPosition[1]; + this.area = screenPosition[2]; + + // Get the object's skew + this.skew = detectedObject.getSkew(); + this.percentOfScreen = detectedObject.getArea(); + } + + /** + * Calculates the object's area and position on the screen. + * + * @param detectedObject The PhotonTrackedTarget who's values will be used to preform the position + * calculations. + * @return An array of doubles containing the object's position on the screen and area it take sup, in pixels. + * {x, y, area} + */ + private double[] calculateScreenPosition(PhotonTrackedTarget detectedObject) { + + // Get all of the detected object's corners. This allows us to use the corners' position to + // figure out where the object is on the screen. + List targetCorners = detectedObject.getMinAreaRectCorners(); + TargetCorner bottomLeftCorner = targetCorners.get(0); + TargetCorner topRightCorner = targetCorners.get(2); + + /* + * Add the x and y coordinates of both corners together and divide the result by two. Doing this + * results in the output being exactly halfway between the two corners, which jut so happens to be + * the center of the object. + */ + double x = (bottomLeftCorner.x + topRightCorner.x) / 2; + double y = (bottomLeftCorner.y + topRightCorner.y) / 2; + + /* + * Then, since our captured image is (1280 x 720p) and the image's origin is in the top left corner, we + * subtract half of this from our calculated coordinates, as doing so sets our origin as the center which + * makes everything easier to work with. + */ + x -= 640; + y -= 360; + y *= -1; + + // Calculate the length and width of the object's bounding box and multiply them together to find the area. + double area = Math.abs(topRightCorner.x - bottomLeftCorner.x) * + Math.abs(topRightCorner.y - bottomLeftCorner.y); + + // Return an array containing the above calculated x and y coordinates and area. + double[] fieldPosition = {x, y, area}; + return fieldPosition; + } + + /** + * Returns the X coordinate of this detected object (Screen position, in pixels). + * + * @return The X coordinate of this detected object (Screen position, in pixels). + */ + public double getX() { + return this.x; + } + + /** + * Returns the Y coordinate of this detected object (Screen position, in pixels). + * + * @return The Y coordinate of this detected object (Screen position, in pixels). + */ + public double getY() { + return this.y; + } + + /** + * Returns the s% of the screen that this object takes up. + * + * @return The space on the screen that this object's bounding box takes up, in pixels. + */ + public double getArea() { + return this.area; + } + + /** + * Returns the percent of the screen taken up by this object (0.0 -> 1.0). + * + * @return The percent of the screen taken up by this object (0.0 -> 1.0). + */ + public double getPercentOfScreen() { + return this.percentOfScreen; + } + + /** + * Returns this object's skew. + * + * @return This object's skew. + */ + public double getSkew() { + return this.skew; + } +} From 918848a9f2a55fcf8fea69668670386f4e90b110 Mon Sep 17 00:00:00 2001 From: NotAppIicable Date: Tue, 26 Mar 2024 22:36:12 -0500 Subject: [PATCH 7/8] fix: Adjust red alliance path rotations. Now the red alliance's paths shouldn't cause the robot to rotate. --- src/main/java/frc/robot/Constants.java | 30 +++++++++++++------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 12bcc3c..0968079 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -212,12 +212,12 @@ public static final class AutoConstants { public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED); - public static final Pose2d BLUE_1_STARTING_POSE = new Pose2d(1.629, 6.457, Rotation2d.fromRadians(0)); - public static final Pose2d BLUE_2_STARTING_POSE = new Pose2d(1.278, 5.603, Rotation2d.fromRadians(0)); - public static final Pose2d BLUE_3_STARTING_POSE = new Pose2d(1.629, 4.749, Rotation2d.fromRadians(0)); - public static final Pose2d RED_1_STARTING_POSE = new Pose2d(14.884, 6.457, Rotation2d.fromRadians(Math.PI)); - public static final Pose2d RED_2_STARTING_POSE = new Pose2d(15.235, 5.603, Rotation2d.fromRadians(Math.PI)); - public static final Pose2d RED_3_STARTING_POSE = new Pose2d(14.884, 4.749, Rotation2d.fromRadians(Math.PI)); + public static final Pose2d BLUE_1_STARTING_POSE = new Pose2d(1.629, 6.457, Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_2_STARTING_POSE = new Pose2d(1.278, 5.603, Rotation2d.fromDegrees(0)); + public static final Pose2d BLUE_3_STARTING_POSE = new Pose2d(1.629, 4.749, Rotation2d.fromDegrees(0)); + public static final Pose2d RED_1_STARTING_POSE = new Pose2d(14.884, 6.457, Rotation2d.fromDegrees(180)); + public static final Pose2d RED_2_STARTING_POSE = new Pose2d(15.235, 5.603, Rotation2d.fromDegrees(180)); + public static final Pose2d RED_3_STARTING_POSE = new Pose2d(14.884, 4.749, Rotation2d.fromDegrees(180)); // Old paths public static final List RED_1_TRAJECTORY = List.of( @@ -261,19 +261,19 @@ public static final class AutoConstants { ); public static final List RED_1_COMPLEX_POSITIONS = List.of( - new Pose2d(14.884, 7.807, Rotation2d.fromDegrees(0)), // Close to amp position - new Pose2d(8.796, 7.436, Rotation2d.fromDegrees(0)), // Topmost note - new Pose2d(8.796, 5.785, Rotation2d.fromDegrees(0)) // Second top note + new Pose2d(14.884, 7.807, Rotation2d.fromDegrees(180)), // Close to amp position + new Pose2d(8.796, 7.436, Rotation2d.fromDegrees(180)), // Topmost note + new Pose2d(8.796, 5.785, Rotation2d.fromDegrees(180)) // Second top note ); public static final List RED_2_COMPLEX_POSITIONS = List.of( - new Pose2d(14.24, 4.088, Rotation2d.fromDegrees(0)), // Bottom Note - new Pose2d(14.24, 5.569, Rotation2d.fromDegrees(0)), // Middle Note - new Pose2d(14.24, 7.004, Rotation2d.fromDegrees(0)) // Top Note + new Pose2d(14.24, 4.088, Rotation2d.fromDegrees(180)), // Bottom Note + new Pose2d(14.24, 5.569, Rotation2d.fromDegrees(180)), // Middle Note + new Pose2d(14.24, 7.004, Rotation2d.fromDegrees(180)) // Top Note ); public static final List RED_3_COMPLEX_POSITIONS = List.of( - new Pose2d(11.787, 4.840, Rotation2d.fromDegrees(0)), // Position under triangle thingy - new Pose2d(8.796, 4.099, Rotation2d.fromDegrees(0)), // Center note in center of field. - new Pose2d(8.796, 2.425, Rotation2d.fromDegrees(0)) // Second to last note in center of field. + new Pose2d(11.787, 4.840, Rotation2d.fromDegrees(180)), // Position under triangle thingy + new Pose2d(8.796, 4.099, Rotation2d.fromDegrees(180)), // Center note in center of field. + new Pose2d(8.796, 2.425, Rotation2d.fromDegrees(180)) // Second to last note in center of field. ); } From 74e0aa1a9084fcb35e2573b990bb23e36a3b7fe1 Mon Sep 17 00:00:00 2001 From: NotAppIicable Date: Tue, 2 Apr 2024 18:37:50 -0500 Subject: [PATCH 8/8] fix: Remove prints so that you can read the output --- src/main/java/frc/robot/commands/FollowTrajectoryCommand.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/FollowTrajectoryCommand.java b/src/main/java/frc/robot/commands/FollowTrajectoryCommand.java index c67c3a7..837975f 100644 --- a/src/main/java/frc/robot/commands/FollowTrajectoryCommand.java +++ b/src/main/java/frc/robot/commands/FollowTrajectoryCommand.java @@ -47,7 +47,6 @@ public void initialize() { */ public void execute() { driveSubsystem.followTrajectory(); - System.out.print("Working"); if (driveSubsystem.isTrajectoryFinished()) { isFinished = true; }