From d046aceed70f525d2a0d6dfbd926836e4716feff Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sat, 8 Feb 2025 14:14:59 -0500 Subject: [PATCH 01/28] Vision ready to test? (Added files + dependencies) --- simgui-ds.json | 92 +++++++++ .../frc/robot/subsystems/vision/Vision.java | 189 ++++++++++++++++++ .../subsystems/vision/VisionConstants.java | 60 ++++++ .../frc/robot/subsystems/vision/VisionIO.java | 49 +++++ .../subsystems/vision/VisionIOLimelight.java | 156 +++++++++++++++ .../vision/VisionIOPhotonVision.java | 146 ++++++++++++++ .../vision/VisionIOPhotonVisionSim.java | 60 ++++++ vendordeps/photonlib.json | 71 +++++++ 8 files changed, 823 insertions(+) create mode 100644 simgui-ds.json create mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionConstants.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java create mode 100644 vendordeps/photonlib.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..7da935b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,189 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionIO.PoseObservationType; +import java.util.LinkedList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + + +public class Vision extends SubsystemBase { + private final VisionConsumer consumer; + private final VisionIO[] io; + private final VisionIOInputsAutoLogged[] inputs; + private final Alert[] disconnectedAlerts; + + public Vision(VisionConsumer consumer, VisionIO... io) { + this.consumer = consumer; + this.io = io; + + // Initialize inputs + this.inputs = new VisionIOInputsAutoLogged[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new VisionIOInputsAutoLogged(); + } + + // Initialize disconnected alerts + this.disconnectedAlerts = new Alert[io.length]; + for (int i = 0; i < inputs.length; i++) { + disconnectedAlerts[i] = + new Alert( + "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + } + } + + /** + * Returns the X 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(); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + } + + // Initialize logging values + List allTagPoses = new LinkedList<>(); + List allRobotPoses = new LinkedList<>(); + List allRobotPosesAccepted = new LinkedList<>(); + List allRobotPosesRejected = new LinkedList<>(); + + // Loop over cameras + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { + // Update disconnected alert + disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + + // Initialize logging values + List tagPoses = new LinkedList<>(); + List robotPoses = new LinkedList<>(); + List robotPosesAccepted = new LinkedList<>(); + List robotPosesRejected = new LinkedList<>(); + + // Add tag poses + for (int tagId : inputs[cameraIndex].tagIds) { + var tagPose = aprilTagLayout.getTagPose(tagId); + if (tagPose.isPresent()) { + tagPoses.add(tagPose.get()); + } + } + + // Loop over pose observations + for (var observation : inputs[cameraIndex].poseObservations) { + // Check whether to reject pose + boolean rejectPose = + observation.tagCount() == 0 // Must have at least one tag + || (observation.tagCount() == 1 + && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity + || Math.abs(observation.pose().getZ()) + > maxZError // Must have realistic Z coordinate + + // Must be within the field boundaries + || observation.pose().getX() < 0.0 + || observation.pose().getX() > aprilTagLayout.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + + // Add pose to log + robotPoses.add(observation.pose()); + if (rejectPose) { + robotPosesRejected.add(observation.pose()); + } else { + robotPosesAccepted.add(observation.pose()); + } + + // Skip if rejected + if (rejectPose) { + continue; + } + + // Calculate standard deviations + double stdDevFactor = + Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double angularStdDev = angularStdDevBaseline * stdDevFactor; + if (observation.type() == PoseObservationType.MEGATAG_2) { + linearStdDev *= linearStdDevMegatag2Factor; + angularStdDev *= angularStdDevMegatag2Factor; + } + if (cameraIndex < cameraStdDevFactors.length) { + linearStdDev *= cameraStdDevFactors[cameraIndex]; + angularStdDev *= cameraStdDevFactors[cameraIndex]; + } + + // Send vision observation + consumer.accept( + observation.pose().toPose2d(), + observation.timestamp(), + VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); + } + + // Log camera datadata + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", + tagPoses.toArray(new Pose3d[tagPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", + robotPoses.toArray(new Pose3d[robotPoses.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()])); + allTagPoses.addAll(tagPoses); + allRobotPoses.addAll(robotPoses); + allRobotPosesAccepted.addAll(robotPosesAccepted); + allRobotPosesRejected.addAll(robotPosesRejected); + } + + // Log summary data + Logger.recordOutput( + "Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[allRobotPoses.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesAccepted", + allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()])); + Logger.recordOutput( + "Vision/Summary/RobotPosesRejected", + allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()])); + } + + @FunctionalInterface + public static interface VisionConsumer { + public void accept( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java new file mode 100644 index 0000000..a82e3d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -0,0 +1,60 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; + +public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "photoncamera_5"; + public static String camera1Name = "photoncamera_6"; + //public static String camera2Name = "photonvision_3"; + //public static String camera3Name = "photonvision_4"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.0, -3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + public static Transform3d robotToCamera1 = + new Transform3d(0.0, 3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + // Multipliers to apply for MegaTag 2 observations + public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve + public static double angularStdDevMegatag2Factor = + Double.POSITIVE_INFINITY; // No rotation data available +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..3183358 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,49 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface VisionIO { + @AutoLog + public static class VisionIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = + new TargetObservation(new Rotation2d(), new Rotation2d()); + 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) {} + + /** Represents a robot pose sample used for pose estimation. */ + public static record PoseObservation( + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + PoseObservationType type) {} + + public static enum PoseObservationType { + MEGATAG_1, + MEGATAG_2, + PHOTONVISION + } + + public default void updateInputs(VisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java new file mode 100644 index 0000000..d6e2a77 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -0,0 +1,156 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.RobotController; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import java.util.function.Supplier; + +/** IO implementation for real Limelight hardware. */ +public class VisionIOLimelight implements VisionIO { + private final Supplier rotationSupplier; + private final DoubleArrayPublisher orientationPublisher; + + private final DoubleSubscriber latencySubscriber; + private final DoubleSubscriber txSubscriber; + private final DoubleSubscriber tySubscriber; + private final DoubleArraySubscriber megatag1Subscriber; + private final DoubleArraySubscriber megatag2Subscriber; + + /** + * Creates a new VisionIOLimelight. + * + * @param name The configured name of the Limelight. + * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. + */ + public VisionIOLimelight(String name, Supplier rotationSupplier) { + var table = NetworkTableInstance.getDefault().getTable(name); + this.rotationSupplier = rotationSupplier; + orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); + latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); + txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); + tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); + megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + megatag2Subscriber = + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + // Update connection status based on whether an update has been seen in the last 250ms + inputs.connected = + ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; + + // Update target observation + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); + + // Update orientation for MegaTag 2 + orientationPublisher.accept( + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + NetworkTableInstance.getDefault() + .flush(); // Increases network traffic but recommended by Limelight + + // Read new pose observations from NetworkTables + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag + rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_1)); + } + for (var rawSample : megatag2Subscriber.readQueue()) { + if (rawSample.value.length == 0) continue; + for (int i = 11; i < rawSample.value.length; i += 7) { + tagIds.add((int) rawSample.value[i]); + } + poseObservations.add( + new PoseObservation( + // Timestamp, based on server timestamp of publish and latency + rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, + + // 3D pose estimate + parsePose(rawSample.value), + + // Ambiguity, zeroed because the pose is already disambiguated + 0.0, + + // Tag count + (int) rawSample.value[7], + + // Average tag distance + rawSample.value[9], + + // Observation type + PoseObservationType.MEGATAG_2)); + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } + + /** Parses the 3D pose from a Limelight botpose array. */ + private static Pose3d parsePose(double[] rawLLArray) { + return new Pose3d( + rawLLArray[0], + rawLLArray[1], + rawLLArray[2], + new Rotation3d( + Units.degreesToRadians(rawLLArray[3]), + Units.degreesToRadians(rawLLArray[4]), + Units.degreesToRadians(rawLLArray[5]))); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java new file mode 100644 index 0000000..8fd7a2c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -0,0 +1,146 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.HashSet; +import java.util.LinkedList; +import java.util.List; +import java.util.Set; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +/** IO implementation for real PhotonVision hardware. */ +public class VisionIOPhotonVision implements VisionIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + + /** + * Creates a new VisionIOPhotonVision. + * + * @param name The configured name of the camera. + * @param rotationSupplier The 3D position of the camera relative to the robot. + */ + public VisionIOPhotonVision(String name, Transform3d robotToCamera) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + } + + // NEW COMMENT + // SECTION 2 + @Override + public void updateInputs(VisionIOInputs inputs) { + + inputs.connected = camera.isConnected(); + + // Read new camera observations + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + + for (var result : camera.getAllUnreadResults()) { + // Update latest target observation + if (result.hasTargets()) { + + inputs.latestTargetObservation = + new TargetObservation( + Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + } else { + inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d()); + } + + // Add pose observation + if (result.multitagResult.isPresent()) { // Multitag result + var multitagResult = result.multitagResult.get(); + + // Calculate robot pose + Transform3d fieldToCamera = multitagResult.estimatedPose.best; + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + + // Calculate average tag distance + double totalTagDistance = 0.0; + for (var target : result.targets) { + totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + } + + // Add tag IDs + tagIds.addAll(multitagResult.fiducialIDsUsed); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + multitagResult.estimatedPose.ambiguity, // Ambiguity + multitagResult.fiducialIDsUsed.size(), // Tag count + totalTagDistance / result.targets.size(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + + // Calculate robot pose + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + //System.out.println("****** " + camera.getName() + " *********************************"); + //System.out.println("tagPose:" + tagPose); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + //System.out.println("fieldToTarget:" + fieldToTarget); + Transform3d cameraToTarget = target.bestCameraToTarget; + //System.out.println("cameraToTarget:" + cameraToTarget); + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + //System.out.println("fieldToCamera:" + fieldToCamera); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + //System.out.println("fieldToRobot:" + fieldToRobot); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + //System.out.println("robotPose:" + robotPose); + + // Add tag ID + tagIds.add((short) target.fiducialId); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.PHOTONVISION)); // Observation type + } + } + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java new file mode 100644 index 0000000..eab4d7f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -0,0 +1,60 @@ +// Copyright 2021-2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** IO implementation for physics sim using PhotonVision simulator. */ +public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { + private static VisionSystemSim visionSim; + + private final Supplier poseSupplier; + private final PhotonCameraSim cameraSim; + + /** + * Creates a new VisionIOPhotonVisionSim. + * + * @param name The name of the camera. + * @param poseSupplier Supplier for the robot pose to use in simulation. + */ + public VisionIOPhotonVisionSim( + String name, Transform3d robotToCamera, Supplier poseSupplier) { + super(name, robotToCamera); + this.poseSupplier = poseSupplier; + + // Initialize vision sim + if (visionSim == null) { + visionSim = new VisionSystemSim("main"); + visionSim.addAprilTags(aprilTagLayout); + } + + // Add sim camera + var cameraProperties = new SimCameraProperties(); + cameraSim = new PhotonCameraSim(camera, cameraProperties); + visionSim.addCamera(cameraSim, robotToCamera); + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + visionSim.update(poseSupplier.get()); + super.updateInputs(inputs); + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6af3d3e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } + ] +} \ No newline at end of file From e017e4ad1d96900c989d6ff16c599c49124e2d49 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sat, 8 Feb 2025 14:27:36 -0500 Subject: [PATCH 02/28] Added credits to AdvantageKit for Vision --- src/main/java/frc/robot/subsystems/vision/Vision.java | 3 +++ src/main/java/frc/robot/subsystems/vision/VisionConstants.java | 3 +++ src/main/java/frc/robot/subsystems/vision/VisionIO.java | 3 +++ .../java/frc/robot/subsystems/vision/VisionIOLimelight.java | 3 +++ .../java/frc/robot/subsystems/vision/VisionIOPhotonVision.java | 3 +++ .../frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java | 3 +++ 6 files changed, 18 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 7da935b..07e3d99 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -1,3 +1,6 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// // Copyright 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index a82e3d8..d2c0d59 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,3 +1,6 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// // Copyright 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 3183358..b1e70f6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,3 +1,6 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// // Copyright 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index d6e2a77..a09d102 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -1,3 +1,6 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// // Copyright 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 8fd7a2c..94a8d53 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -1,3 +1,6 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// // Copyright 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index eab4d7f..a0fd43c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -1,3 +1,6 @@ +// Code modified from AdvantageKit Vision Template (v4.0.1): +// https://github.com/Mechanical-Advantage/AdvantageKit/releases +// // Copyright 2021-2025 FRC 6328 // http://github.com/Mechanical-Advantage // From d7ef141712f3897af5cca4531a4d22a86b65a465 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sun, 9 Feb 2025 18:16:59 -0500 Subject: [PATCH 03/28] Vision is now instantiated (oops) --- .../java/frc/robot/HardwareConstants.java | 17 ++++++++++ src/main/java/frc/robot/Robot.java | 32 ++++++++++++------- src/main/java/frc/robot/RobotContainer.java | 10 ++++++ 3 files changed, 48 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 8c87b18..fed50ed 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -1,6 +1,23 @@ package frc.robot; +import edu.wpi.first.wpilibj.RobotBase; + public class HardwareConstants { + + public static final Mode simMode = Mode.SIM; + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + public class CAN { // Add CAN IDs here public static final int PDH_ID = 1; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1f77ae4..7224c30 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,22 +23,32 @@ public class Robot extends LoggedRobot { private final RobotContainer m_robotContainer; public Robot() { - Logger.recordMetadata("ProjectName", "ReefCode"); // Set a metadata value - if (isReal()) { - Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - new PowerDistribution(CAN.PDH_ID, ModuleType.kRev); // Enables power distribution logging - } else { - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) - Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + switch (HardwareConstants.currentMode) { + case REAL: + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + new PowerDistribution(CAN.PDH_ID, ModuleType.kRev); + break; + + case SIM: + System.out.println("SIM!!!"); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + break; } + Logger.recordMetadata("ProjectName", "ReefCode"); // Set a metadata value + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may // be added. - + m_robotContainer = new RobotContainer(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a34ff4b..98c1261 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.vision.VisionConstants.robotToCamera0; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.pathplanner.lib.auto.NamedCommands; @@ -29,12 +30,16 @@ import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; import frc.robot.subsystems.presets.Preset; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIOPhotonVision; +import static frc.robot.subsystems.vision.VisionConstants.*; public class RobotContainer { private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); + private final Vision vision; private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); @@ -75,6 +80,11 @@ public class RobotContainer { public RobotContainer() { + vision = new Vision( + drivetrain::addVisionMeasurement, + new VisionIOPhotonVision(camera0Name, robotToCamera0), + new VisionIOPhotonVision(camera1Name, robotToCamera1)); + // All AutoAligns for reef will align to Left position // NamedCommands.getCommand("L1"); // AutoAlignToReef + ScoreL1 From f8bc90f6aad93fe20c915a023a6ece5f16334b8e Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sun, 9 Feb 2025 21:08:46 -0500 Subject: [PATCH 04/28] Vision - renamed cameras, updated vendordeps --- build.gradle | 2 +- .../frc/robot/subsystems/arm/ArmCommands.java | 5 +- .../MultiSubsystemCommands.java | 7 ++- .../subsystems/vision/VisionConstants.java | 11 +++- ...rLib-2025.2.1.json => PathplannerLib.json} | 8 +-- ....2.1.json => Phoenix6-frc2025-latest.json} | 58 +++++++++---------- .../{REVLib-2025.0.1.json => REVLib.json} | 12 ++-- 7 files changed, 58 insertions(+), 45 deletions(-) rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib.json} (87%) rename vendordeps/{Phoenix6-25.2.1.json => Phoenix6-frc2025-latest.json} (92%) rename vendordeps/{REVLib-2025.0.1.json => REVLib.json} (90%) diff --git a/build.gradle b/build.gradle index 0ab3b81..80a1ec6 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index ec1668d..a8242b1 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -26,6 +26,9 @@ public Command spit() { } public Command setArmPosition(ArmPosition setpoint) { + + return new InstantCommand(); + /* if (setpoint == ArmPosition.L4_Score) { return new InstantCommand( () -> { @@ -37,7 +40,7 @@ public Command setArmPosition(ArmPosition setpoint) { () -> { _arm.setArmSetpoint(setpoint); }, - _arm); + _arm); */ } public Command intake() { diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index bae1e00..b3ed436 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -73,6 +73,10 @@ private Command score(OverallPosition setpoint) { } public Command scoreGamepieceAtPosition(OverallPosition setpoint) { + + return new InstantCommand(); + + /* if (setpoint == OverallPosition.Stow || setpoint == OverallPosition.Loading || setpoint == OverallPosition.L4_Score) { throw new RuntimeException("scoreGamepieceAtPosition cannot run to stow,loading,or L4_score"); @@ -81,5 +85,6 @@ public Command scoreGamepieceAtPosition(OverallPosition setpoint) { .andThen(waitForOverallMechanism()) .andThen(score(setpoint)) .andThen(setOverallSetpoint(OverallPosition.Stow)); - } + */ + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index d2c0d59..0552fb0 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,11 +24,16 @@ public class VisionConstants { // AprilTag layout public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); // Camera names, must match names configured on coprocessor - public static String camera0Name = "photoncamera_5"; - public static String camera1Name = "photoncamera_6"; + + // New cameras for 2025 season are 5,6,7,8 + + //public static String camera0Name = "photoncamera_5"; + //public static String camera1Name = "photoncamera_6"; + public static String camera0Name = "photoncamera_7"; + public static String camera1Name = "photoncamera_8"; //public static String camera2Name = "photonvision_3"; //public static String camera3Name = "photonvision_4"; diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib.json index 71e25f3..2021898 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 92% rename from vendordeps/Phoenix6-25.2.1.json rename to vendordeps/Phoenix6-frc2025-latest.json index 1397da1..acc78db 100644 --- a/vendordeps/Phoenix6-25.2.1.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.1.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib.json similarity index 90% rename from vendordeps/REVLib-2025.0.1.json rename to vendordeps/REVLib.json index c998054..7194603 100644 --- a/vendordeps/REVLib-2025.0.1.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.1.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 00f8e4b78d9901da6c5fa008afeb47eed3470dbf Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Wed, 12 Feb 2025 17:46:43 -0500 Subject: [PATCH 05/28] Add cameras 6 and 7 --- src/main/java/frc/robot/RobotContainer.java | 5 +++- .../subsystems/vision/VisionConstants.java | 26 ++++++++++++------- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 98c1261..f3eebf3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,7 +83,10 @@ public RobotContainer() { vision = new Vision( drivetrain::addVisionMeasurement, new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1)); + new VisionIOPhotonVision(camera1Name, robotToCamera1), + new VisionIOPhotonVision(camera2Name, robotToCamera2), + new VisionIOPhotonVision(camera3Name, robotToCamera3) + ); // All AutoAligns for reef will align to Left position diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 0552fb0..54a1df5 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -30,18 +30,24 @@ public class VisionConstants { // New cameras for 2025 season are 5,6,7,8 - //public static String camera0Name = "photoncamera_5"; - //public static String camera1Name = "photoncamera_6"; - public static String camera0Name = "photoncamera_7"; - public static String camera1Name = "photoncamera_8"; - //public static String camera2Name = "photonvision_3"; - //public static String camera3Name = "photonvision_4"; + public static String camera0Name = "photoncamera_5"; + public static String camera1Name = "photoncamera_6"; + public static String camera2Name = "photoncamera_7"; + public static String camera3Name = "photoncamera_8"; // Robot to camera transforms // (Not used by Limelight, configure in web UI instead) + + // [TODO: SET VALUES] Should be on back of robot public static Transform3d robotToCamera0 = - new Transform3d(0.0, -3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + new Transform3d(0.0, -3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, Math.PI)); public static Transform3d robotToCamera1 = + new Transform3d(0.0, 3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, Math.PI)); + + // On front of robot, centered + public static Transform3d robotToCamera2 = + new Transform3d(0.0, -3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + public static Transform3d robotToCamera3 = new Transform3d(0.0, 3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); // Basic filtering thresholds @@ -58,11 +64,13 @@ public class VisionConstants { public static double[] cameraStdDevFactors = new double[] { 1.0, // Camera 0 - 1.0 // Camera 1 + 1.0, // Camera 1 + 1.0, // Camera 2 + 1.0 // Camera 3 }; // Multipliers to apply for MegaTag 2 observations public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available + Double.POSITIVE_INFINITY; // Ignore rotation data } From 92231420feb468297bf512dc417c1cc1af6eed95 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Thu, 13 Feb 2025 18:06:55 -0500 Subject: [PATCH 06/28] Remove duplicate multisubsystem intake command --- .../multisubsystemcommands/MultiSubsystemCommands.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 753329d..4fcb7fd 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -93,12 +93,5 @@ public Command intake() { .andThen(_armCommands.intake()) .andThen(setOverallSetpoint(OverallPosition.Stow)); } - - public Command intake() { - return setOverallSetpoint(OverallPosition.Loading) - .andThen(waitForOverallMechanism()) - .andThen(_armCommands.intake()) - .andThen(setOverallSetpoint(OverallPosition.Stow)); - } } \ No newline at end of file From 85fe423238ea8168c668e00971c4b5c2515fd05a Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sat, 15 Feb 2025 19:02:14 -0500 Subject: [PATCH 07/28] Update field type to Andymark, changed camera constants --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 53 +++++++++++++------ .../subsystems/vision/VisionConstants.java | 18 ++++--- 3 files changed, 50 insertions(+), 23 deletions(-) diff --git a/build.gradle b/build.gradle index 80a1ec6..369011a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" } java { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce09dbe..ba34cbe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; @@ -49,12 +50,12 @@ public class RobotContainer { private final Drive drive; - private final CommandXboxController controller = new CommandXboxController(0); private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); private final Vision vision; + private final CommandXboxController joystick = new CommandXboxController(0); private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); @@ -75,8 +76,6 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final CommandXboxController joystick = new CommandXboxController(0); - private final GenericHID buttonbox1 = new GenericHID(1); private final JoystickButton L1Button = new JoystickButton(buttonbox1, 1); private final JoystickButton L2Button = new JoystickButton(buttonbox1, 2); @@ -109,11 +108,13 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.BackRight)); vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1)); - + new Vision( + drive::addVisionMeasurement, + new VisionIOPhotonVision(camera0Name, robotToCamera0), + new VisionIOPhotonVision(camera1Name, robotToCamera1), + new VisionIOPhotonVision(camera2Name, robotToCamera2), + new VisionIOPhotonVision(camera3Name, robotToCamera3) + ); break; case SIM: @@ -129,8 +130,11 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), - new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); + new VisionIOPhotonVision(camera0Name, robotToCamera0), + new VisionIOPhotonVision(camera1Name, robotToCamera1), + new VisionIOPhotonVision(camera2Name, robotToCamera2), + new VisionIOPhotonVision(camera3Name, robotToCamera3) + ); break; default: @@ -143,7 +147,13 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}); break; } @@ -180,11 +190,10 @@ private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); + () -> -joystick.getLeftY(), + () -> -joystick.getLeftX(), + () -> -joystick.getRightX())); - // drivetrain.setDefaultCommand( // // Drivetrain will execute this command periodically // drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with @@ -204,7 +213,19 @@ private void configureBindings() { joystick.start().and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); joystick.start().and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); - // reset the field-centric heading on left bumper press + + // Reset gyro to 0° when left bumper button is pressed + joystick + .leftBumper() + .onTrue( + Commands.runOnce( + () -> + drive.setPose( + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + drive) + .ignoringDisable(true)); + + // // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); // drive.registerTelemetry(logger::telemeterize); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 54a1df5..ee52cef 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,7 +24,7 @@ public class VisionConstants { // AprilTag layout public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); // Camera names, must match names configured on coprocessor @@ -39,16 +39,22 @@ public class VisionConstants { // (Not used by Limelight, configure in web UI instead) // [TODO: SET VALUES] Should be on back of robot + // Note: 0.0254 multiplier converts inches to meters + + // Camera 5 public static Transform3d robotToCamera0 = - new Transform3d(0.0, -3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, Math.PI)); + new Transform3d(-14.0*0.0254, -11.25*0.0254, 10.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + // Camera 6 public static Transform3d robotToCamera1 = - new Transform3d(0.0, 3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, Math.PI)); + new Transform3d(-14.0*0.0254, 11.25*0.0254, 10.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + - // On front of robot, centered + // Camera 7 public static Transform3d robotToCamera2 = - new Transform3d(0.0, -3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + new Transform3d(0.5*0.0254, 4*0.0254, 9*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + // Camera 8 public static Transform3d robotToCamera3 = - new Transform3d(0.0, 3.25*0.0254, 9.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + new Transform3d(0.5*0.0254, 4*0.0254, 9*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); // Basic filtering thresholds public static double maxAmbiguity = 0.3; From a617f6b3dfb160f34a1e71d399161a5e65f0dcef Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sat, 15 Feb 2025 19:25:55 -0500 Subject: [PATCH 08/28] Refined camera constants --- src/main/java/frc/robot/RobotContainer.java | 16 ++++----- .../subsystems/vision/VisionConstants.java | 33 +++++++++---------- 2 files changed, 24 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ba34cbe..855459d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -110,10 +110,10 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1), - new VisionIOPhotonVision(camera2Name, robotToCamera2), - new VisionIOPhotonVision(camera3Name, robotToCamera3) + new VisionIOPhotonVision(camera5Name, robotToCamera5), + new VisionIOPhotonVision(camera6Name, robotToCamera6), + new VisionIOPhotonVision(camera7Name, robotToCamera7), + new VisionIOPhotonVision(camera8Name, robotToCamera8) ); break; @@ -130,10 +130,10 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera0Name, robotToCamera0), - new VisionIOPhotonVision(camera1Name, robotToCamera1), - new VisionIOPhotonVision(camera2Name, robotToCamera2), - new VisionIOPhotonVision(camera3Name, robotToCamera3) + new VisionIOPhotonVision(camera5Name, robotToCamera5), + new VisionIOPhotonVision(camera6Name, robotToCamera6), + new VisionIOPhotonVision(camera7Name, robotToCamera7), + new VisionIOPhotonVision(camera8Name, robotToCamera8) ); break; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index ee52cef..86a0036 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -30,31 +30,30 @@ public class VisionConstants { // New cameras for 2025 season are 5,6,7,8 - public static String camera0Name = "photoncamera_5"; - public static String camera1Name = "photoncamera_6"; - public static String camera2Name = "photoncamera_7"; - public static String camera3Name = "photoncamera_8"; + public static String camera5Name = "photoncamera_5"; + public static String camera6Name = "photoncamera_6"; + public static String camera7Name = "photoncamera_7"; + public static String camera8Name = "photoncamera_8"; // Robot to camera transforms // (Not used by Limelight, configure in web UI instead) - // [TODO: SET VALUES] Should be on back of robot // Note: 0.0254 multiplier converts inches to meters // Camera 5 - public static Transform3d robotToCamera0 = - new Transform3d(-14.0*0.0254, -11.25*0.0254, 10.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + public static Transform3d robotToCamera5 = + new Transform3d(-13.75*0.0254, 11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); // Camera 6 - public static Transform3d robotToCamera1 = - new Transform3d(-14.0*0.0254, 11.25*0.0254, 10.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + public static Transform3d robotToCamera6 = + new Transform3d(-13.75*0.0254, -11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); // Camera 7 - public static Transform3d robotToCamera2 = - new Transform3d(0.5*0.0254, 4*0.0254, 9*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + public static Transform3d robotToCamera7 = + new Transform3d(13.75*0.0254, -4*0.0254, 8.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); // Camera 8 - public static Transform3d robotToCamera3 = - new Transform3d(0.5*0.0254, 4*0.0254, 9*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + public static Transform3d robotToCamera8 = + new Transform3d(13.75*0.0254, 4*0.0254, 8.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); // Basic filtering thresholds public static double maxAmbiguity = 0.3; @@ -69,10 +68,10 @@ public class VisionConstants { // (Adjust to trust some cameras more than others) public static double[] cameraStdDevFactors = new double[] { - 1.0, // Camera 0 - 1.0, // Camera 1 - 1.0, // Camera 2 - 1.0 // Camera 3 + 1.0, // Camera 5 + 1.0, // Camera 6 + 1.0, // Camera 7 + 1.0 // Camera 8 }; // Multipliers to apply for MegaTag 2 observations From 1409b0f2393aa64dde9d929c7cf729d1d93d8944 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sun, 16 Feb 2025 13:41:54 -0500 Subject: [PATCH 09/28] flipped negative sign of cameras 5 and 6 --- .../java/frc/robot/subsystems/vision/VisionConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 86a0036..271ee0f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -42,10 +42,10 @@ public class VisionConstants { // Camera 5 public static Transform3d robotToCamera5 = - new Transform3d(-13.75*0.0254, 11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + new Transform3d(-13.75*0.0254, -11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); // Camera 6 public static Transform3d robotToCamera6 = - new Transform3d(-13.75*0.0254, -11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + new Transform3d(-13.75*0.0254, 11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); // Camera 7 From 006e0d3a6b6a6c9229d9bf196ad77b2c0bf46643 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Sun, 16 Feb 2025 15:11:44 -0500 Subject: [PATCH 10/28] scrimmage testing bad --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/subsystems/arm/Arm.java | 8 +++++++- src/main/java/frc/robot/subsystems/arm/ArmCommands.java | 5 +---- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 2 +- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index df367ca..522c5c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -201,8 +201,8 @@ private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -joystick.getLeftY(), - () -> -joystick.getLeftX(), + () -> joystick.getLeftY(), + () -> joystick.getLeftX(), () -> -joystick.getRightX())); // drivetrain.setDefaultCommand( @@ -232,7 +232,7 @@ private void configureBindings() { Commands.runOnce( () -> drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), + new Pose2d(drive.getPose().getTranslation(), new Rotation2d(Math.PI))), drive) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 30de58a..f358121 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -49,7 +49,7 @@ public enum ArmPosition { private ProfiledPIDController _armPidController; private static final double KP = 0.09; - private static final double KI = 0.01; + private static final double KI = 0;//0.01; private static final double KD = 0; private static final double PROFILE_VEL = 160; private static final double PROFILE_ACC = 145; @@ -72,9 +72,14 @@ public Arm(ArmIO io) { _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); _armPidController.setTolerance(7); + + _currentPos = ArmPosition.Stow; + _desiredPos = ArmPosition.Stow; + _armPidController.setGoal(ArmPosition.Stow.angle); } public void setArmSetpoint(ArmPosition setpoint) { + System.out.println("SETTING POS"); _armPidController.reset(_inputs._armEncoderPositionDegrees); _armPidController.setGoal(setpoint.angle); _desiredPos = setpoint; @@ -84,6 +89,7 @@ public void setIntakeSpeed(double speed) { _io.setIntakeMotorSpeed(speed); } + public void spit() { setIntakeSpeed(0.5); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index de2a975..cf6750a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -30,9 +30,6 @@ public Command spit() { } public Command setArmPosition(ArmPosition setpoint) { - - return new InstantCommand(); - /* if (setpoint == ArmPosition.L4_Score) { return new InstantCommand( () -> { @@ -44,7 +41,7 @@ public Command setArmPosition(ArmPosition setpoint) { () -> { _arm.setArmSetpoint(setpoint); }, - _arm); */ + _arm); } public Command intake() { diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index aa82fe1..fafceaf 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -64,7 +64,7 @@ private ElevatorPosition(double setpoint) { private static final double DECREMENT_CONSTANT = 1; private static final double ELEVATOR_MOTOR_KP = 0.75; - private static final double ELEVATOR_MOTOR_KI = 0.15; + private static final double ELEVATOR_MOTOR_KI = 0;//0.15; private static final double ELEVATOR_MOTOR_KD = 0; private static final double ELEVATOR_PID_VEL = 220; private static final double ELEVATOR_PID_ACC = 215; From 8dd3a39cfd6b754d2054475713991b282900a84a Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Wed, 19 Feb 2025 19:51:22 -0500 Subject: [PATCH 11/28] DRIVE ODOMETRY FIXED RAHHHH --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/subsystems/drive/DriveCommands.java | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 522c5c3..df367ca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -201,8 +201,8 @@ private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> joystick.getLeftY(), - () -> joystick.getLeftX(), + () -> -joystick.getLeftY(), + () -> -joystick.getLeftX(), () -> -joystick.getRightX())); // drivetrain.setDefaultCommand( @@ -232,7 +232,7 @@ private void configureBindings() { Commands.runOnce( () -> drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d(Math.PI))), + new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index fb3ecca..dac4123 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -89,9 +89,9 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; + boolean isFlipped = false; + // DriverStation.getAlliance().isPresent() + // && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, From 147df761fc49f3db567b4270261ebb8ff45f0834 Mon Sep 17 00:00:00 2001 From: kt-h <113950897+kt-h@users.noreply.github.com> Date: Mon, 3 Mar 2025 18:45:38 -0500 Subject: [PATCH 12/28] added isFlipped that checks alliance every periodic loop --- .../frc/robot/subsystems/drive/Drive.java | 20 +++++++++++++++++++ .../robot/subsystems/drive/DriveCommands.java | 8 ++++---- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5270b06..c8b29c1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -109,9 +109,12 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); + private Alliance _alliance; + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -166,6 +169,15 @@ public Drive( @Override public void periodic() { + + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); @@ -372,4 +384,12 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } + + // Notice: Despite its name, this method was not intended to be offensive. -KtH 2025-03-03 + public boolean getFlipped() { + if (DriverStation.getAlliance().isPresent()){ + _alliance = DriverStation.getAlliance().get(); + } + return _alliance == Alliance.Red; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index dac4123..c0a485d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -89,15 +89,15 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); - boolean isFlipped = false; + drive.getFlipped(); // DriverStation.getAlliance().isPresent() // && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, - isFlipped - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); + drive.getFlipped() + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive); } From 5fd9d5dfe9948ae7df62c8e49ae10edd858c328f Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 14:00:23 -0500 Subject: [PATCH 13/28] Tuning vision and swerve --- gradlew | 0 .../java/frc/robot/HardwareConstants.java | 5 +- src/main/java/frc/robot/Robot.java | 6 - src/main/java/frc/robot/RobotContainer.java | 92 ++---------- .../java/frc/robot/subsystems/arm/Arm.java | 120 +++------------- .../frc/robot/subsystems/arm/ArmCommands.java | 31 ++-- .../robot/subsystems/arm/{ => io}/ArmIO.java | 7 +- .../subsystems/arm/{ => io}/RealArmIO.java | 44 ++---- .../frc/robot/subsystems/climber/Climber.java | 26 ---- .../subsystems/climber/ClimberCommands.java | 22 --- .../robot/subsystems/climber/ClimberIO.java | 16 --- .../subsystems/climber/RealClimberIO.java | 30 ---- ....java => CommandSwerveDrivetrain.java.old} | 0 .../frc/robot/subsystems/drive/Drive.java | 20 --- .../robot/subsystems/drive/DriveCommands.java | 11 +- .../subsystems/drive/TunerConstants.java | 15 +- .../robot/subsystems/elevator/Elevator.java | 135 ++++++++++-------- .../subsystems/elevator/ElevatorCommands.java | 1 - .../elevator/{ => io}/ElevatorIO.java | 2 +- .../elevator/{ => io}/RealElevatorIO.java | 17 ++- .../java/frc/robot/subsystems/leds/LEDs.java | 25 +--- .../robot/subsystems/leds/LEDsCommands.java | 24 +--- .../MultiSubsystemCommands.java | 129 +---------------- .../frc/robot/subsystems/presets/Preset.java | 2 +- .../subsystems/vision/VisionConstants.java | 11 +- 25 files changed, 175 insertions(+), 616 deletions(-) mode change 100644 => 100755 gradlew rename src/main/java/frc/robot/subsystems/arm/{ => io}/ArmIO.java (80%) rename src/main/java/frc/robot/subsystems/arm/{ => io}/RealArmIO.java (53%) delete mode 100644 src/main/java/frc/robot/subsystems/climber/Climber.java delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberCommands.java delete mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIO.java delete mode 100644 src/main/java/frc/robot/subsystems/climber/RealClimberIO.java rename src/main/java/frc/robot/subsystems/drive/{CommandSwerveDrivetrain.java => CommandSwerveDrivetrain.java.old} (100%) rename src/main/java/frc/robot/subsystems/elevator/{ => io}/ElevatorIO.java (94%) rename src/main/java/frc/robot/subsystems/elevator/{ => io}/RealElevatorIO.java (75%) diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 73ac298..2e2512f 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -4,7 +4,7 @@ public class HardwareConstants { - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.REPLAY; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public static enum Mode { @@ -24,14 +24,11 @@ public class CAN { public static final int ARM_MTR_ID = 14; public static final int INTAKE_MTR_ID = 15; - public static final int CLIMBER_MTR_ID = 1; public static final int PRIMARY_ELEVATOR_ID = 16; public static final int SECONDARY_ELEVATOR_ID = 17; public static final int CANDLE_ID = 50; - - public static final int FEEDER_MTR_ID = 20; } public class DIO { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9cb1760..7224c30 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -59,7 +59,6 @@ public void robotPeriodic() { @Override public void disabledInit() { - m_robotContainer.startIdleAnimations(); } @Override @@ -77,9 +76,6 @@ public void autonomousInit() { if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } - - m_robotContainer.calibrateAndStartPIDs(); - m_robotContainer.startEnabledLEDs(); } @Override @@ -95,8 +91,6 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - m_robotContainer.calibrateAndStartPIDs(); - m_robotContainer.startEnabledLEDs(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7ecbff8..0da42f6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,9 @@ import static edu.wpi.first.units.Units.*; +import java.lang.invoke.VarHandle.AccessMode; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.pathplanner.lib.auto.NamedCommands; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -13,7 +16,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -21,15 +23,11 @@ import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmCommands; import frc.robot.subsystems.arm.Arm.ArmPosition; -import frc.robot.subsystems.climber.Climber; -import frc.robot.subsystems.climber.ClimberCommands; -import frc.robot.subsystems.climber.RealClimberIO; -import frc.robot.subsystems.arm.RealArmIO; +import frc.robot.subsystems.arm.io.RealArmIO; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveCommands; import frc.robot.subsystems.drive.Telemetry; import frc.robot.subsystems.drive.TunerConstants; -import frc.robot.subsystems.elevator.CmdElevatorCalibrate; import frc.robot.subsystems.drive.io.GyroIO; import frc.robot.subsystems.drive.io.GyroIOPigeon2; import frc.robot.subsystems.drive.io.ModuleIO; @@ -37,10 +35,8 @@ import frc.robot.subsystems.drive.io.ModuleIOTalonFX; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorCommands; -import frc.robot.subsystems.elevator.RealElevatorIO; -import frc.robot.subsystems.leds.LEDs; -import frc.robot.subsystems.leds.LEDsCommands; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; +import frc.robot.subsystems.elevator.io.RealElevatorIO; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; import frc.robot.subsystems.presets.Preset; @@ -55,14 +51,10 @@ public class RobotContainer { private final Drive drive; private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); - private final LEDs LEDSubsystem = new LEDs(); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); private final Vision vision; - private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); - - private final Climber climber = new Climber(new RealClimberIO()); - private final ClimberCommands ClimberCommands = new ClimberCommands(climber); + private final CommandXboxController joystick = new CommandXboxController(0); private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); @@ -75,7 +67,7 @@ public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max // angular velocity - + /* Setting up bindings for necessary control of the swerve drive platform */ // private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() // .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband @@ -83,9 +75,6 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final CommandXboxController joystick = new CommandXboxController(0); - private final CommandXboxController climberstick = new CommandXboxController(1); - private final GenericHID buttonbox1 = new GenericHID(1); private final JoystickButton L1Button = new JoystickButton(buttonbox1, 1); private final JoystickButton L2Button = new JoystickButton(buttonbox1, 2); @@ -93,14 +82,12 @@ public class RobotContainer { private final JoystickButton L3Button = new JoystickButton(buttonbox1, 4); private final JoystickButton L4Button = new JoystickButton(buttonbox1, 5); private final JoystickButton LoadingButton = new JoystickButton(buttonbox1, 6); - private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); + private final JoystickButton button7 = new JoystickButton(buttonbox1, 7); private final JoystickButton L4_scoreButton = new JoystickButton(buttonbox1, 8); - private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); + private final JoystickButton button9 = new JoystickButton(buttonbox1, 9); private final GenericHID buttonbox2 = new GenericHID(2); private final JoystickButton presetButton = new JoystickButton(buttonbox2, 2); - private final JoystickButton incrementButton = new JoystickButton(buttonbox2, 4); - private final JoystickButton decrementButton = new JoystickButton(buttonbox2, 7); // public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @@ -190,7 +177,7 @@ public RobotContainer() { // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", - multiSubsystemCommands.loadGamepiece()); + multiSubsystemCommands.intake()); configureBindings(); } @@ -206,11 +193,6 @@ private void configureBindings() { () -> -joystick.getLeftX(), () -> -joystick.getRightX())); - climber.setDefaultCommand( - ClimberCommands.runClimber( - () -> -climberstick.getLeftY())); - - // drivetrain.setDefaultCommand( // // Drivetrain will execute this command periodically // drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with @@ -230,10 +212,6 @@ private void configureBindings() { joystick.start().and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); joystick.start().and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); - //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()) - - //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()) - // Reset gyro to 0° when left bumper button is pressed joystick @@ -251,8 +229,9 @@ private void configureBindings() { // drive.registerTelemetry(logger::telemeterize); - intakeButton.onTrue(multiSubsystemCommands.loadGamepiece().raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); - spitButton.onTrue(armCommands.spit()); + // Elevator sys id routines + button7.whileTrue(elevatorSubsystem.sysIdDynamic(Direction.kForward)); + button9.whileTrue(elevatorSubsystem.sysIdDynamic(Direction.kReverse)); L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); @@ -264,60 +243,19 @@ private void configureBindings() { // Runs the preset to score unless the preset is invalid. joystick.rightBumper().onTrue( - multiSubsystemCommands.scoreGamepieceAtPosition(() -> preset.getLevel()).unless(() - -> !preset.isPresetValid())); + multiSubsystemCommands.scoreGamepieceAtPosition(preset.getLevel()).unless(() -> !preset.isPresetValid())); // Resets the preset when we don't have a piece. - armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); + armSubsystem._hasPiece.onFalse(preset.resetPreset()); // Sets the level preset presetButton.and(L1Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L1)); presetButton.and(L2Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L2)); presetButton.and(L3Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L3)); presetButton.and(L4Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L4)); - - incrementButton.onTrue(elevatorCommands.incrementElevatorPosition()); - decrementButton.onTrue(elevatorCommands.decrementElevatorPosition()); } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } - - public void calibrateAndStartPIDs() { - // PID commands: we only want one of them so start/stop works correctly - Command elevatorPIDCommand = elevatorCommands.runElevatorPID(); - Command armPIDCommand = armCommands.runArmPID(); - // Start elevator pid - if (elevatorSubsystem.isCalibrated()) { - elevatorCommands.runElevatorPID(); - if (!CommandScheduler.getInstance().isScheduled(elevatorPIDCommand)) { - CommandScheduler.getInstance().schedule(elevatorPIDCommand); - } - } else { - Command calibCommand = new CmdElevatorCalibrate(elevatorSubsystem).andThen(elevatorPIDCommand); - CommandScheduler.getInstance().schedule(calibCommand); - } - - // Start arm pid - if (!CommandScheduler.getInstance().isScheduled(armPIDCommand)) { - CommandScheduler.getInstance().schedule(armPIDCommand); - } - - // Set initial positions - CommandScheduler.getInstance().schedule(elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); - CommandScheduler.getInstance().schedule(armCommands.setArmPosition(ArmPosition.Stow)); - } - - public void startIdleAnimations() { - Command disabled1 = LEDCommands.disabledAnimation1(); - if (!CommandScheduler.getInstance().isScheduled(disabled1)) - CommandScheduler.getInstance().schedule(disabled1); - } - - public void startEnabledLEDs() { - Command initialLEDs = LEDCommands.pickingUpCoral(); - if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) - CommandScheduler.getInstance().schedule(initialLEDs); - } } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index cbd57b6..869cb97 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -3,32 +3,32 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Volt; +import java.util.function.BooleanSupplier; -import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; +import frc.robot.subsystems.arm.io.ArmIO; +import frc.robot.subsystems.arm.io.ArmIO.ArmIOInputs; public class Arm extends SubsystemBase { public enum ArmPosition { - Stow(80), - Loading_Coral(120), - Loading_Algae(50), - Loading(120), - L4_Score(45), - Algae_Score(60); + Stow(40), + Loading(30), + L4_Score(45); double angle; @@ -40,73 +40,36 @@ public enum ArmPosition { public final Trigger _hasPiece = new Trigger(() -> hasPiece()); private ArmIO _io; - private ArmIOInputsAutoLogged _inputs; + private ArmIOInputs _inputs; private boolean _prevLightSensorVal; - private boolean _hasGamepiece; - private int _intakeSpikeCounter; - private ArmPosition _currentPos; - private ArmPosition _desiredPos; - private MultiSubsystemCommands.GamepieceMode _currentMode; private ProfiledPIDController _armPidController; - private static final double KP = 0.09; - private static final double KI = 0;//0.01; + private static final double KP = 0; + private static final double KI = 0; private static final double KD = 0; - private static final double PROFILE_VEL = 160; - private static final double PROFILE_ACC = 145; - - private static final double HAS_ALGAE_CURRENT = 2; - - private static final double ARM_WEIGHT_N = 3.5 * 9.81; - private static final double ARM_STALL_TORQUE_NM = 3.6; - private static final double ARM_STALL_CURRENT = 211; - private static final double ARM_KT = ARM_STALL_TORQUE_NM / ARM_STALL_CURRENT; - private static final double ARM_RESISTANCE = 0.057; - private static final double ARM_MOMENT_METERS = 0.3928; - private static final double ARM_GEARING = 17; - private static final double ARM_FEEDFORWARD_COEFF = 0.53; + private static final double PROFILE_VEL = 0; + private static final double PROFILE_ACC = 0; SysIdRoutine routine = new SysIdRoutine(new Config(), new SysIdRoutine.Mechanism(this::setArmVoltage, this::populateLog, this)); public Arm(ArmIO io) { _io = io; - _inputs = new ArmIOInputsAutoLogged(); + _inputs = new ArmIOInputs(); _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); - _armPidController.setTolerance(7); - _currentPos = ArmPosition.Stow; - _desiredPos = ArmPosition.Stow; - _armPidController.setGoal(ArmPosition.Stow.angle); } public void setArmSetpoint(ArmPosition setpoint) { - if (setpoint == ArmPosition.Loading) - setpoint = (_currentMode == GamepieceMode.ALGAE) ? ArmPosition.Loading_Algae : ArmPosition.Loading_Coral; - - _armPidController.reset(_inputs._armEncoderPositionDegrees); _armPidController.setGoal(setpoint.angle); - _desiredPos = setpoint; } public void setIntakeSpeed(double speed) { _io.setIntakeMotorSpeed(speed); } - public void setFeederSpeed(double speed) { - _io.setFeederMotorSpeed(speed); - } - - public void spit() { - setIntakeSpeed(0.5); - } - - public void clearHasGamepiece() { - _hasGamepiece = false; - } - public void setArmVoltage(Voltage voltage) { _io.setArmMotorVoltage(voltage); } @@ -116,56 +79,22 @@ public void resetIntakeEncoders() { } public boolean intakeAtDesiredRotations() { - return _inputs._intakeMotorPositionRotations <= -2; + return _inputs._intakeMotorPositionRotations >= 2; } public boolean hasPiece() { - boolean hasPiece; - if (_currentMode == GamepieceMode.CORAL) { - boolean currentState = _inputs._lightSensorState; - hasPiece = _prevLightSensorVal && !currentState; - _prevLightSensorVal = currentState; - } else { - if (_inputs._intakeMotorCurrent >= HAS_ALGAE_CURRENT) { - _intakeSpikeCounter++; - } - hasPiece = _intakeSpikeCounter >= 5; - } - + boolean current = _inputs._lightSensorState; + boolean hasPiece = _prevLightSensorVal && !current; + _prevLightSensorVal = current; return hasPiece; } - public void resetIntakeSpikeCounter() { - _intakeSpikeCounter = 0; - } - - public boolean lightSensorSeesGamepiece() { - return _inputs._lightSensorState; - } - public boolean armAtSetpoint() { - boolean atSetpoint = _armPidController.atGoal(); - if (atSetpoint) - _currentPos = _desiredPos; - return atSetpoint; - } - - public ArmPosition getCurrentPos() { - return _currentPos; + return _armPidController.atGoal(); } public void runArmPID() { - double out = (_armPidController.calculate(_inputs._armEncoderPositionDegrees) - + ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees))); - _io.setArmMotorVoltage(Voltage.ofBaseUnits(out, Volt)); - } - - public GamepieceMode getCurrentMode() { - return _currentMode; - } - - public void setCurrentMode(GamepieceMode mode) { - _currentMode = mode; + _io.setArmMotorSpeed(_armPidController.calculate(_inputs._armEncoderPositionDegrees)); } public void populateLog(SysIdRoutineLog log) { @@ -187,12 +116,5 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { _io.updateInputs(_inputs); - Logger.processInputs("Arm", _inputs); - - Logger.recordOutput("Arm/desiredPos", _armPidController.getSetpoint().position); - Logger.recordOutput("Arm/hasPiece", hasPiece()); - Logger.recordOutput("Arm/atSetpoint", armAtSetpoint()); - Logger.recordOutput("Arm/currentPosEnum", _currentPos); - Logger.recordOutput("Arm/desiredPosEnum", _desiredPos); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index 7ae61c1..a8242b1 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -17,58 +17,43 @@ public ArmCommands(Arm arm) { public Command spit() { return new StartEndCommand( () -> { - if (_arm.getCurrentPos() == ArmPosition.L4_Score) - _arm.setArmSetpoint(ArmPosition.Stow); - _arm.spit(); + _arm.setIntakeSpeed(0.5); }, () -> { _arm.setIntakeSpeed(0); - _arm.clearHasGamepiece(); }, - _arm).withTimeout(0.5); - + _arm).withTimeout(1); } public Command setArmPosition(ArmPosition setpoint) { + + return new InstantCommand(); + /* if (setpoint == ArmPosition.L4_Score) { return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); }, - _arm).andThen(intakeForNumberOfRotations()); + _arm).alongWith(intakeForNumberOfRotations()); } return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); }, - _arm); + _arm); */ } public Command intake() { return new StartEndCommand( () -> { - _arm.resetIntakeSpikeCounter(); - _arm.setIntakeSpeed(0.45); - _arm.setFeederSpeed(0.45); + _arm.setIntakeSpeed(0.5); }, () -> { _arm.setIntakeSpeed(0); - _arm.setFeederSpeed(0); }, _arm).until(() -> _arm.hasPiece()); } - public Command moveGamepieceToLightSensor() { - return new StartEndCommand( - () -> { - _arm.setIntakeSpeed(-0.3); - }, - () -> { - _arm.setIntakeSpeed(0); - }, - _arm).until(() -> _arm.lightSensorSeesGamepiece()); - } - public Command runArmPID() { return Commands.run(() -> { _arm.runArmPID(); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java similarity index 80% rename from src/main/java/frc/robot/subsystems/arm/ArmIO.java rename to src/main/java/frc/robot/subsystems/arm/io/ArmIO.java index 9f4ca5d..7f4633b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.arm; +package frc.robot.subsystems.arm.io; import org.littletonrobotics.junction.AutoLog; @@ -11,10 +11,6 @@ public static class ArmIOInputs { public double _armMotorVoltage = 0.0; public double _armMotorSpeed = 0.0; - public double _feederMotorCurrent = 0.0; - public double _feederMotorVoltage = 0.0; - public double _feederMotorSpeed = 0.0; - public double _armEncoderPositionDegrees = 0.0; public double _armEncoderVelocity = 0.0; @@ -36,5 +32,4 @@ public default void resetIntakeEncoders() {} public default void setArmMotorVoltage(Voltage voltage) {} - public default void setFeederMotorSpeed(double speed) {} } diff --git a/src/main/java/frc/robot/subsystems/arm/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java similarity index 53% rename from src/main/java/frc/robot/subsystems/arm/RealArmIO.java rename to src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java index 920e7bf..b0e01c3 100644 --- a/src/main/java/frc/robot/subsystems/arm/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java @@ -1,13 +1,9 @@ -package frc.robot.subsystems.arm; +package frc.robot.subsystems.arm.io; import com.revrobotics.spark.SparkAbsoluteEncoder; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; @@ -16,30 +12,22 @@ public class RealArmIO implements ArmIO { - private static final double POS_AT_90 = 0.447; - private static final double POS_AT_0 = 0.701; - private static final double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) / 90.0; + public double POS_AT_90 = 0.0; + public double POS_AT_0 = 0.0; + public double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) * 90; private double INTAKE_ROTATION_CONVERSION = 1; private SparkFlex _armMotor; private DigitalInput _lightSensor; - private SparkFlex _intakeMotor; + private SparkMax _intakeMotor; private SparkAbsoluteEncoder _armEncoder; - private SparkMax _feederMotor; public RealArmIO() { _armMotor = new SparkFlex(CAN.ARM_MTR_ID, MotorType.kBrushless); - _intakeMotor = new SparkFlex(CAN.INTAKE_MTR_ID, MotorType.kBrushless); - _feederMotor = new SparkMax(CAN.FEEDER_MTR_ID, MotorType.kBrushless); + _intakeMotor = new SparkMax(CAN.INTAKE_MTR_ID, MotorType.kBrushless); _lightSensor = new DigitalInput(DIO.LIGHT_SENSOR_CHANNEL); - _armEncoder = _armMotor.getAbsoluteEncoder(); - - SparkFlexConfig armConfig = new SparkFlexConfig(); - armConfig.idleMode(IdleMode.kBrake); - armConfig.inverted(true); - armConfig.voltageCompensation(12); - _armMotor.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + _armEncoder = _intakeMotor.getAbsoluteEncoder(); } public void updateInputs(ArmIOInputs inputs) { @@ -47,17 +35,13 @@ public void updateInputs(ArmIOInputs inputs) { inputs._armMotorCurrent = _armMotor.getOutputCurrent(); inputs._armMotorVoltage = _armMotor.getAppliedOutput() * _armMotor.getBusVoltage(); - inputs._feederMotorSpeed = _feederMotor.get(); - inputs._feederMotorCurrent = _feederMotor.getOutputCurrent(); - inputs._feederMotorVoltage = _feederMotor.getAppliedOutput() * _feederMotor.getBusVoltage(); - inputs._lightSensorState = !_lightSensor.get(); inputs._intakeMotorVelocityRotationsPerMin = _intakeMotor.get(); inputs._intakeMotorCurrent = _intakeMotor.getOutputCurrent(); inputs._intakeMotorVoltage = _intakeMotor.getAppliedOutput() * _armMotor.getBusVoltage(); inputs._intakeMotorPositionRotations = _intakeMotor.getEncoder().getPosition() * INTAKE_ROTATION_CONVERSION; - inputs._armEncoderPositionDegrees = (_armEncoder.getPosition() - POS_AT_0) / ENCODER_CONVERSION; + inputs._armEncoderPositionDegrees = _armEncoder.getPosition() * ENCODER_CONVERSION; inputs._armEncoderVelocity = _armEncoder.getVelocity(); } @@ -65,14 +49,10 @@ public void setArmMotorSpeed(double speed) { _armMotor.set(speed); } - public void setIntakeMotorSpeed(double speed) { + public void setIntakeSpeed(double speed) { _intakeMotor.set(speed); } - public void setFeederMotorSpeed(double speed){ - _feederMotor.set(speed); - - } public void resetIntakeEncoders() { _intakeMotor.getEncoder().setPosition(0); } @@ -80,8 +60,4 @@ public void resetIntakeEncoders() { public void setArmMotorVoltage(Voltage voltage) { _armMotor.setVoltage(voltage); } - - public void setFeederMotorVoltage(Voltage voltage) { - _feederMotor.setVoltage(voltage); - } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java deleted file mode 100644 index 464f269..0000000 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.subsystems.climber; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Climber extends SubsystemBase { - - private ClimberIO _io; - private ClimberIOInputsAutoLogged _inputs; - - public Climber(ClimberIO io) { - _io = io; - _inputs = new ClimberIOInputsAutoLogged(); - } - - public void setClimberSpeed(double speed) { - _io.setClimberMotorSpeed(speed); - } - - @Override - public void periodic() { - _io.updateInputs(_inputs); - Logger.processInputs("Climber", _inputs); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java deleted file mode 100644 index 5474c0f..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.subsystems.climber; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - -public class ClimberCommands { - private Climber _climber; - - public ClimberCommands(Climber climber) { - _climber = climber; - } - - public Command runClimber(DoubleSupplier ySupplier) { - return Commands.run( - () -> { - _climber.setClimberSpeed(ySupplier.getAsDouble()); - }, - _climber); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java deleted file mode 100644 index 90bcb9f..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.subsystems.climber; - -import org.littletonrobotics.junction.AutoLog; - -import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; - -public interface ClimberIO { - @AutoLog - public static class ClimberIOInputs { - public double _climberMotorSpeed = 0.0; - } - - public default void updateInputs(ClimberIOInputs inputs) {} - - public default void setClimberMotorSpeed(double speed) {} -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java b/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java deleted file mode 100644 index 16b4b90..0000000 --- a/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.subsystems.climber; - -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import frc.robot.HardwareConstants.CAN; - -public class RealClimberIO implements ClimberIO{ - private SparkFlex _climberMotor; - - public RealClimberIO() { - _climberMotor = new SparkFlex(CAN.CLIMBER_MTR_ID, MotorType.kBrushless); - - SparkFlexConfig climberConfig = new SparkFlexConfig(); - climberConfig.idleMode(IdleMode.kBrake); - _climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - public void updateInputs(ClimberIOInputs inputs) { - inputs._climberMotorSpeed = _climberMotor.get(); - } - - public void setClimberMotorSpeed(double speed) { - _climberMotor.set(speed); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java.old similarity index 100% rename from src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java rename to src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java.old diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c8b29c1..5270b06 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -109,12 +109,9 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - private Alliance _alliance; - public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -169,15 +166,6 @@ public Drive( @Override public void periodic() { - - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); @@ -384,12 +372,4 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } - - // Notice: Despite its name, this method was not intended to be offensive. -KtH 2025-03-03 - public boolean getFlipped() { - if (DriverStation.getAlliance().isPresent()){ - _alliance = DriverStation.getAlliance().get(); - } - return _alliance == Alliance.Red; - } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index c0a485d..9d087cc 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -89,15 +89,14 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); - drive.getFlipped(); - // DriverStation.getAlliance().isPresent() - // && DriverStation.getAlliance().get() == Alliance.Red; + boolean isFlipped = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, - drive.getFlipped() - ? drive.getRotation().plus(new Rotation2d(Math.PI)) - : drive.getRotation())); + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation())); }, drive); } diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index f6cdc10..c9905ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -22,13 +22,13 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) + .withKP(107).withKI(0).withKD(0.7) .withKS(0.1).withKV(2.66).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) + .withKP(2.3).withKI(0).withKD(0) .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; @@ -127,7 +127,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.378662109375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.3748); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.221435546875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.170); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.2783203125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.2615); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.142333984375); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.1426); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; @@ -193,11 +193,12 @@ public class TunerConstants { * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ + /* public static CommandSwerveDrivetrain createDrivetrain() { return new CommandSwerveDrivetrain( DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight ); - } + } */ /** diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 2b3ca41..facbe08 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -7,33 +7,49 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Volt; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.VelocityUnit; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; - +import frc.robot.HardwareConstants.CAN; import frc.robot.subsystems.arm.Arm.ArmPosition; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; +import frc.robot.subsystems.arm.io.ArmIO.ArmIOInputs; +import frc.robot.subsystems.elevator.io.ElevatorIO; +import frc.robot.subsystems.elevator.io.ElevatorIO.ElevatorIOInputs; public class Elevator extends SubsystemBase { public enum ElevatorPosition { - L1(5), - L2(9), - L3(25.5), - L4(48), - Stow(0.5); + L1(1), + L2(2), + L3(3), + L4(4), + Stow(5); public final double setpoint; @@ -43,65 +59,65 @@ private ElevatorPosition(double setpoint) { } private static final double CALIBRATION_SPEED = -0.1; - private static final double HARD_STOP_CURRENT_LIMIT = 50; + private static final double HARD_STOP_CURRENT_LIMIT = 30; private static final double INCREMENT_CONSTANT = 1; private static final double DECREMENT_CONSTANT = 1; - private static final double ELEVATOR_MOTOR_KP = 0.75; - private static final double ELEVATOR_MOTOR_KI = 0;//0.15; - private static final double ELEVATOR_MOTOR_KD = 0; - private static final double ELEVATOR_PID_VEL = 220; - private static final double ELEVATOR_PID_ACC = 215; + private static final double ELEVATOR_MOTOR_KP = 0.21; // 0.08 + private static final double ELEVATOR_MOTOR_KI = 0; // 0.001 + private static final double ELEVATOR_MOTOR_KD = 0; // 0.002 + private static final double ELEVATOR_PID_VEL = 120; + private static final double ELEVATOR_PID_ACC = 125; - private static final double ELEVATOR_MAX_INCHES = 48; - private static final double ELEVATOR_MAX_ROTATIONS = 36.4; + private static final double ELEVATOR_MOTOR_TOLERANCE = 0.0; + + private static final double ELEVATOR_MAX_INCHES = 14.0; + private static final double ELEVATOR_MAX_ROTATIONS = 28.67; private static final double MOTOR_CONVERSION = ELEVATOR_MAX_INCHES / ELEVATOR_MAX_ROTATIONS; - // In newtons - private static final double ELEVATOR_STAGE1_WEIGHT_N = 2.053 * 9.81; - private static final double ELEVATOR_STAGE2_WEIGHT_N = 7.554 * 9.81; - private static final double ELEVATOR_TOTAL_WEIGHT_N = ELEVATOR_STAGE1_WEIGHT_N + (ELEVATOR_STAGE2_WEIGHT_N); + // In pounds + private static final double ELEVATOR_STAGE1_WEIGHT = 3.75; + private static final double ELEVATOR_STAGE2_WEIGHT = 0; + + // In inches + private static final double SPOOL_DIAMETER = 1.6; - // In m - private static final double SPOOL_DIAMETER = 0.0527; + // In pound - inches + private static final double STALL_TORQUE = 62.75; + + // In amps + private static final double STALL_CURRENT = 366; + + // In Ohms + private static final double RESISTANCE = 0.0185; - private static final double ELEVATOR_STALL_TORQUE_LB_IN = 3.6; - private static final double ELEVATOR_STALL_CURRENT = 211; - private static final double ELEVATOR_KT = ELEVATOR_STALL_TORQUE_LB_IN / ELEVATOR_STALL_CURRENT; private static final double GEAR_RATIO = 7.75; private static final double NUMBER_OF_MOTORS = 2; - private static final double EFFECTIVE_KT = ELEVATOR_KT * NUMBER_OF_MOTORS * GEAR_RATIO; - private static final double ELEVATOR_RESISTANCE = 0.057; - private static final double FEEDFORWARD_CONSTANT = ((ELEVATOR_TOTAL_WEIGHT_N * SPOOL_DIAMETER * ELEVATOR_RESISTANCE) / (EFFECTIVE_KT)) - 0.65; + private static final double FEEDFORWARD_CONSTANT = ((ELEVATOR_STAGE1_WEIGHT + (2 * ELEVATOR_STAGE2_WEIGHT)) + * SPOOL_DIAMETER * STALL_CURRENT * RESISTANCE) / (NUMBER_OF_MOTORS * GEAR_RATIO * STALL_TORQUE); private boolean _isCalibrated; private ElevatorPosition _currentPos; - private ElevatorPosition _desiredPos; - private MultiSubsystemCommands.GamepieceMode _currentMode; private ProfiledPIDController _elevatorMotorPID; private ElevatorIO _io; - private ElevatorIOInputsAutoLogged _inputs; - - + private ElevatorIOInputs _inputs; SysIdRoutine routine = new SysIdRoutine(new Config(), new SysIdRoutine.Mechanism(this::setElevatorVoltage, this::populateLog, this)); public Elevator(ElevatorIO io) { _io = io; - _inputs = new ElevatorIOInputsAutoLogged(); + _inputs = new ElevatorIOInputs(); _elevatorMotorPID = new ProfiledPIDController(ELEVATOR_MOTOR_KP, ELEVATOR_MOTOR_KI, ELEVATOR_MOTOR_KD, new Constraints(ELEVATOR_PID_VEL, ELEVATOR_PID_ACC)); - _elevatorMotorPID.setTolerance(0.5); - - _currentPos = ElevatorPosition.Stow; - _desiredPos = ElevatorPosition.Stow; + _elevatorMotorPID.setTolerance(ELEVATOR_MOTOR_TOLERANCE); + _elevatorMotorPID.setGoal(0.25); } // Runs the motors down at the calibration speed @@ -119,23 +135,20 @@ public void setElevatorVoltage(Voltage voltage) { public void setSetpoint(ElevatorPosition setpoint) { setSetpoint(setpoint.setpoint); - _desiredPos = setpoint; + _currentPos = setpoint; } // Sets the setpoint of the PID public void setSetpoint(double setpoint) { if (setpoint < 0.25 || setpoint > ELEVATOR_MAX_INCHES) return; - _elevatorMotorPID.reset(getCurrentPosInches()); _elevatorMotorPID.setGoal(setpoint); + _elevatorMotorPID.reset(getCurrentPosInches()); } // Checks if it is at the setpoint public boolean isAtSetpoint() { - boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 0.5; - if (atSetpoint) - _currentPos = _desiredPos; - return atSetpoint; + return _elevatorMotorPID.atSetpoint(); } // Increases elevator position @@ -161,8 +174,7 @@ public void resetEncoders() { // Runs motors with PID public void runMotorsWithPID() { - // _io.setElevatorSpeed(); - _io.setElevatorVoltage(Voltage.ofBaseUnits(_elevatorMotorPID.calculate(getCurrentPosInches()) + FEEDFORWARD_CONSTANT, Volt)); + _io.setElevatorSpeed(_elevatorMotorPID.calculate(getCurrentPosInches())); } public boolean isCalibrated() { @@ -175,23 +187,26 @@ public void setIsCalibrated(boolean isCalibrated) { // Converts the motors position from weird units to normal people inches public double getCurrentPosInches() { - return (_inputs._elevatorPosition * MOTOR_CONVERSION) - 0.25; + return _inputs._elevatorPosition * MOTOR_CONVERSION; } public ElevatorPosition getCurrentPos() { return _currentPos; } - public GamepieceMode getCurrentMode() { - return _currentMode; - } - - public void setCurrentMode(GamepieceMode mode) { - _currentMode = mode; - } - - public double getElevatorMaxHeight() { - return ELEVATOR_MAX_INCHES; + public boolean canMoveToPos(ElevatorPosition currentElevator, ArmPosition desiredArm) { + switch (currentElevator) { + case L1: + case L2: + case L3: + return (desiredArm != ArmPosition.L4_Score) || (desiredArm != ArmPosition.Loading); + case L4: + return desiredArm != ArmPosition.Loading; + case Stow: + return desiredArm != ArmPosition.L4_Score; + default: + return false; + } } public void populateLog(SysIdRoutineLog log) { @@ -213,7 +228,6 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { _io.updateInputs(_inputs); - Logger.processInputs("Elevator", _inputs); // This method will be called once per scheduler run Logger.recordOutput("Elevator/currentPos", getCurrentPosInches()); @@ -222,9 +236,8 @@ public void periodic() { Logger.recordOutput("Elevator/Current", _inputs._elevatorMotorCurrent); Logger.recordOutput("Elevator/motorSpeed", _inputs._elevatorSpeed); Logger.recordOutput("Elevator/CurrentSetpoint", _elevatorMotorPID.getSetpoint().position); - Logger.recordOutput("Elevator/atSetpoint", isAtSetpoint()); - Logger.recordOutput("Elevator/currentPosEnum", _currentPos); - Logger.recordOutput("Elevator/desiredPosEnum", _desiredPos); + + // Logger.recordOutput("Elevator/motorSpeed", _primaryMotor.get()); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 38f5634..02025b2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; -import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; public final class ElevatorCommands { private Elevator _elevator; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java similarity index 94% rename from src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java index a681447..e354123 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator; +package frc.robot.subsystems.elevator.io; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java similarity index 75% rename from src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java index 5986afc..e8275a1 100644 --- a/src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator; +package frc.robot.subsystems.elevator.io; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -11,8 +11,8 @@ import com.revrobotics.spark.config.SparkFlexConfig; import frc.robot.HardwareConstants.CAN; -import frc.robot.subsystems.arm.ArmIO; -import frc.robot.subsystems.elevator.ElevatorIO.ElevatorIOInputs; +import frc.robot.subsystems.arm.io.ArmIO; +import frc.robot.subsystems.elevator.io.ElevatorIO.ElevatorIOInputs; public class RealElevatorIO implements ElevatorIO { private SparkFlex _primaryMotor; @@ -24,14 +24,13 @@ public RealElevatorIO() { _secondaryMotor = new SparkFlex(CAN.SECONDARY_ELEVATOR_ID, MotorType.kBrushless); SparkFlexConfig primaryConfig = new SparkFlexConfig(); - primaryConfig.inverted(false); - primaryConfig.idleMode(IdleMode.kBrake); - _primaryMotor.configure(primaryConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + primaryConfig.inverted(true); + primaryConfig.idleMode(IdleMode.kCoast); + _primaryMotor.configure(primaryConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); SparkFlexConfig secondaryConfig = new SparkFlexConfig(); secondaryConfig.follow(CAN.PRIMARY_ELEVATOR_ID); - secondaryConfig.idleMode(IdleMode.kBrake); - _secondaryMotor.configure(secondaryConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + _secondaryMotor.configure(secondaryConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } public void updateInputs(ElevatorIOInputs inputs) { @@ -43,7 +42,7 @@ public void updateInputs(ElevatorIOInputs inputs) { } public void setElevatorSpeed(double speed) { - _primaryMotor.set(speed); + _primaryMotor.set(speed); } public void setElevatorVoltage(Voltage voltage) { diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index 11da210..a2614b3 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -6,11 +6,9 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.HardwareConstants; import frc.robot.HardwareConstants.CAN; -import frc.robot.subsystems.elevator.Elevator; import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; @@ -22,12 +20,10 @@ public class LEDs extends SubsystemBase { CANdle _candle = new CANdle(CAN.CANDLE_ID); - static int _numLEDs = 60; //leds/height * currentheight + static int _numLEDs = 8; boolean _alreadyRunning = false; LEDAnimation _currentAnimation = LEDAnimation.None; - Elevator _elevator; - public enum LEDAnimation { None(null, null, 0), @@ -47,9 +43,7 @@ public enum LEDAnimation { SolidTeal(new LEDColor(0, 225, 174), null, 0), - SolidCoral(new LEDColor(255, 80, 15), null, 0), - - SolidRed(new LEDColor(255, 0, 0), null, 0); + SolidCoral(new LEDColor(255, 80, 15), null, 0); LEDColor _color; Animation _animation; @@ -87,11 +81,6 @@ public void runAnimation(LEDAnimation animation) { _candle.clearAnimation(0); _candle.setLEDs(0, 0, 0); _candle.animate(animation.getAnimation()); - } else if (animation == LEDAnimation.SolidRed) { - _candle.clearAnimation(0); - LEDColor color = animation.getColor(); - _candle.setLEDs(color.getR(), color.getG(), color.getB(),0, 0, - (int) Math.round(_numLEDs/_elevator.getElevatorMaxHeight() * _elevator.getCurrentPosInches())); } else if (animation.getAnimation() == null) { LEDColor color = animation.getColor(); _candle.clearAnimation(0); @@ -130,7 +119,7 @@ public void pickingUpCoral() { public void elevatorOrArmIsMoving() { if (!_alreadyRunning) { - runAnimation(LEDAnimation.SolidDarkBlue); + runAnimation(LEDAnimation.BlinkDarkBlue); _alreadyRunning = false; } } @@ -156,13 +145,6 @@ public void robotHasClimbed() { } } - public void disabledAnimation1() { - if (!_alreadyRunning) { - runAnimation(LEDAnimation.PartyMode); - _alreadyRunning = false; - } - } - public void reset() { _candle.clearAnimation(0); _alreadyRunning = false; @@ -170,7 +152,6 @@ public void reset() { @Override public void periodic() { - Logger.recordOutput("LEDs/alreadyRunning", _alreadyRunning); Logger.recordOutput("LEDs/CurrentAnimation", _currentAnimation.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java index 1649a9d..a664aca 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java @@ -14,7 +14,6 @@ public LEDsCommands(LEDs leds) { _leds = leds; } - //Runs when intake button is pushed public Command intaking() { return new StartEndCommand( () -> { @@ -26,7 +25,6 @@ public Command intaking() { _leds); } - //Runs when we have just acquired a piece public Command hasPiece() { return new StartEndCommand( () -> { @@ -35,10 +33,9 @@ public Command hasPiece() { () -> { _leds.reset(); }, - _leds).withTimeout(3); + _leds); } - // Runs when we have no gamepiece and we are toggled to pick up coral public Command pickingUpCoral() { return new StartEndCommand( () -> { @@ -50,7 +47,6 @@ public Command pickingUpCoral() { _leds); } - // Runs when we have no gamepiece and we are toggled to pick up coral public Command pickingUpAlgae() { return new StartEndCommand( () -> { @@ -62,7 +58,6 @@ public Command pickingUpAlgae() { _leds); } - //We have a piece and hasPiece animation has already run public Command elevatorOrArmIsMoving() { return new StartEndCommand( () -> { @@ -74,7 +69,6 @@ public Command elevatorOrArmIsMoving() { _leds); } - //Runs when robot has finished climbing public Command robotHasClimbed() { return new StartEndCommand( () -> { @@ -85,21 +79,15 @@ public Command robotHasClimbed() { }, _leds); } - - public Command disabledAnimation1() { + + public Command elevatorAndArmAtSetpoints() { return new StartEndCommand( () -> { - _leds.disabledAnimation1(); - }, + _leds.elevatorAndArmAtSetpoints(); + }, () -> { _leds.reset(); }, - _leds) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }; + _leds); } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index a1cf80d..4fcb7fd 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -1,9 +1,6 @@ package frc.robot.subsystems.multisubsystemcommands; -import java.util.function.Supplier; - import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.Arm.ArmPosition; @@ -15,7 +12,7 @@ public class MultiSubsystemCommands { public enum OverallPosition { Stow(ElevatorPosition.Stow, ArmPosition.Stow), - Loading(ElevatorPosition.Stow, ArmPosition.Loading_Coral), + Loading(ElevatorPosition.Stow, ArmPosition.Loading), L1(ElevatorPosition.L1, ArmPosition.Stow), L2(ElevatorPosition.L2, ArmPosition.Stow), L3(ElevatorPosition.L3, ArmPosition.Stow), @@ -39,11 +36,6 @@ ArmPosition getArmPosition() { } } - public enum GamepieceMode { - ALGAE, - CORAL; - } - private Elevator _elevator; private Arm _arm; private ElevatorCommands _elevatorCommands; @@ -60,16 +52,7 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva public Command setOverallSetpoint(OverallPosition setpoint) { return _elevatorCommands.setElevatorSetpoint(setpoint.getElevatorPosition()) .alongWith(_armCommands.setArmPosition(setpoint.getArmPosition())) - .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), setpoint.getElevatorPosition(), - _arm.getCurrentPos(), setpoint.getArmPosition())); - } - - public Command setGamepieceMode(GamepieceMode mode) { - return new InstantCommand( - () -> { - _elevator.setCurrentMode(mode); - _arm.setCurrentMode(mode); - }, _elevator, _arm); + .unless(() -> !_elevator.canMoveToPos(_elevator.getCurrentPos(), setpoint.getArmPosition())); } public Command waitForOverallMechanism() { @@ -89,10 +72,6 @@ private Command score(OverallPosition setpoint) { } } - public Command scoreGamepieceAtPosition(Supplier setpoint) { - return scoreGamepieceAtPosition(setpoint.get()); - } - public Command scoreGamepieceAtPosition(OverallPosition setpoint) { return new InstantCommand(); @@ -108,107 +87,11 @@ public Command scoreGamepieceAtPosition(OverallPosition setpoint) { .andThen(setOverallSetpoint(OverallPosition.Stow)); */ } - public Command loadGamepiece() { - return Commands.either(loadAlgae(), loadCoral(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); - } - - public Command loadCoral() { - return setOverallSetpoint(OverallPosition.Loading) - .andThen(waitForOverallMechanism()) - .andThen(_armCommands.intake()) - .andThen(setOverallSetpoint(OverallPosition.Stow)) - .andThen(_armCommands.moveGamepieceToLightSensor()) - .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.Stow, - _arm.getCurrentPos(), ArmPosition.Loading_Coral)); - - } - - public Command loadAlgae() { + public Command intake() { return setOverallSetpoint(OverallPosition.Loading) .andThen(waitForOverallMechanism()) .andThen(_armCommands.intake()) - .andThen(setOverallSetpoint(OverallPosition.Stow)) - .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.L2, - _arm.getCurrentPos(), ArmPosition.Loading_Algae)); - } - - public boolean canMoveToPos(ElevatorPosition currentElevator, ElevatorPosition desiredElevator, - ArmPosition currentArm, ArmPosition desiredArm) { - boolean canMoveArm = false; - boolean canMoveElevator = false; - - if (_arm.getCurrentMode() == GamepieceMode.CORAL) { - switch (currentElevator) { - case L1: - case L2: - case L3: - canMoveArm = (desiredArm != ArmPosition.L4_Score) && (desiredArm != ArmPosition.Loading_Coral); - break; - case L4: - canMoveArm = (desiredArm != ArmPosition.Loading_Coral); - break; - case Stow: - canMoveArm = (desiredArm != ArmPosition.L4_Score); - break; - default: - canMoveArm = false; - break; - } - - switch (desiredElevator) { - case L1: - case L2: - case L3: - canMoveElevator = (currentArm != ArmPosition.L4_Score) && (currentArm != ArmPosition.Loading_Coral); - break; - case L4: - canMoveElevator = (currentArm != ArmPosition.Loading_Coral); - break; - case Stow: - canMoveElevator = (currentArm != ArmPosition.L4_Score); - break; - default: - canMoveElevator = false; - break; - } - } else { - switch (currentElevator) { - case L1: - case L4: - canMoveArm = false; - break; - case L2: - case L3: - canMoveArm = desiredArm != ArmPosition.Algae_Score; - break; - case Stow: - canMoveArm = desiredArm != ArmPosition.Loading_Algae; - break; - default: - canMoveArm = false; - break; - } - - switch (desiredElevator) { - case L1: - case L4: - canMoveElevator = false; - break; - case L2: - case L3: - canMoveElevator = currentArm != ArmPosition.Algae_Score; - break; - case Stow: - canMoveElevator = currentArm != ArmPosition.Loading_Algae; - break; - default: - canMoveElevator = false; - break; - } - } - System.out.println("Arm: " + canMoveArm + " Elevator: " + canMoveElevator); - - return canMoveArm && canMoveElevator; + .andThen(setOverallSetpoint(OverallPosition.Stow)); } - -} \ No newline at end of file +} + \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/presets/Preset.java b/src/main/java/frc/robot/subsystems/presets/Preset.java index cf0a89f..9b4dac4 100644 --- a/src/main/java/frc/robot/subsystems/presets/Preset.java +++ b/src/main/java/frc/robot/subsystems/presets/Preset.java @@ -10,7 +10,7 @@ public enum ReefSide { Right; } - private OverallPosition _level = OverallPosition.L1; + private OverallPosition _level = OverallPosition.Stow; private ReefSide _side = ReefSide.Right; private boolean _isLevelValid = false; private boolean _isSideValid = false; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 271ee0f..88359a7 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -42,18 +42,21 @@ public class VisionConstants { // Camera 5 public static Transform3d robotToCamera5 = - new Transform3d(-13.75*0.0254, -11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + new Transform3d(-13.75*0.0254, -11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, -Math.PI/4, Math.PI)); // Camera 6 public static Transform3d robotToCamera6 = - new Transform3d(-13.75*0.0254, 11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 30.0/360.0*2.0*Math.PI, Math.PI)); + new Transform3d(-13.75*0.0254, 11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 0, Math.PI)); // Camera 7 + // Front right public static Transform3d robotToCamera7 = - new Transform3d(13.75*0.0254, -4*0.0254, 8.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + new Transform3d(14.125*0.0254, -8.5*0.0254, 7.75*0.0254, new Rotation3d(0.0, 0, 0)); + + // Front Left // Camera 8 public static Transform3d robotToCamera8 = - new Transform3d(13.75*0.0254, 4*0.0254, 8.5*0.0254, new Rotation3d(0.0, 20.0/360.0*2.0*Math.PI, 0)); + new Transform3d(14.125*0.0254, 8.5*0.0254, 7.75*0.0254, new Rotation3d(0.0, 0, 0)); // Basic filtering thresholds public static double maxAmbiguity = 0.3; From 5768fcdce196d2df592fc19283634748aae688fa Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 15:38:08 -0500 Subject: [PATCH 14/28] added some auto-align to reef tag --- .vscode/settings.json | 3 +- src/main/java/frc/robot/FieldConstants.java | 241 ++++++++++++++++++ .../java/frc/robot/HardwareConstants.java | 5 +- src/main/java/frc/robot/RobotContainer.java | 37 ++- .../robot/subsystems/drive/DriveCommands.java | 106 ++++++++ .../subsystems/drive/TunerConstants.java | 2 +- .../frc/robot/util/LoggedTunableNumber.java | 133 ++++++++++ .../frc/robot/util/TuneableProfiledPID.java | 51 ++++ 8 files changed, 574 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/FieldConstants.java create mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java create mode 100644 src/main/java/frc/robot/util/TuneableProfiledPID.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..3c0df25 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,241 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * Contains various field dimensions and useful reference points. All units are in meters and poses + * have a blue alliance origin. + */ +public class FieldConstants { + public static final double fieldLength = Units.inchesToMeters(690.876); + public static final double fieldWidth = Units.inchesToMeters(317); + public static final Translation2d fieldCenter = + new Translation2d(fieldLength / 2, fieldWidth / 2); + public static final double startingLineX = + Units.inchesToMeters(299.438); // Measured from the inside of starting + // line + + public static class Processor { + public static final Pose2d centerFace = + new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90)); + } + + public static class Barge { + public static final Translation2d farCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779)); + public static final Translation2d middleCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(242.855)); + public static final Translation2d closeCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); + + // Measured from floor to bottom of cage + public static final double deepHeight = Units.inchesToMeters(3.125); + public static final double shallowHeight = Units.inchesToMeters(30.125); + } + + public static class CoralStation { + public static final Pose2d leftCenterFace = + new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(291.176), + Rotation2d.fromDegrees(90 - 144.011)); + public static final Pose2d rightCenterFace = + new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824), + Rotation2d.fromDegrees(144.011 - 90)); + } + + public static class Reef { + public static final Translation2d centerOfReef = + new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); + public static final double faceToZoneLine = + Units.inchesToMeters(12); // Side of the reef to the inside of the + // reef zone line + + public static final Pose2d[] centerFaces = + new Pose2d[12]; // Starting facing the driver station in clockwise + // order + public static final List> branchPositions = + new ArrayList<>(); // Starting at the right + // branch facing the + // driver station in + // clockwise + + static { + // Initialize faces + centerFaces[0] = + new Pose2d( + Units.inchesToMeters(144.003), + Units.inchesToMeters(158.500), + Rotation2d.fromDegrees(180)); + centerFaces[1] = + new Pose2d( + Units.inchesToMeters(160.373), + Units.inchesToMeters(186.857), + Rotation2d.fromDegrees(120)); + centerFaces[2] = + new Pose2d( + Units.inchesToMeters(193.116), + Units.inchesToMeters(186.858), + Rotation2d.fromDegrees(60)); + centerFaces[3] = + new Pose2d( + Units.inchesToMeters(209.489), + Units.inchesToMeters(158.502), + Rotation2d.fromDegrees(0)); + centerFaces[4] = + new Pose2d( + Units.inchesToMeters(193.118), + Units.inchesToMeters(130.145), + Rotation2d.fromDegrees(-60)); + centerFaces[5] = + new Pose2d( + Units.inchesToMeters(160.375), + Units.inchesToMeters(130.144), + Rotation2d.fromDegrees(-120)); + + centerFaces[6] = centerFaces[0].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[7] = centerFaces[1].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[8] = centerFaces[2].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[9] = centerFaces[3].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[10] = centerFaces[4].rotateAround(fieldCenter, Rotation2d.k180deg); + centerFaces[11] = centerFaces[5].rotateAround(fieldCenter, Rotation2d.k180deg); + + // Initialize branch positions + for (int face = 0; face < centerFaces.length; face++) { + Map fillRight = new HashMap<>(); + Map fillLeft = new HashMap<>(); + for (var level : ReefHeight.values()) { + Pose2d poseDirection = new Pose2d(); + if (face < 6) { + poseDirection = + new Pose2d(centerOfReef, centerFaces[face].getRotation()); + } else { + poseDirection = + new Pose2d(centerOfReef.rotateAround(fieldCenter, Rotation2d.k180deg), + centerFaces[face].getRotation()); + } + + double adjustX = Units.inchesToMeters(30.738); // Depth of branch from reef face + double adjustY = Units.inchesToMeters(6.469); // Offset from reef face + // centerline to branch + + fillRight.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy( + new Transform2d(adjustX, adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustX, adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + fillLeft.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy( + new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + } + branchPositions.add(fillRight); + branchPositions.add(fillLeft); + } + + + } + } + + public static class StagingPositions { + // Measured from the center of the ice cream + public static final Pose2d leftIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d()); + public static final Pose2d middleIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); + public static final Pose2d rightIceCream = + new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); + } + + public enum ReefHeight { + L4(Units.inchesToMeters(72), -90), + L3(Units.inchesToMeters(47.625), -35), + L2(Units.inchesToMeters(31.875), -35), + L1(Units.inchesToMeters(18), 0); + + ReefHeight(double height, double pitch) + { + this.height = height; + this.pitch = pitch; // in degrees + } + + public final double height; + public final double pitch; + } + + public static Pose2d getNearestReefFace(Pose2d currentPose) + { + return currentPose.nearest(List.of(FieldConstants.Reef.centerFaces)); + } + + public enum ReefSide { + LEFT, + RIGHT + } + + public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side) + { + return FieldConstants.Reef.branchPositions + .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) + * 2 + (side == ReefSide.LEFT ? 1 : 0)) + .get(FieldConstants.ReefHeight.L1).toPose2d(); + } + + public static Pose2d getNearestCoralStation(Pose2d currentPose) + { + if (currentPose.getTranslation().getX() > FieldConstants.fieldLength / 2) { + if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { + return FieldConstants.CoralStation.rightCenterFace + .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); + } else { + return FieldConstants.CoralStation.leftCenterFace + .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); + } + } else { + if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { + return FieldConstants.CoralStation.leftCenterFace; + } else { + return FieldConstants.CoralStation.rightCenterFace; + } + } + } +} diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 2e2512f..8a37a03 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -4,7 +4,7 @@ public class HardwareConstants { - public static final Mode simMode = Mode.REPLAY; + public static final Mode simMode = Mode.SIM; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public static enum Mode { @@ -35,4 +35,7 @@ public class DIO { // Add digitial I/O ports used here public static final int LIGHT_SENSOR_CHANNEL = 0; } + + // Use LoggedTunableNumbers + public static final boolean tuningMode = true; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0da42f6..d1b69f6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.*; import java.lang.invoke.VarHandle.AccessMode; +import java.util.function.Supplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.pathplanner.lib.auto.NamedCommands; @@ -44,6 +45,8 @@ import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.FieldConstants.ReefSide; import static frc.robot.subsystems.vision.VisionConstants.*; @@ -90,6 +93,7 @@ public class RobotContainer { private final JoystickButton presetButton = new JoystickButton(buttonbox2, 2); // public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private double speedMultiplier = 0.9; private final Telemetry logger = new Telemetry(MaxSpeed); @@ -179,9 +183,18 @@ public RobotContainer() { NamedCommands.registerCommand("Intake", multiSubsystemCommands.intake()); + configureBindings(); } + private Command joystickApproach(Supplier approachPose) + { + return DriveCommands.joystickApproach( + drive, + () -> -joystick.getLeftY() * speedMultiplier, + approachPose); + } + private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. @@ -212,7 +225,26 @@ private void configureBindings() { joystick.start().and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); joystick.start().and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); - + // Driver Right Bumper: Approach Nearest Right-Side Reef Branch + joystick.rightBumper() + .whileTrue( + joystickApproach( + () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.RIGHT))); + + // Driver Left Bumper: Approach Nearest Left-Side Reef Branch + joystick.leftBumper() + .whileTrue( + joystickApproach( + () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); + + /* + // Driver Left Bumper and Algae mode: Approach Nearest Reef Face + joystick.rightBumper() + .whileTrue( + joystickApproach(() -> FieldConstants.getNearestReefFace(drive.getPose()))); + + */ + // Reset gyro to 0° when left bumper button is pressed joystick .leftBumper() @@ -241,9 +273,12 @@ private void configureBindings() { LoadingButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Loading)); L4_scoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); + // Runs the preset to score unless the preset is invalid. + /* joystick.rightBumper().onTrue( multiSubsystemCommands.scoreGamepieceAtPosition(preset.getLevel()).unless(() -> !preset.isPresetValid())); + */ // Resets the preset when we don't have a piece. armSubsystem._hasPiece.onFalse(preset.resetPreset()); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index 9d087cc..098cd7a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; @@ -35,6 +36,8 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import frc.robot.util.TuneableProfiledPID; +import org.littletonrobotics.junction.Logger; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -47,6 +50,14 @@ public class DriveCommands { private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + public enum DriveMode { + dmJoystick, + dmAngle, + dmApproach +} + +private static DriveMode currentDriveMode = DriveMode.dmJoystick; + private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { @@ -155,6 +166,101 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } + /** + * Robot relative drive command using joystick for linear control towards the approach target, + * PID for aligning with the target laterally, and PID for angular control. Used for approaching + * a known target, usually from a short distance. The approachSupplier must supply a Pose2d with + * a rotation facing away from the target + */ + public static Command joystickApproach( + Drive drive, + DoubleSupplier ySupplier, + Supplier approachSupplier) + { + + // Create PID controller + TuneableProfiledPID angleController = + new TuneableProfiledPID( + "angleController", + ANGLE_KP, + 0.0, + ANGLE_KD, + ANGLE_MAX_VELOCITY, + ANGLE_MAX_ACCELERATION); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + TuneableProfiledPID alignController = + new TuneableProfiledPID( + "alignController", + 0.3, + 0.0, + 0, + 20, + 8); + alignController.setGoal(0); + + // Construct command + return Commands.run( + () -> { + + //System.out.println("JOYSTICK APPROACH!!!"); + currentDriveMode = DriveMode.dmApproach; + // Name constants + Translation2d currentTranslation = drive.getPose().getTranslation(); + Translation2d approachTranslation = approachSupplier.get().getTranslation(); + double distanceToApproach = currentTranslation.getDistance(approachTranslation); + + Rotation2d alignmentDirection = approachSupplier.get().getRotation(); + + // Find lateral distance from goal + Translation2d goalTranslation = new Translation2d( + alignmentDirection.getCos() * distanceToApproach + approachTranslation.getX(), + alignmentDirection.getSin() * distanceToApproach + approachTranslation.getY()); + + Translation2d robotToGoal = currentTranslation.minus(goalTranslation); + double distanceToGoal = + Math.hypot(robotToGoal.getX(), robotToGoal.getY()); + + // Calculate lateral linear velocity + Translation2d offsetVector = + new Translation2d(alignController.calculate(distanceToGoal), 0) + .rotateBy(robotToGoal.getAngle()); + + Logger.recordOutput("AlignDebug/Current", distanceToGoal); + + // Calculate total linear velocity + Translation2d linearVelocity = + getLinearVelocityFromJoysticks(0, + ySupplier.getAsDouble()).rotateBy( + approachSupplier.get().getRotation()).rotateBy(Rotation2d.kCCW_90deg) + .plus(offsetVector); + + SmartDashboard.putData(alignController); // TODO: Calibrate PID + Logger.recordOutput("AlignDebug/approachTarget", approachTranslation); + + // Calculate angular speed + double omega = + angleController.calculate( + drive.getRotation().getRadians(), approachSupplier.get().getRotation() + .rotateBy(Rotation2d.k180deg).getRadians()); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + omega); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + drive.getRotation())); + }, + drive) + + // Reset PID controller when command starts + .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); + } + /** * Measures the velocity feedforward constants for the drive motors. * diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index c9905ed..b4b6325 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -28,7 +28,7 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(2.3).withKI(0).withKD(0) + .withKP(1.3).withKI(0).withKD(0) .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java new file mode 100644 index 0000000..5574778 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -0,0 +1,133 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import frc.robot.HardwareConstants; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +@SuppressWarnings("UnusedVariable") +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) + { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) + { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) + { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (HardwareConstants.tuningMode) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() + { + if (!hasDefault) { + return 0.0; + } else { + return HardwareConstants.tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) + { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) + { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept( + Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) + { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() + { + return get(); + } +} diff --git a/src/main/java/frc/robot/util/TuneableProfiledPID.java b/src/main/java/frc/robot/util/TuneableProfiledPID.java new file mode 100644 index 0000000..1972576 --- /dev/null +++ b/src/main/java/frc/robot/util/TuneableProfiledPID.java @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +/** Add your docs here. */ +public class TuneableProfiledPID extends ProfiledPIDController { + + // Instance name for tagging log values + String m_name; + + // Tunable numbers + private LoggedTunableNumber m_kP, m_kI, m_kD, m_maxV, m_maxA; + + public TuneableProfiledPID(String name, double kP, double kI, + double kD, double maxV, double maxA) + { + super(kP, kI, kD, new TrapezoidProfile.Constraints(maxV, maxA)); + + m_name = name; + + // Tunable numbers for PID and motion gain constants + m_kP = new LoggedTunableNumber(m_name + "/kP", kP); + m_kI = new LoggedTunableNumber(m_name + "/kI", kI); + m_kD = new LoggedTunableNumber(m_name + "/kD", kD); + + m_maxV = new LoggedTunableNumber(m_name + "/maxV", maxV); + m_maxA = new LoggedTunableNumber(m_name + "/maxA", maxA); + + } + + public void updatePID() + { + // If changed, update controller constants from Tuneable Numbers + if (m_kP.hasChanged(hashCode()) + || m_kI.hasChanged(hashCode()) + || m_kD.hasChanged(hashCode())) { + this.setPID(m_kP.get(), m_kI.get(), m_kD.get()); + } + + if (m_maxV.hasChanged(hashCode()) + || m_maxA.hasChanged(hashCode())) { + this.setConstraints(new TrapezoidProfile.Constraints(m_maxV.get(), m_maxA.get())); + } + } + +} From 51042921973baa57156481d9ec00be39915ed46a Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 15:39:50 -0500 Subject: [PATCH 15/28] added some auto-align stuff --- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1b69f6..b4f5ba0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -237,6 +237,8 @@ private void configureBindings() { joystickApproach( () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); + + /* // Driver Left Bumper and Algae mode: Approach Nearest Reef Face joystick.rightBumper() @@ -244,10 +246,9 @@ private void configureBindings() { joystickApproach(() -> FieldConstants.getNearestReefFace(drive.getPose()))); */ - - // Reset gyro to 0° when left bumper button is pressed - joystick - .leftBumper() + + // Reset gyro to 0° when Y button is pressed + joystick.y() .onTrue( Commands.runOnce( () -> From 3194649850122c741ff8adee86213ac30c56820a Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 15:59:16 -0500 Subject: [PATCH 16/28] tuning auto align and vision --- src/main/java/frc/robot/subsystems/vision/VisionConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 88359a7..435e0f6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -56,7 +56,7 @@ public class VisionConstants { // Front Left // Camera 8 public static Transform3d robotToCamera8 = - new Transform3d(14.125*0.0254, 8.5*0.0254, 7.75*0.0254, new Rotation3d(0.0, 0, 0)); + new Transform3d(14.125*0.0254, 11.5*0.0254, 8.25*0.0254, new Rotation3d(0, (-10/360.0)*2*Math.PI, 0)); // Basic filtering thresholds public static double maxAmbiguity = 0.3; From ef111d3baef06cda9fa5847b4ecf47be9a522236 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 16:01:05 -0500 Subject: [PATCH 17/28] swerve gain tuning --- src/main/java/frc/robot/subsystems/drive/TunerConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index b4b6325..c9905ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -28,7 +28,7 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(1.3).withKI(0).withKD(0) + .withKP(2.3).withKI(0).withKD(0) .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; From c8a6265fb27336db537cf24c47879156432c1f3e Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 18:59:55 -0500 Subject: [PATCH 18/28] updates and ability to sim --- simgui-ds.json | 5 ++ src/main/java/frc/robot/RobotContainer.java | 3 + ...rLib.json => PathplannerLib-2025.2.5.json} | 8 +- ...c2025-latest.json => Phoenix6-25.3.1.json} | 88 +++++++++++++------ vendordeps/REVLib.json | 10 +-- vendordeps/photonlib.json | 12 +-- 6 files changed, 82 insertions(+), 44 deletions(-) rename vendordeps/{PathplannerLib.json => PathplannerLib-2025.2.5.json} (87%) rename vendordeps/{Phoenix6-frc2025-latest.json => Phoenix6-25.3.1.json} (86%) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b4f5ba0..002f6cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -184,6 +185,8 @@ public RobotContainer() { multiSubsystemCommands.intake()); + // hide the joystick missing warnings + DriverStation.silenceJoystickConnectionWarning(true); configureBindings(); } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.2.5.json similarity index 87% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.2.5.json index 2021898..b2a7265 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.2.5.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.2.5.json", "name": "PathplannerLib", - "version": "2025.2.3", + "version": "2025.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.3" + "version": "2025.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.3", + "version": "2025.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-25.3.1.json similarity index 86% rename from vendordeps/Phoenix6-frc2025-latest.json rename to vendordeps/Phoenix6-25.3.1.json index acc78db..a216d97 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-25.3.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2025-latest.json", + "fileName": "Phoenix6-25.3.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", + "version": "25.3.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.3.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.3.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.3.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 7194603..ac62be8 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.2", + "version": "2025.0.3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.2" + "version": "2025.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 6af3d3e..1219919 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.1.1", + "version": "v2025.2.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", + "version": "v2025.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.1.1", + "version": "v2025.2.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", + "version": "v2025.2.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.1.1" + "version": "v2025.2.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.1.1" + "version": "v2025.2.1" } ] } \ No newline at end of file From 630cc683464061df2fbcee19e14e45924aab723d Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 19:01:45 -0500 Subject: [PATCH 19/28] updated vision constants --- .../robot/subsystems/vision/VisionConstants.java | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 435e0f6..e4ce08f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -20,6 +20,7 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; public class VisionConstants { // AprilTag layout @@ -42,21 +43,25 @@ public class VisionConstants { // Camera 5 public static Transform3d robotToCamera5 = - new Transform3d(-13.75*0.0254, -11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, -Math.PI/4, Math.PI)); + new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(-11.25), Units.inchesToMeters(9.25), + new Rotation3d(0.0, -Math.PI/4, Math.PI)); // Camera 6 public static Transform3d robotToCamera6 = - new Transform3d(-13.75*0.0254, 11.25*0.0254, 9.25*0.0254, new Rotation3d(0.0, 0, Math.PI)); + new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(11.25), Units.inchesToMeters(9.25), + new Rotation3d(0.0, 0, Math.PI)); // Camera 7 // Front right public static Transform3d robotToCamera7 = - new Transform3d(14.125*0.0254, -8.5*0.0254, 7.75*0.0254, new Rotation3d(0.0, 0, 0)); + new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(-8.5), Units.inchesToMeters(7.75), + new Rotation3d(0.0, 0, 0)); // Front Left // Camera 8 public static Transform3d robotToCamera8 = - new Transform3d(14.125*0.0254, 11.5*0.0254, 8.25*0.0254, new Rotation3d(0, (-10/360.0)*2*Math.PI, 0)); + new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(11.5), Units.inchesToMeters(8.25), + new Rotation3d(0, Units.degreesToRadians(-10), 0)); // Basic filtering thresholds public static double maxAmbiguity = 0.3; From 0c3d463eb960c08f409fbc51138b3fb2720d6f29 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sat, 8 Mar 2025 19:03:28 -0500 Subject: [PATCH 20/28] simulate changes --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 002f6cf..5545079 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,8 +134,6 @@ public RobotContainer() { vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera5Name, robotToCamera5), - new VisionIOPhotonVision(camera6Name, robotToCamera6), new VisionIOPhotonVision(camera7Name, robotToCamera7), new VisionIOPhotonVision(camera8Name, robotToCamera8) ); From 141e0ab908bfb674c0cd99f4a2d7245ecae2898f Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sun, 9 Mar 2025 14:30:06 -0400 Subject: [PATCH 21/28] building first autos --- src/main/deploy/pathplanner/paths/HP2_R4.path | 2 +- src/main/deploy/pathplanner/paths/R3_HP2.path | 4 ++-- src/main/deploy/pathplanner/settings.json | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 12 +++++++++++- 4 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index 23f58bd..548ac7c 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 3.2040620895951974, - "y": 0.27841135001952744 + "y": 0.2784113500195273 }, "isLocked": false, "linkedName": "HP2" diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 71ea67a..64efbd6 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -20,8 +20,8 @@ "y": 5.576493502516883 }, "prevControl": { - "x": 3.1619672723435244, - "y": 5.822659919233285 + "x": 3.161967272343525, + "y": 5.822659919233287 }, "nextControl": { "x": 2.589902912627433, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 46d6cb0..a02cd38 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -14,12 +14,12 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 50.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, - "maxDriveSpeed": 5.45, + "maxDriveSpeed": 3.0, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5545079..006238f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,10 @@ import java.lang.invoke.VarHandle.AccessMode; import java.util.function.Supplier; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -17,6 +20,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -93,6 +97,8 @@ public class RobotContainer { private final GenericHID buttonbox2 = new GenericHID(2); private final JoystickButton presetButton = new JoystickButton(buttonbox2, 2); + private final LoggedDashboardChooser autoChooser; + // public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private double speedMultiplier = 0.9; @@ -183,6 +189,8 @@ public RobotContainer() { multiSubsystemCommands.intake()); + autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + // hide the joystick missing warnings DriverStation.silenceJoystickConnectionWarning(true); configureBindings(); @@ -292,7 +300,9 @@ private void configureBindings() { presetButton.and(L4Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L4)); } + public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return autoChooser.get(); } + } From 59c72137b9b9c9c6d7457803eda74e841b364cea Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sun, 9 Mar 2025 14:45:35 -0400 Subject: [PATCH 22/28] basic paths added --- src/main/deploy/pathplanner/settings.json | 22 +--------------------- 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a02cd38..32dbc55 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -13,25 +13,5 @@ "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "defaultNominalVoltage": 12.0, - "robotMass": 50.0, - "robotMOI": 6.883, - "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, - "maxDriveSpeed": 3.0, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, - "bumperOffsetX": 0.0, - "bumperOffsetY": 0.0, - "robotFeatures": [] + "defaultNominalVoltage": 12.0 } \ No newline at end of file From eea8596d6741ea7114ff42f904772e25c1356882 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sun, 9 Mar 2025 15:17:46 -0400 Subject: [PATCH 23/28] more autos --- src/main/deploy/pathplanner/autos/A_R3.auto | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/A_R3.auto diff --git a/src/main/deploy/pathplanner/autos/A_R3.auto b/src/main/deploy/pathplanner/autos/A_R3.auto new file mode 100644 index 0000000..a2d8abf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A_R3.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file From 9d22504f1c2ce36f7fac8f9f0b932398ca2998df Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sun, 9 Mar 2025 15:31:14 -0400 Subject: [PATCH 24/28] auto init stuff --- src/main/deploy/pathplanner/settings.json | 22 +++++- src/main/java/frc/robot/Robot.java | 75 +++++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 4 ++ 3 files changed, 100 insertions(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 32dbc55..96a7b66 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -13,5 +13,25 @@ "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "defaultNominalVoltage": 12.0 + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.0486, + "driveGearing": 5.143, + "maxDriveSpeed": 3.929331883993818, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7224c30..d7778d8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,10 @@ package frc.robot; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -11,14 +15,36 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.HardwareConstants.CAN; +import java.io.IOException; +import org.json.simple.parser.ParseException; + public class Robot extends LoggedRobot { private Command m_autonomousCommand; + private Command m_lastAutonomousCommand; + private List m_pathsToShow = new ArrayList(); + private Field2d m_autoTraj = new Field2d(); + public static final double fieldLength = Units.inchesToMeters(690.876); + public static final double fieldWidth = Units.inchesToMeters(317); + public static final Translation2d fieldCenter = + new Translation2d(fieldLength / 2, fieldWidth / 2); private final RobotContainer m_robotContainer; @@ -63,6 +89,54 @@ public void disabledInit() { @Override public void disabledPeriodic() { + { + var m_alliance = DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + // Get currently selected command + + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + // Check if is the same as the last one + if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) { + // Check if its contained in the list of our autos + if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) { + // Clear the current path + m_pathsToShow.clear(); + // Grabs all paths from the auto + try { + for (PathPlannerPath path : PathPlannerAuto + .getPathGroupFromAutoFile(m_autonomousCommand.getName())) { + // Adds all poses to master list + m_pathsToShow.addAll(path.getPathPoses()); + } + } catch (IOException | ParseException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + // Check to see which alliance we are on Red Alliance + if (m_alliance) { + for (int i = 0; i < m_pathsToShow.size(); i++) { + m_pathsToShow.set(i, + m_pathsToShow.get(i).rotateAround(fieldCenter, Rotation2d.k180deg)); + } + } + // Displays all poses on Field2d widget + m_autoTraj.getObject("traj").setPoses(m_pathsToShow); + } + } + m_lastAutonomousCommand = m_autonomousCommand; + + if (!m_pathsToShow.isEmpty()) { + var firstPose = m_pathsToShow.get(0); + Logger.recordOutput("Alignment/StartPose", firstPose); + SmartDashboard.putBoolean("Alignment/Translation", + firstPose.getTranslation().getDistance( + m_robotContainer.getDrive().getPose().getTranslation()) <= Units + .inchesToMeters(1.5)); + SmartDashboard.putBoolean("Alignment/Rotation", + firstPose.getRotation().minus(m_robotContainer.getDrive().getPose().getRotation()) + .getDegrees() < 1); + } + } } @Override @@ -73,6 +147,7 @@ public void disabledExit() { public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + System.out.println("!!!!!!!!!!Autonomous init!!!!!!!!!!!"); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 006238f..fb0dcf5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -304,5 +304,9 @@ private void configureBindings() { public Command getAutonomousCommand() { return autoChooser.get(); } + + public Drive getDrive() { + return drive; + } } From 1fb37974ecdebd4d075dcd02c24de68ddaab006d Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sun, 9 Mar 2025 16:24:59 -0400 Subject: [PATCH 25/28] debugging paths --- .../deploy/pathplanner/autos/3Piece-A.auto | 73 ------------------- src/main/deploy/pathplanner/autos/HP2_R6.auto | 19 +++++ .../deploy/pathplanner/autos/Leave A.auto | 19 +++++ src/main/deploy/pathplanner/paths/A_R1.path | 4 +- src/main/deploy/pathplanner/paths/A_R2.path | 4 +- src/main/deploy/pathplanner/paths/A_R3.path | 4 +- src/main/deploy/pathplanner/paths/A_R4.path | 4 +- src/main/deploy/pathplanner/paths/B_R2.path | 4 +- src/main/deploy/pathplanner/paths/B_R3.path | 4 +- src/main/deploy/pathplanner/paths/B_R4.path | 4 +- src/main/deploy/pathplanner/paths/B_R5.path | 4 +- src/main/deploy/pathplanner/paths/C_R3.path | 4 +- src/main/deploy/pathplanner/paths/C_R4.path | 4 +- src/main/deploy/pathplanner/paths/C_R5.path | 4 +- src/main/deploy/pathplanner/paths/C_R6.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R1.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R2.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R3.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R4.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R5.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R6.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R1.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R2.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R3.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R4.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R5.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R6.path | 4 +- src/main/deploy/pathplanner/paths/LeaveA.path | 4 +- src/main/deploy/pathplanner/paths/LeaveB.path | 4 +- src/main/deploy/pathplanner/paths/LeaveC.path | 4 +- src/main/deploy/pathplanner/paths/R1_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R1_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R2_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R2_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R3_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R3_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R4_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R4_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R5_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R5_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R6_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R6_HP2.path | 4 +- src/main/deploy/pathplanner/settings.json | 6 +- src/main/java/frc/robot/Robot.java | 3 + src/main/java/frc/robot/RobotContainer.java | 16 +++- 45 files changed, 135 insertions(+), 157 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/3Piece-A.auto create mode 100644 src/main/deploy/pathplanner/autos/HP2_R6.auto create mode 100644 src/main/deploy/pathplanner/autos/Leave A.auto diff --git a/src/main/deploy/pathplanner/autos/3Piece-A.auto b/src/main/deploy/pathplanner/autos/3Piece-A.auto deleted file mode 100644 index cc12487..0000000 --- a/src/main/deploy/pathplanner/autos/3Piece-A.auto +++ /dev/null @@ -1,73 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R1" - } - }, - { - "type": "named", - "data": { - "name": "L1" - } - }, - { - "type": "path", - "data": { - "pathName": "R1_HP1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "HP1_R2" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "path", - "data": { - "pathName": "R2_HP1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "HP1_R3" - } - }, - { - "type": "named", - "data": { - "name": "L1" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HP2_R6.auto b/src/main/deploy/pathplanner/autos/HP2_R6.auto new file mode 100644 index 0000000..c9b3428 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HP2_R6.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP2_R6" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Leave A.auto b/src/main/deploy/pathplanner/autos/Leave A.auto new file mode 100644 index 0000000..d8b7a51 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Leave A.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeaveA" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R1.path b/src/main/deploy/pathplanner/paths/A_R1.path index c2b9654..5c70f0c 100644 --- a/src/main/deploy/pathplanner/paths/A_R1.path +++ b/src/main/deploy/pathplanner/paths/A_R1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/A_R2.path b/src/main/deploy/pathplanner/paths/A_R2.path index 62b3951..7158c61 100644 --- a/src/main/deploy/pathplanner/paths/A_R2.path +++ b/src/main/deploy/pathplanner/paths/A_R2.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index 93317dd..b26aaf7 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/A_R4.path b/src/main/deploy/pathplanner/paths/A_R4.path index 53feb30..2223976 100644 --- a/src/main/deploy/pathplanner/paths/A_R4.path +++ b/src/main/deploy/pathplanner/paths/A_R4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R2.path b/src/main/deploy/pathplanner/paths/B_R2.path index a0ac52b..af8c051 100644 --- a/src/main/deploy/pathplanner/paths/B_R2.path +++ b/src/main/deploy/pathplanner/paths/B_R2.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index 2eb1129..d2ef462 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R4.path b/src/main/deploy/pathplanner/paths/B_R4.path index b7abbf9..20ad7df 100644 --- a/src/main/deploy/pathplanner/paths/B_R4.path +++ b/src/main/deploy/pathplanner/paths/B_R4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index dd7a13f..f365ee3 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 4786955..929ad91 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index 69bb49c..9e99db2 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 5a6d9cc..9f7d064 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index cbd1143..0ddaf9f 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index f362011..c021235 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index 147af5b..db40e5e 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index a08014b..57bc2e2 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 860a822..7cc7298 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index 97afd75..5fed557 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index 3522805..6d2c7f2 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index 14932d3..fb7f29c 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index 2c2baee..d7a6e54 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index 488b3a0..fcb9a8d 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index 548ac7c..a69efb3 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index 6305e1b..5049ec3 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index c0ad666..09a277b 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveA.path b/src/main/deploy/pathplanner/paths/LeaveA.path index 635629b..c7734a2 100644 --- a/src/main/deploy/pathplanner/paths/LeaveA.path +++ b/src/main/deploy/pathplanner/paths/LeaveA.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveB.path b/src/main/deploy/pathplanner/paths/LeaveB.path index e958cd2..7d78c5e 100644 --- a/src/main/deploy/pathplanner/paths/LeaveB.path +++ b/src/main/deploy/pathplanner/paths/LeaveB.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveC.path b/src/main/deploy/pathplanner/paths/LeaveC.path index 13521e5..a8087e8 100644 --- a/src/main/deploy/pathplanner/paths/LeaveC.path +++ b/src/main/deploy/pathplanner/paths/LeaveC.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index 243f4d7..8fbde00 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index 56f97de..12bb796 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 295b6b3..62330ad 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index 5906654..6894598 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 53a657b..8914163 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 64efbd6..81b8762 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index 0add02c..0698cc1 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index a9ce710..f362e42 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index ad4ec15..fc9723f 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index 58dcf8c..e9bb79b 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index 338a258..766067b 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index 11ee9c4..ef2b4c6 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 96a7b66..ba8fb50 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,8 +9,8 @@ "Start to Reef" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 3.5, + "defaultMaxAccel": 3.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, @@ -19,7 +19,7 @@ "robotTrackwidth": 0.546, "driveWheelRadius": 0.0486, "driveGearing": 5.143, - "maxDriveSpeed": 3.929331883993818, + "maxDriveSpeed": 6.0, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d7778d8..a7d55b3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -146,6 +146,8 @@ public void disabledExit() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + System.out.println(m_autonomousCommand); + System.out.println("!!!!!!!!!!Autonomous init!!!!!!!!!!!"); if (m_autonomousCommand != null) { @@ -155,6 +157,7 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { + } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb0dcf5..c101472 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -137,12 +137,22 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); + /* We should be using this VisionIOPhotonVisionSim here but it's running too slow and + causing the a loop overrun. vision = new Vision( drive::addVisionMeasurement, - new VisionIOPhotonVision(camera7Name, robotToCamera7), - new VisionIOPhotonVision(camera8Name, robotToCamera8) - ); + new VisionIOPhotonVisionSim(camera7Name, robotToCamera7, drive::getPose), + new VisionIOPhotonVisionSim(camera8Name, robotToCamera8, drive::getPose) + ); */ + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}); + break; default: From 97f401c378813703f27091d7a00cc76d449a18ef Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Sun, 9 Mar 2025 19:16:54 -0400 Subject: [PATCH 26/28] enable desktop support per wpilib docs --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 369011a..ed9b99f 100644 --- a/build.gradle +++ b/build.gradle @@ -47,7 +47,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. From 22a4879e57ec66273f9b5f4d837277a4a85cd1be Mon Sep 17 00:00:00 2001 From: Garrett Hart Date: Sun, 9 Mar 2025 19:17:24 -0400 Subject: [PATCH 27/28] fix path planner issue --- src/main/java/frc/robot/subsystems/drive/TunerConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index c9905ed..5538d02 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -72,8 +72,8 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21*0.7); - + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21); + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; From 9077b4d862b19423e2f768d65f6c628741119e54 Mon Sep 17 00:00:00 2001 From: "Henthorn Lab @ RHIT" Date: Sun, 9 Mar 2025 20:57:30 -0400 Subject: [PATCH 28/28] path planner working with sim --- build.gradle | 2 +- .../deploy/pathplanner/autos/TestAuto.auto | 19 +++++ src/main/deploy/pathplanner/paths/A_R1.path | 4 +- src/main/deploy/pathplanner/paths/A_R2.path | 4 +- src/main/deploy/pathplanner/paths/A_R3.path | 4 +- src/main/deploy/pathplanner/paths/A_R4.path | 4 +- src/main/deploy/pathplanner/paths/B_R2.path | 4 +- src/main/deploy/pathplanner/paths/B_R3.path | 4 +- src/main/deploy/pathplanner/paths/B_R4.path | 4 +- src/main/deploy/pathplanner/paths/B_R5.path | 4 +- src/main/deploy/pathplanner/paths/C_R3.path | 4 +- src/main/deploy/pathplanner/paths/C_R4.path | 4 +- src/main/deploy/pathplanner/paths/C_R5.path | 4 +- src/main/deploy/pathplanner/paths/C_R6.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R1.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R2.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R3.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R4.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R5.path | 4 +- src/main/deploy/pathplanner/paths/HP1_R6.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R1.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R2.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R3.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R4.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R5.path | 4 +- src/main/deploy/pathplanner/paths/HP2_R6.path | 4 +- src/main/deploy/pathplanner/paths/LeaveA.path | 4 +- src/main/deploy/pathplanner/paths/LeaveB.path | 4 +- src/main/deploy/pathplanner/paths/LeaveC.path | 4 +- src/main/deploy/pathplanner/paths/R1_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R1_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R2_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R2_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R3_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R3_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R4_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R4_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R5_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R5_HP2.path | 4 +- src/main/deploy/pathplanner/paths/R6_HP1.path | 4 +- src/main/deploy/pathplanner/paths/R6_HP2.path | 4 +- .../deploy/pathplanner/paths/TestPath.path | 54 +++++++++++++ src/main/deploy/pathplanner/settings.json | 15 ++-- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 12 +-- .../frc/robot/subsystems/drive/Drive.java | 80 ++++++++++++++----- 46 files changed, 226 insertions(+), 118 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/TestAuto.auto create mode 100644 src/main/deploy/pathplanner/paths/TestPath.path diff --git a/build.gradle b/build.gradle index 369011a..ad44e3c 100644 --- a/build.gradle +++ b/build.gradle @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto new file mode 100644 index 0000000..ba05359 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TestAuto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TestPath" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R1.path b/src/main/deploy/pathplanner/paths/A_R1.path index 5c70f0c..b13a00e 100644 --- a/src/main/deploy/pathplanner/paths/A_R1.path +++ b/src/main/deploy/pathplanner/paths/A_R1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/A_R2.path b/src/main/deploy/pathplanner/paths/A_R2.path index 7158c61..138c08e 100644 --- a/src/main/deploy/pathplanner/paths/A_R2.path +++ b/src/main/deploy/pathplanner/paths/A_R2.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index b26aaf7..3e052d8 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/A_R4.path b/src/main/deploy/pathplanner/paths/A_R4.path index 2223976..37e0d52 100644 --- a/src/main/deploy/pathplanner/paths/A_R4.path +++ b/src/main/deploy/pathplanner/paths/A_R4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R2.path b/src/main/deploy/pathplanner/paths/B_R2.path index af8c051..1a9d6ff 100644 --- a/src/main/deploy/pathplanner/paths/B_R2.path +++ b/src/main/deploy/pathplanner/paths/B_R2.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index d2ef462..71b925e 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R4.path b/src/main/deploy/pathplanner/paths/B_R4.path index 20ad7df..4510ba8 100644 --- a/src/main/deploy/pathplanner/paths/B_R4.path +++ b/src/main/deploy/pathplanner/paths/B_R4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index f365ee3..95defe6 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 929ad91..4e34e79 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index 9e99db2..01c8c00 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 9f7d064..0f821b9 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index 0ddaf9f..3152363 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index c021235..d9f60e1 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index db40e5e..813b803 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index 57bc2e2..cf08bd7 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 7cc7298..2ecfc54 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index 5fed557..8c0f456 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index 6d2c7f2..6dc5c85 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index fb7f29c..b6c7542 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index d7a6e54..f06c78a 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index fcb9a8d..228d14e 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index a69efb3..5fa6fb1 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index 5049ec3..09ca8d4 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 09a277b..9c5d000 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveA.path b/src/main/deploy/pathplanner/paths/LeaveA.path index c7734a2..99efd9f 100644 --- a/src/main/deploy/pathplanner/paths/LeaveA.path +++ b/src/main/deploy/pathplanner/paths/LeaveA.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveB.path b/src/main/deploy/pathplanner/paths/LeaveB.path index 7d78c5e..eed1340 100644 --- a/src/main/deploy/pathplanner/paths/LeaveB.path +++ b/src/main/deploy/pathplanner/paths/LeaveB.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveC.path b/src/main/deploy/pathplanner/paths/LeaveC.path index a8087e8..17b95ca 100644 --- a/src/main/deploy/pathplanner/paths/LeaveC.path +++ b/src/main/deploy/pathplanner/paths/LeaveC.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index 8fbde00..aa38231 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index 12bb796..69151dd 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 62330ad..411551d 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index 6894598..9226a7d 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 8914163..3e64fc9 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 81b8762..7c49a68 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index 0698cc1..29982f6 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index f362e42..d6df8ec 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index fc9723f..8fd7314 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -70,8 +70,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index e9bb79b..c3d055a 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index 766067b..ecb0299 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index ef2b4c6..e1140cc 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -54,8 +54,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 5.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/TestPath.path b/src/main/deploy/pathplanner/paths/TestPath.path new file mode 100644 index 0000000..42d6faf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TestPath.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Test", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index ba8fb50..566711c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,21 +5,22 @@ "pathFolders": [ "HP to Reef", "Leave Start", + "Test", "Reef to HP", "Start to Reef" ], "autoFolders": [], - "defaultMaxVel": 3.5, - "defaultMaxAccel": 3.5, + "defaultMaxVel": 5.0, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, - "robotMOI": 6.883, + "robotMass": 50.0, + "robotMOI": 5.1, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.0486, - "driveGearing": 5.143, - "maxDriveSpeed": 6.0, + "driveWheelRadius": 0.049, + "driveGearing": 6.122, + "maxDriveSpeed": 5.0, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a7d55b3..44c6726 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -81,6 +81,7 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + } @Override @@ -151,14 +152,13 @@ public void autonomousInit() { System.out.println("!!!!!!!!!!Autonomous init!!!!!!!!!!!"); if (m_autonomousCommand != null) { + System.out.println("FOUND A COMMAND"); m_autonomousCommand.schedule(); } } @Override - public void autonomousPeriodic() { - - } + public void autonomousPeriodic() {} @Override public void autonomousExit() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c101472..6a470a5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -218,20 +218,14 @@ private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. + drive.setDefaultCommand( DriveCommands.joystickDrive( drive, () -> -joystick.getLeftY(), () -> -joystick.getLeftX(), - () -> -joystick.getRightX())); - - // drivetrain.setDefaultCommand( - // // Drivetrain will execute this command periodically - // drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with - // // negative Y (forward) - // .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) - // .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) - // )); + () -> -joystick.getRightX())); + // joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); // joystick.b().whileTrue(drivetrain diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5270b06..9f7b52d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -74,23 +74,12 @@ public class Drive extends SubsystemBase { Math.hypot(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY))); - // PathPlanner config constants - private static final double ROBOT_MASS_KG = 74.088; - private static final double ROBOT_MOI = 6.883; - private static final double WHEEL_COF = 1.2; - private static final RobotConfig PP_CONFIG = - new RobotConfig( - ROBOT_MASS_KG, - ROBOT_MOI, - new ModuleConfig( - TunerConstants.FrontLeft.WheelRadius, - TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), - WHEEL_COF, - DCMotor.getKrakenX60Foc(1) - .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), - TunerConstants.FrontLeft.SlipCurrent, - 1), - getModuleTranslations()); + // PathPlanner config constants to use if we can't read in the path planner json file + private static final double ROBOT_MASS_KG = 50.0; + private static final double ROBOT_MOI = 5.10; + private static final double WHEEL_COF = 1.13; + + static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -130,6 +119,53 @@ public Drive( // Start odometry thread PhoenixOdometryThread.getInstance().start(); + + RobotConfig config; + try{ + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + config = new RobotConfig( + ROBOT_MASS_KG, + ROBOT_MOI, + new ModuleConfig( + TunerConstants.FrontLeft.WheelRadius, + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), + WHEEL_COF, + DCMotor.getKrakenX60Foc(1) + .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), + TunerConstants.FrontLeft.SlipCurrent, + 1), + getModuleTranslations()); + } + + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + + /* // Configure AutoBuilder for PathPlanner AutoBuilder.configure( this::getPose, @@ -137,16 +173,19 @@ public Drive( this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - PP_CONFIG, + new PIDConstants(3, 0.0, 0.0), new PIDConstants(5.5, 0.0, 0.0)), + PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); + this); */ + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback( (activePath) -> { Logger.recordOutput( "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); }); + PathPlannerLogging.setLogTargetPoseCallback( (targetPose) -> { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); @@ -229,6 +268,7 @@ public void periodic() { * @param speeds Speeds in meters/sec */ public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);