From 714a9e9e65d86e561a5294a66afd167f533fdd56 Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 1 Jul 2025 20:07:32 -0400 Subject: [PATCH 01/11] Started adding vision --- build.gradle | 2 +- gradlew | 0 .../frc/robot/constants/VisionConstants.java | 6 +++ .../subsystems/vision/WalleyeSubsystem.java | 49 +++++++++++++++++++ 4 files changed, 56 insertions(+), 1 deletion(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/frc/robot/constants/VisionConstants.java create mode 100644 src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java diff --git a/build.gradle b/build.gradle index 1f571dc..855dbc7 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.diffplug.spotless" version "6.12.1" id "com.peterabeles.gversion" version "1.10" } diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java new file mode 100644 index 0000000..56e9adf --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class VisionConstants { + public static int kNumCams = 1; + +} diff --git a/src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java b/src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java new file mode 100644 index 0000000..3aa4e67 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems.vision; + +import WallEye.*; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.math.geometry.Rotation3d; +import java.util.Set; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; +import frc.robot.constants.VisionConstants; +import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; + + +public class WalleyeSubsystem extends MeasurableSubsystem{ + + private WallEyeCam[] cams; + private Translation3d[] camPositions; + private double[] camZs; + private Rotation3d[] camRotations; + private String[] camNames; + private int[] camIndexs; + private UdpSubscriber[] udpSubscribers; + + private Logger logger; + private LoggerFactory textLogger; + private CircularBuffer turrentBuffer = new CircularBuffer<>(1000); + private AprilTagFieldLayout field; + + public WalleyeSubsystem(){ + // field = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025Reefscape.m_resourceFile); + cams = new WallEyeCam[VisionConstants.kNumCams]; + + for(int i = 0; i < VisionConstants.kNumCams; i++){ + cams[i] = new WallEyeCam(camNames[i], -1); + + } + } + + +@Override +public Set getMeasures() { + return null; +} +} \ No newline at end of file From ccb81c177ab677b00adf25cdc920be29804385fc Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 8 Jul 2025 18:48:13 -0400 Subject: [PATCH 02/11] Added vision system from comp bot --- .../frc/robot/constants/VisionConstants.java | 130 ++++- .../subsystems/turret/TurretSubsystem.java | 21 + .../subsystems/vision/VisionSubsystem.java | 514 ++++++++++++++++++ .../subsystems/vision/WalleyeSubsystem.java | 49 -- 4 files changed, 662 insertions(+), 52 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 56e9adf..697c1ae 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -1,6 +1,130 @@ package frc.robot.constants; -public class VisionConstants { - public static int kNumCams = 1; - +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import net.jafama.FastMath; + +public final class VisionConstants { + public static final double kMaxTimeNoVision = 20; + public static final double kTimeToResetWheelCount = 5; + public static final int kResultsForWheels = 5; + public static final double kTimeToDecayDev = 10; + public static final double kStdDevDecayCoeff = -0.005; + public static final double kMinStdDev = 0.01; + public static final double kMaxAmbig = 1.0; + public static final int kMaxTimesOffWheels = 5; + public static final double kBumperPixelLine = 87; // 100 + public static final double kRobotHeight = 0.5; + + // public static final double kThetaStdDevUsed = Units.degreesToRadians(0.02); + // public static final double kThetaStdDevRejected = Units.degreesToRadians(360); + // public static final double kThetaStdThres = 0.2; + + // Velocity Filter + public static final double kLinearCoeffOnVelFilter = 0.1; + public static final double kOffsetOnVelFilter = 0.10; + public static final double kSquaredCoeffOnVelFilter = 0.1; + + public static Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.0); + + public static final double kTimeStampOffset = 0.0; + + // StdDev scaling + public static final double singleTagCoeff = 25.0 / 100.0; + public static final double multiTagCoeff = 18.0 / 100.0; + public static final double baseNumber = Math.E; + public static final double powerNumber = 2.0; + public static final double baseTrust = 3.0; + + public static final double FOV45MultiTagCoeff = 16.0 / 100.0; + public static final double FOV45powerNumber = 4.5; + public static final double FOV45SingleTagCoeff = 21.0 / 100.0; + public static final double FOV45BaseTrust = 2.0; + + public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0; + public static final double FOV58MJPGPowerNumber = 3.5; + public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0; + public static final double FOV58MJPGBaseTrust = 3.0; + + public static final double FOV58YUYVMultiTagCoeff = 17.0 / 100.0; + public static final double FOV58YUYVPowerNumber = 2.0; + public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV58YUYVBaseTrust = 3.0; + + public static final double FOV75YUYVMultiTagCoeff = 17.0 / 100.0; + public static final double FOV75YUYVPowerNumber = 2.0; + public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV75YUYVBaseTrust = 3.0; + + // Gyro error + public static final double kYawErrorThreshold = Units.degreesToRadians(30); + public static final double kCamErrorZThreshold = 0.3; + + // Constants for cameras + public static final int kNumCams = 5; + public static final int kNumPis = 3; + public static final int[] kUdpIndex = {0, 1, 2}; + + // Camera Ports + public static final int[] kCamPorts = {5802, 5804, 5803, 5804, 5804}; + + // Names + public static final String kCam1Name = "Left Servo"; + public static final String kCam2Name = "Rear Right"; // when looking out the back + public static final String kCam3Name = "Right Servo"; + public static final String kCam4Name = "Rear Left"; + public static final String kCam5Name = "Rear"; + + // public static final String kPi1Name = "Left"; + // public static final String kPi2Name = "Right"; + // public static final String kPi3Name = "Rear"; + + // Indexs + // public static final int kCam1Idx = 0; + // public static final int kCam2Idx = 0; + // public static final int kCam3Idx = 0; + // public static final int kCam4Idx = 0; + // public static final int kCam5Idx = 2; + + public static final double kLoopTime = 0.02; + public static final int kCircularBufferSize = 1000; + // Poses + public static final Pose3d kCam1Pose = + new Pose3d(new Translation3d(0.305, 0.0255, 0.311), new Rotation3d()); + public static final Pose3d kCam2Pose = + new Pose3d( + new Translation3d(-0.275, -0.21, 0.49), + new Rotation3d( + Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(145))); + public static final Pose3d kCam3Pose = + new Pose3d( + new Translation3d(0.133, -0.3, 0.311), // -0.305 + new Rotation3d(0, Units.degreesToRadians(-2), Units.degreesToRadians(1))); + public static final Pose3d kCam4Pose = + new Pose3d( + new Translation3d(-0.275, -0.145, 0.49), + new Rotation3d( + Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(-145))); + public static final Pose3d kCam5Pose = + new Pose3d( + new Translation3d(-0.229, 0.073, 0.934), + new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(180))); + // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is + // in the form [theta], with units in radians. + public static final Matrix kLocalMeasurementStdDevs = + VecBuilder.fill(Units.degreesToRadians(0.01)); + + public static final double kIgnoreYawStdDev = 10000 * FastMath.PI; + public static final double kTrustYawStdDev = 0.0005; + // Increase these numbers to trust global measurements from vision less. This matrix is in the + // form [x, y, theta]ᵀ, with units in meters and radians. + // Vision Odometry Standard devs + public static final Matrix kVisionMeasurementStdDevs = + VecBuilder.fill(0.05, 0.05, kIgnoreYawStdDev); } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java new file mode 100644 index 0000000..3a114a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.turret; + +import edu.wpi.first.math.geometry.Pose2d; +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.math.Matrix; + +public class TurretSubsystem { + public TurretSubsystem(){ + + } +//Stubbed functions until the turret subsystem is done. +public Rotation2d getGyroRotation2d(){ + return null; +} +public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDev ){ + +} + +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java new file mode 100644 index 0000000..580593b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -0,0 +1,514 @@ +package frc.robot.subsystems.vision; + +import static edu.wpi.first.units.Units.*; + +import WallEye.*; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +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.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.VisionConstants; +//import frc.robot.subsystems.drive.DriveSubsystem; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Set; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; +import frc.robot.subsystems.turret.TurretSubsystem; + +public class VisionSubsystem extends MeasurableSubsystem { + + // Array of cameras + private WallEyeCam[] cams; + + // Array of camera positions + private Translation3d[] camPositions = { + VisionConstants.kCam1Pose.getTranslation(), + VisionConstants.kCam2Pose.getTranslation(), + VisionConstants.kCam3Pose.getTranslation(), + }; + + private double[] camHeights = { + camPositions[0].getZ(), + camPositions[1].getZ(), + camPositions[2].getZ(), + }; + + // Array of camera rotations + private Rotation3d[] camRotations = { + VisionConstants.kCam1Pose.getRotation(), + VisionConstants.kCam2Pose.getRotation(), + VisionConstants.kCam3Pose.getRotation(), + }; + + // Array of camera names + private String[] camNames = { + VisionConstants.kCam1Name, + VisionConstants.kCam2Name, + VisionConstants.kCam3Name, + }; + + private int trustedCameraYawIdx = -1; + + //private DriveSubsystem driveSubsystem; + /*Because we use two seperate loggers we can import one and then define the + other here.*/ + private org.slf4j.Logger textLogger; + private UdpSubscriber[] udpSubscriber; + private AprilTagFieldLayout field; + private boolean visionUpdating = true; + private int minTags; + private CircularBuffer gyroBuffer = + new CircularBuffer(VisionConstants.kCircularBufferSize); + private double timeSinceLastUpdate; + private int updatesToWheels; + private Matrix adaptiveMatrix; + private ArrayList> validResults = new ArrayList<>(); + private WallEyeTagResult[] lastResult = new WallEyeTagResult[VisionConstants.kNumCams]; + private Matrix adativeMatrix; + private Matrix stdMatrix; + private TurretSubsystem turretSubsystem; + private boolean[] acceptUpdates = new boolean[VisionConstants.kNumCams]; + private boolean ignoreRearCams = false; + private boolean isAuto = false; + + public VisionSubsystem(TurretSubsystem turretSubsystem) { + this.turretSubsystem = turretSubsystem; + //this.driveSubsystem = driveSubsystem; + textLogger = LoggerFactory.getLogger("Vision"); + + cams = new WallEyeCam[VisionConstants.kNumCams]; + udpSubscriber = new UdpSubscriber[VisionConstants.kNumPis]; + // We copy the matrix from our constants + adaptiveMatrix = VisionConstants.kVisionMeasurementStdDevs.copy(); + // We then copy the adaptive matrix because they will differ later + stdMatrix = adaptiveMatrix.copy(); + // I'm not sure why we put this in a try catch maybe could be removed + field = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + // Fill our camera array + for (int i = 0; i < VisionConstants.kNumCams; i++) { + cams[i] = new WallEyeCam(camNames[i], -1); + acceptUpdates[i] = true; + } + // Initialize our udpSubscribers + udpSubscriber[0] = new UdpSubscriber(5802, cams[0]); + udpSubscriber[1] = new UdpSubscriber(5803, cams[2]); + udpSubscriber[2] = new UdpSubscriber(5804, cams[1], cams[3], cams[4]); + } + // Setter Methods + public void setVisionUpdating(boolean updating) { + this.visionUpdating = updating; + } + + public void setYawUpdateCamera(int idx) { + trustedCameraYawIdx = idx; + Logger.recordOutput("Vision/Trusted Yaw Camera Index", trustedCameraYawIdx); + } + + public void setMinTags(int minTags) { + this.minTags = minTags; + } + + public void setIgnoreRearCams(boolean ignore) { + this.ignoreRearCams = ignore; + acceptUpdates[1] = acceptUpdates[3] = !ignore; + } + + public void setIsAuto(boolean isAuto) { + this.isAuto = isAuto; + acceptUpdates[1] = acceptUpdates[3] = !isAuto; + } + + public void setUsingLeftCam(boolean useLeft) { + acceptUpdates[0] = useLeft; + } + + public void setUsingRightCam(boolean useRight) { + acceptUpdates[2] = useRight; + } + + // Getter Methods + public double getYawUpdateCamera() { + return trustedCameraYawIdx; + } + + public boolean isVisionUpdating() { + return visionUpdating; + } + + public boolean cameraConnected(int index) { + return cams[index].isCameraConnected(); + } + + private double getSeconds() { + return RobotController.getFPGATime() / 1_000_000; + } + + private double minTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + int[] ids = result.getTagIDs(); + // This number's value doesn't really matter it just needs to be large + double minDistance = 2000; + + // Go through the camera locations and finds the one closest to the tag + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + + double dist = + Math.sqrt( + FastMath.pow2(tagPose.getX() - camLoc.getX()) + + FastMath.pow2(tagPose.getY() - camLoc.getY())); + + if (dist < minDistance) { + minDistance = dist; + } + } + return minDistance; + } + + private double averageTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + + int[] ids = result.getTagIDs(); + double totalDistance = 0.0; + + // Goes through the camera locations and gets the average distance + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + totalDistance += + Math.sqrt( + FastMath.pow2(tagPose.getX() - camLoc.getX()) + + FastMath.pow2(tagPose.getY() - camLoc.getY())); + } + return totalDistance / ids.length; + } + + // Filters + /* Commented until a Drive Subsystem + private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) { + + ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed(); + Pose2d curPose = driveSubsystem.getPoseMeters(); + + Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d())); + + double velMagnitude = + Math.sqrt(FastMath.pow2(vel.vxMetersPerSecond) + FastMath.pow2(vel.vyMetersPerSecond)); + + double dispMagnitude = Math.sqrt(FastMath.pow2(disp.getX()) + FastMath.pow2(disp.getY())); + + /*This gets our displacement and compares it to who much we could + have moved.It does this by getting the velocity and plotting it on a + graph. The graph will be in the docs. + return result.getNumTags() >= minTags + && dispMagnitude + <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter + + VisionConstants.kOffsetOnVelFilter + + FastMath.pow2(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter)); + }*/ + + private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) { + return (result.getNumTags() >= 2 || result.getAmbiguity() < VisionConstants.kMaxAmbig) + && pose.getX() < field.getFieldLength() + && pose.getX() > 0 + && pose.getY() < field.getFieldWidth() + && pose.getY() > 0; + } + + /*Large switch case to see get the standard deviation factor based on camera, how many + tags we see and distance. An example of this graph will be in the docs.*/ + private double getStdDevFactor(double distance, int numTags, String camName) { + switch (camName) { + case VisionConstants.kCam2Name, VisionConstants.kCam4Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58YUYVSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58YUYVMultiTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + case VisionConstants.kCam5Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + + case VisionConstants.kCam1Name, VisionConstants.kCam3Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV75YUYVSingleTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.FOV75YUYVMultiTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + } + + default -> { + if (numTags == 1) + return 1 + / (VisionConstants.baseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.singleTagCoeff * distance, VisionConstants.powerNumber))); + return 1 + / (VisionConstants.baseTrust + * FastMath.exp( + FastMath.pow( + VisionConstants.multiTagCoeff * distance, VisionConstants.powerNumber))); + } + } + } + + private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation, int idx) { + /*Which pose rotation is closer to our gyro. We subtract the absolute value of the gyro + from the rotation of the pose and compare the two*/ + + double pose1Error = 2767; + double pose2Error = 2767; + if (pose1 != null) { + pose1Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose1.getRotation().rotateBy(camRotations[idx]).toRotation2d()) + .getRadians()); + } + if (pose2 != null) { + pose2Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose2.getRotation().rotateBy(camRotations[idx]).toRotation2d()) + .getRadians()); + } + Logger.recordOutput("Vision/Pose 1 Yaw Error", pose1Error); + Logger.recordOutput("Vision/Pose 2 Yaw Error", pose2Error); + Logger.recordOutput("Vision/Historical yaw", rotation); + + if (pose1Error > VisionConstants.kYawErrorThreshold) { + pose1 = null; + // textLogger.info("Reject 1 due to yaw"); + } + if (pose2Error > VisionConstants.kYawErrorThreshold) { + pose2 = null; + // textLogger.info("Reject 2 due to yaw"); + } + + if (pose1 == null && pose2 == null) { + return null; + } + + if (pose1Error < pose2Error) { + // textLogger.info("Accept 1"); + return pose1; + } else { + // textLogger.info("Accept 2"); + return pose2; + } + } + + private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) { + double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ()); + double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ()); + + // // This filters out results by seeing if they are close to the right height + if (dist1 > VisionConstants.kCamErrorZThreshold) { + // textLogger.info("Reject 1 due to z"); + pose1 = null; + } + if (dist2 > VisionConstants.kCamErrorZThreshold) { + pose2 = null; + // textLogger.info("Reject 2 due to z"); + } + + // If we don't have enough data in the gyro buffer we default to returning a pose + if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) { + if (pose1 != null) { + return pose1; + } else { + return pose2; + } + } + + // See what pose is closer the the gyro at the time of the photo's capture. + double rotation = + gyroBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime))); + Logger.recordOutput( + "Vision/Gyro Queried Loop Count", + FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); + return getCloserPose(pose1, pose2, rotation, camIndex); + } + + @Override + public void periodic() { + Logger.recordOutput("Vision/Vision Updates On", visionUpdating); + double gyroData = FastMath.normalizeMinusPiPi(turretSubsystem.getGyroRotation2d().getRadians()); + gyroBuffer.addFirst(gyroData); + + Logger.recordOutput("Vision/Gyro Buffer", gyroData); + + if (getSeconds() - timeSinceLastUpdate > VisionConstants.kMaxTimeNoVision) { + updatesToWheels = 0; + } + + validResults.clear(); + + for (int i = 0; i < VisionConstants.kNumCams; i++) { + if (!ignoreRearCams && !isAuto || (i == 0 || i == 2) && acceptUpdates[i]) { + if (cams[i].hasNewUpdate()) { + timeSinceLastUpdate = getSeconds(); + validResults.add(new Pair(cams[i].getResults(), i)); + lastResult[i] = (WallEyeTagResult) cams[i].getResults(); + } + } + } + + if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) { + // Decrease the thresholds required for a good pose over time. A graph is in the docs + for (int i = 0; i < 2; i++) { + double scaledWeight = + VisionConstants.kVisionMeasurementStdDevs.get(i, 0) + + VisionConstants.kStdDevDecayCoeff + * ((getSeconds() - timeSinceLastUpdate) - VisionConstants.kTimeToDecayDev); + + adaptiveMatrix.set( + i, + 0, + scaledWeight >= VisionConstants.kMinStdDev ? scaledWeight : VisionConstants.kMinStdDev); + } + } + + for (Pair res : validResults) { + if (res.getFirst() instanceof WallEyePoseResult) { + /*Reset the adaptive matrix to a stricter value once we get a result. + We set it to a stricter value then initially.*/ + adaptiveMatrix.set(0, 0, .1); + adaptiveMatrix.set(1, 0, .1); + + WallEyePoseResult result = (WallEyePoseResult) res.getFirst(); + + if (result.getCameraPose() == null) { // TODO figure out why it sometimes is null + textLogger.error("Pose is null, skipping!"); + continue; + } + + int idx = res.getSecond(); + + for (int i = 0; i < 2; i++) { + stdMatrix.set( + i, + 0, + .1 / getStdDevFactor(minTagDistance(result), result.getNumTags(), camNames[idx])); + } + + Pose3d cameraPose; + Translation3d robotTranslation; + Rotation3d cameraRotation; + + if (result.getNumTags() > 1) { + // If there our more then one tag in an image we can get pose possible pose + cameraPose = result.getCameraPose(); + + cameraRotation = cameraPose.getRotation().rotateBy(camRotations[idx]); + robotTranslation = + cameraPose.getTranslation().minus(camPositions[idx].rotateBy(cameraRotation)); + + } else { + // If there is one we get two possible poses and have to filter them. + Pose3d cam1Pose = result.getFirstPose(); + Pose3d cam2Pose = result.getSecondPose(); + + Logger.recordOutput("Vision/Raw Camera 1 " + camNames[idx], cam1Pose); + Logger.recordOutput("Vision/Raw Camera 2 " + camNames[idx], cam2Pose); + + // textLogger.info("Processing camera {}", camNames[idx]); + cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx); + + if (cameraPose == null) { + // textLogger.info("Both poses rejected!"); + continue; + } + + cameraRotation = cameraPose.getRotation().rotateBy(camRotations[idx]); + robotTranslation = + cameraPose.getTranslation().minus(camPositions[idx].rotateBy(cameraRotation)); + } + Pose2d robotPose = + new Pose2d(robotTranslation.toTranslation2d(), cameraRotation.toRotation2d()); + if (camsWithinField(robotTranslation, result)) { + // Is the pose in the field? If so, enjoy a updated position drive subsystem + updatesToWheels++; + Logger.recordOutput("Vision/Accepted Cam " + camNames[idx], robotPose); + // However we do have to be accepting the poses to use them + if (visionUpdating) { + if (idx == trustedCameraYawIdx) { + stdMatrix.set(2, 0, VisionConstants.kTrustYawStdDev); + } + + turretSubsystem.addVisionMeasurement( + robotPose, result.getTimeStamp() / 1_000_000.0, stdMatrix); + stdMatrix.set(2, 0, VisionConstants.kIgnoreYawStdDev); + } + } else { + Logger.recordOutput("Vision/Rejected Cam " + camNames[idx], robotPose); + } + } + } + } + + public WallEyeTagResult getLastResult(int index) { + return lastResult[index]; + } + + @Override + public Set getMeasures() { + return Set.of(); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + super.registerWith(telemetryService); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java b/src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java deleted file mode 100644 index 3aa4e67..0000000 --- a/src/main/java/frc/robot/subsystems/vision/WalleyeSubsystem.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc.robot.subsystems.vision; - -import WallEye.*; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.util.CircularBuffer; -import edu.wpi.first.math.geometry.Rotation3d; -import java.util.Set; -import org.littletonrobotics.junction.Logger; -import org.slf4j.LoggerFactory; -import org.strykeforce.telemetry.TelemetryService; -import org.strykeforce.telemetry.measurable.MeasurableSubsystem; -import org.strykeforce.telemetry.measurable.Measure; -import frc.robot.constants.VisionConstants; -import edu.wpi.first.util.CircularBuffer; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; - - -public class WalleyeSubsystem extends MeasurableSubsystem{ - - private WallEyeCam[] cams; - private Translation3d[] camPositions; - private double[] camZs; - private Rotation3d[] camRotations; - private String[] camNames; - private int[] camIndexs; - private UdpSubscriber[] udpSubscribers; - - private Logger logger; - private LoggerFactory textLogger; - private CircularBuffer turrentBuffer = new CircularBuffer<>(1000); - private AprilTagFieldLayout field; - - public WalleyeSubsystem(){ - // field = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025Reefscape.m_resourceFile); - cams = new WallEyeCam[VisionConstants.kNumCams]; - - for(int i = 0; i < VisionConstants.kNumCams; i++){ - cams[i] = new WallEyeCam(camNames[i], -1); - - } - } - - -@Override -public Set getMeasures() { - return null; -} -} \ No newline at end of file From 0c52c372fa32f5ff5a5e199eae005ec0f92199f3 Mon Sep 17 00:00:00 2001 From: Huck Date: Thu, 28 Aug 2025 19:54:32 -0400 Subject: [PATCH 03/11] Added drive and started work on TurretSubsystem --- .../frc/robot/constants/DriveConstants.java | 253 +++++++++++ .../frc/robot/constants/RobotConstants.java | 298 +++++++++++++ .../frc/robot/constants/TurretConstants.java | 34 ++ .../subsystems/drive/DriveSubsystem.java | 417 ++++++++++++++++++ .../frc/robot/subsystems/drive/Swerve.java | 384 ++++++++++++++++ .../frc/robot/subsystems/drive/SwerveFXS.java | 336 ++++++++++++++ .../frc/robot/subsystems/drive/SwerveIO.java | 115 +++++ .../subsystems/laser/LaserSubsystem.java | 12 + .../subsystems/turret/TurretSubsystem.java | 22 +- .../subsystems/vision/VisionSubsystem.java | 2 +- 10 files changed, 1867 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/constants/DriveConstants.java create mode 100644 src/main/java/frc/robot/constants/RobotConstants.java create mode 100644 src/main/java/frc/robot/constants/TurretConstants.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/drive/Swerve.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveFXS.java create mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveIO.java create mode 100644 src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java new file mode 100644 index 0000000..b7757de --- /dev/null +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -0,0 +1,253 @@ +package frc.robot.constants; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; +import com.ctre.phoenix6.configs.CommutationConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Pigeon2FeaturesConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorPhaseValue; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class DriveConstants { + public static final double kAlgaeRemovalSpeed = 0; + public static final double kBargeScoreStickMultiplier = 0.5; + + public static final double kDeadbandAllStick = 0.075; + public static final double kExpoScaleYawFactor = 0.75; + public static final double kRateLimitFwdStr = 3.5; + public static final double kRateLimitYaw = 8.0; + + public static final double kDriveMotorOutputGear = 22; + public static final double kDriveInputGear = 52; + public static final double kBevelInputGear = 15; + public static final double kBevelOutputGear = 45; + + public static final double kDriveGearRatio = + (kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear); + + public static final double kWheelDiameterInches = 3.375; + public static final double kMaxSpeedMetersPerSecond = 3.782; + public static final double kSpeedStillThreshold = 0.2; // meters per second + public static final double kGyroRateStillThreshold = 10.0; // was 10 + public static final double kGyroDifferentThreshold = 0.3; // 5 degrees + public static final int kGyroDifferentCount = 3; + + public static final double kRobotLength = 0.61595; + public static final double kRobotWidth = 0.61595; + public static final double kFieldMaxX = 17.526; + public static final double kFieldMaxY = 8.0518; + public static final double kCenterLineX = 8.763; + + public static final double kPOmega = 6.0; + public static final double kIOmega = 0.0; + public static final double kDOmega = 0.0; + public static final double kMaxAutoOmega = 10000; + public static final double kMaxVelOmega = + (kMaxSpeedMetersPerSecond / Math.hypot(kRobotWidth / 2.0, kRobotLength / 2.0)) / 2.0; + public static final double kMaxAccelOmega = 100000; // was 5 + + public static final double kPHolonomic = 4.0; // was 3 + public static final double kIHolonomic = 0.0000; + public static final double kDHolonomic = 0.00; // kPHolonomic/100 + + public static Translation2d[] getWheelLocationMeters() { + final double x = kRobotLength / 2.0; // front-back, was ROBOT_LENGTH + final double y = kRobotWidth / 2.0; // left-right, was ROBOT_WIDTH + Translation2d[] locs = new Translation2d[4]; + locs[0] = new Translation2d(x, y); // left front + locs[1] = new Translation2d(x, -y); // right front + locs[2] = new Translation2d(-x, y); // left rear + locs[3] = new Translation2d(-x, -y); // right rear + return locs; + } + + // temp stuff + public static final int kTempAvgCount = 25; + public static final double kTripTemp = 1300; + public static final double kRecoverTemp = 1290; + public static final double kNotifyTemp = 1295; + + public static final Pose2d kResetOdomPose = + new Pose2d(new Translation2d(0.5, 3.62), Rotation2d.fromDegrees(67)); + + public static TalonFXSConfiguration getAzimuthFXSConfig() { + // constructor sets encoder to Quad/CTRE_MagEncoder_Relative + TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration(); + + HardwareLimitSwitchConfigs hardwareLimitSwitchConfigs = new HardwareLimitSwitchConfigs(); + hardwareLimitSwitchConfigs.ForwardLimitEnable = false; + hardwareLimitSwitchConfigs.ReverseLimitEnable = false; + azimuthConfig.HardwareLimitSwitch = hardwareLimitSwitchConfigs; + + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 20; + currentConfig.SupplyCurrentLowerTime = 20; + currentConfig.SupplyCurrentLowerLimit = 1; + + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = false; + + azimuthConfig.CurrentLimits = currentConfig; + + Slot0Configs slot0Config = new Slot0Configs(); + slot0Config.kP = 300; + slot0Config.kI = 0.0; + slot0Config.kD = 3; + slot0Config.kV = 4.3; + + azimuthConfig.Slot0 = slot0Config; + + ExternalFeedbackConfigs externalFeedbackConfigs = new ExternalFeedbackConfigs(); + externalFeedbackConfigs.VelocityFilterTimeConstant = 0.02; // ? + externalFeedbackConfigs.ExternalFeedbackSensorSource = + ExternalFeedbackSensorSourceValue.Quadrature; + externalFeedbackConfigs.SensorPhase = SensorPhaseValue.Opposed; + azimuthConfig.ExternalFeedback = externalFeedbackConfigs; + + // VoltageConfigs voltageConfig = new VoltageConfigs(); + // voltageConfig.SupplyVoltageTimeConstant = 3.2; // FIXME, seems very long + // azimuthConfig.Voltage = voltageConfig; + + MotionMagicConfigs motionConfig = new MotionMagicConfigs(); + motionConfig.MotionMagicCruiseVelocity = 2; + motionConfig.MotionMagicAcceleration = 20; + azimuthConfig.MotionMagic = motionConfig; + + MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); + // motorConfigs.DutyCycleNeutralDeadband = 0.04; + motorConfigs.NeutralMode = NeutralModeValue.Coast; + motorConfigs.Inverted = InvertedValue.Clockwise_Positive; + azimuthConfig.MotorOutput = motorConfigs; + + CommutationConfigs commutationConfigs = new CommutationConfigs(); + commutationConfigs.MotorArrangement = MotorArrangementValue.Minion_JST; + + azimuthConfig.Commutation = commutationConfigs; + + return azimuthConfig; + } + + public static TalonSRXConfiguration getAzimuthTalonConfig() { + // constructor sets encoder to Quad/CTRE_MagEncoder_Relative + TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration(); + + azimuthConfig.primaryPID.selectedFeedbackCoefficient = 1.0; + azimuthConfig.auxiliaryPID.selectedFeedbackSensor = FeedbackDevice.None; + + azimuthConfig.forwardLimitSwitchSource = LimitSwitchSource.Deactivated; + azimuthConfig.reverseLimitSwitchSource = LimitSwitchSource.Deactivated; + + azimuthConfig.continuousCurrentLimit = 10; + azimuthConfig.peakCurrentDuration = 0; + azimuthConfig.peakCurrentLimit = 0; + + azimuthConfig.slot0.kP = 15.0; + azimuthConfig.slot0.kI = 0.0; + azimuthConfig.slot0.kD = 150.0; + azimuthConfig.slot0.kF = 1.0; + azimuthConfig.slot0.integralZone = 0; + azimuthConfig.slot0.allowableClosedloopError = 0; + azimuthConfig.slot0.maxIntegralAccumulator = 0; + + azimuthConfig.motionCruiseVelocity = 800; + azimuthConfig.motionAcceleration = 10_000; + azimuthConfig.velocityMeasurementWindow = 64; + azimuthConfig.velocityMeasurementPeriod = SensorVelocityMeasPeriod.Period_100Ms; + azimuthConfig.voltageCompSaturation = 12; + azimuthConfig.voltageMeasurementFilter = 32; + azimuthConfig.neutralDeadband = 0.04; + return azimuthConfig; + } + + public static TalonFXConfiguration getDriveTalonConfig() { + TalonFXConfiguration driveConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 80; + currentConfig.SupplyCurrentLowerLimit = 80; + currentConfig.SupplyCurrentLowerTime = 1.0; + currentConfig.StatorCurrentLimit = 160; + + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = true; + + driveConfig.CurrentLimits = currentConfig; + + Slot0Configs slot0Config = new Slot0Configs(); + slot0Config.kP = 0.2; + slot0Config.kI = 0.0; + slot0Config.kD = 0.0; + slot0Config.kV = 0.117; + slot0Config.kS = 0.0; + slot0Config.kA = 0.0; + slot0Config.kG = 0.0; + driveConfig.Slot0 = slot0Config; + + MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); + motorConfigs.DutyCycleNeutralDeadband = 0.01; + motorConfigs.NeutralMode = NeutralModeValue.Brake; + driveConfig.MotorOutput = motorConfigs; + + return driveConfig; + } + + public static final int kPigeonCanID = 4; + + public static Pigeon2Configuration getPigeon2Configuration() { + Pigeon2Configuration config = new Pigeon2Configuration(); + + config.MountPose.MountPoseYaw = 0.0; + config.MountPose.MountPoseRoll = 0.0; + config.MountPose.MountPosePitch = 0.0; + + config.GyroTrim.GyroScalarX = -1.2; + config.GyroTrim.GyroScalarY = 4.8; + config.GyroTrim.GyroScalarZ = -2.9; + + return config; + } + + public static Pigeon2Configuration getPigeon2NoMotionDisabledConfiguration() { + Pigeon2FeaturesConfigs featureConfigs = + new Pigeon2FeaturesConfigs().withDisableNoMotionCalibration(true); + Pigeon2Configuration config = getPigeon2Configuration(); + config.withPigeon2Features(featureConfigs); + + return config; + } + + public static CurrentLimitsConfigs getSafeDriveLimits() { + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 30; + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = false; + return currentConfig; + } + + public static CurrentLimitsConfigs getNormDriveLimits() { + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 30; + + currentConfig.StatorCurrentLimit = 140; + + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = true; + return currentConfig; + } +} diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java new file mode 100644 index 0000000..1bba738 --- /dev/null +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -0,0 +1,298 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.CommutationConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotController; +import org.slf4j.LoggerFactory; + +public class RobotConstants { + private org.slf4j.Logger logger = LoggerFactory.getLogger(RobotConstants.class); + public static final String protoSerial = "032243F2"; + public static final boolean isComp = true; + public static final int kTalonConfigTimeout = 10; // ms + + public static final double kJoystickDeadband = 0.1; + public static final double kTriggerDeadband = 0.5; + public static final double kTestingDeadband = 0.5; + + // Elevator + public static Angle kElevatorFunnelSetpoint; // Elevator + public static Angle kElevatorL1LoadSetpoint; // Elevator + public static Angle kElevatorStowSetpoint; // Elevator + + // Biscuit + public static TalonFXSConfiguration talonFXSConfig; + public static MotionMagicConfigs algaeMotionConfig; + public static MotionMagicConfigs algaeRemovalMotionConfig; + public static MotionMagicConfigs noAlgaeMotionConfig; + + public static double kTicksPerRot; + + public static double kBiscuitZero; + public static double kSafeToStowUpper; + public static double kSafeToStowLower; + + // Speeds + // public static final double kDosntHaveAlgaeSpeed = 500; + + // Setpoints + // Idle + public static Angle kStowSetpoint; + public static Angle kFunnelSetpoint; + public static Angle kL1CoralLoadSetpoint; + public static Angle kPrestageSetpoint; + public static Angle kPrestageAlgaeSetpoint; + + // Algae removal + public static Angle kL2AlgaeSetpoint; + public static Angle kL3AlgaeSetpoint; + + public static Angle kL2AlgaeRemovalSetpoint; + public static Angle kL3AlgaeRemovalSetpoint; + + // Coral score + public static Angle kL1CoralSetpoint; + public static Angle kL2CoralSetpoint; + public static Angle kL3CoralSetpoint; + public static Angle kL4CoralSetpoint; + + // Algae obtaining + public static Angle kFloorAlgaeSetpoint; + public static Angle kMicAlgaeSetpoint; + public static Angle kHpAlgaeSetpoint; + + // Algae scoring + public static Angle kProcessorSetpoint; + public static Angle kBargeSetpoint; + // public static Angle kBargeBackwardSetpoint; + + public static double kTagAlignThreshold; + + public static final int kMinAutoSwitchID = 4; + public static final int kMaxAutoSwitchID = 9; + + public RobotConstants() { + // Proto constants + kElevatorFunnelSetpoint = ProtoConstants.kElevatorFunnelSetpoint; + kElevatorL1LoadSetpoint = ProtoConstants.kElevatorL1LoadSetpoint; + kElevatorStowSetpoint = ProtoConstants.kElevatorStowSetpoint; + talonFXSConfig = ProtoConstants.getFXSConfig(); + algaeMotionConfig = ProtoConstants.getAlgaeMotionConfig(); + algaeRemovalMotionConfig = algaeMotionConfig; + noAlgaeMotionConfig = ProtoConstants.getNoAlgaeMotionConfig(); + kTicksPerRot = 160; + logger.info("Using Proto Constants"); + + // Biscuit + + kBiscuitZero = ProtoConstants.kZero; + kSafeToStowUpper = ProtoConstants.kSafeToStowUpper; + kSafeToStowLower = ProtoConstants.kSafeToStowLower; + + // Setpoints + // Idle + kStowSetpoint = ProtoConstants.kBiscuitStowSetpoint; + kFunnelSetpoint = ProtoConstants.kFunnelSetpoint; + kL1CoralLoadSetpoint = ProtoConstants.kL1CoralLoadSetpoint; + kPrestageSetpoint = ProtoConstants.kPrestageSetpoint; + kPrestageAlgaeSetpoint = ProtoConstants.kPrestageAlgaeSetpoint; + + // Algae removal + kL2AlgaeSetpoint = ProtoConstants.kL2AlgaeSetpoint; + kL3AlgaeSetpoint = ProtoConstants.kL3AlgaeSetpoint; + + kL2AlgaeRemovalSetpoint = ProtoConstants.kL2AlgaeRemovalSetpoint; + kL3AlgaeRemovalSetpoint = ProtoConstants.kL3AlgaeRemovalSetpoint; + + // Coral score + kL1CoralSetpoint = ProtoConstants.kL1CoralSetpoint; + kL2CoralSetpoint = ProtoConstants.kL2CoralSetpoint; + kL3CoralSetpoint = ProtoConstants.kL3CoralSetpoint; + kL4CoralSetpoint = ProtoConstants.kL4CoralSetpoint; + + // Algae obtaining + kFloorAlgaeSetpoint = ProtoConstants.kFloorAlgaeSetpoint; + kMicAlgaeSetpoint = ProtoConstants.kMicAlgaeSetpoint; + kHpAlgaeSetpoint = ProtoConstants.kHpAlgaeSetpoint; + + // Algae scoring + kProcessorSetpoint = ProtoConstants.kProcessorSetpoint; + kBargeSetpoint = ProtoConstants.kBargeSetpoint; + // kBargeBackwardSetpoint = ProtoConstants.kBargeBackwardSetpoint; + + kTagAlignThreshold = ProtoConstants.kTagAlignThreshold; + } + + public static class ProtoConstants { + + // Biscuit + public static TalonFXSConfiguration talonFXSConfig; + public static MotionMagicConfigs alageMotionConfig; + public static MotionMagicConfigs noAlageMotionConfig; + + public static double kTicksPerRot = 160; + + public static final double kZero = .69; + public static final double kSafeToStowUpper = 4.06; + public static final double kSafeToStowLower = -5 / 2; + + // Speeds + // public static final double kDosntHaveAlgaeSpeed = 500; + + // Setpoints + // Idle + public static Angle kBiscuitStowSetpoint = Rotations.of(1.862 / 2); + public static Angle kFunnelSetpoint = kBiscuitStowSetpoint; + public static Angle kL1CoralLoadSetpoint = Rotations.of(3.96); + public static Angle kPrestageSetpoint = kBiscuitStowSetpoint; + public static Angle kPrestageAlgaeSetpoint = Rotations.of(9.089 / 2); + + // Algae removal + public static Angle kL2AlgaeSetpoint = Rotations.of(24.104 / 2); + public static Angle kL3AlgaeSetpoint = Rotations.of(24.562 / 2); + + public static Angle kL2AlgaeRemovalSetpoint = kBiscuitStowSetpoint; + public static Angle kL3AlgaeRemovalSetpoint = kBiscuitStowSetpoint; + + public static double kTagAlignThreshold = 20.0 / 2; + + // Coral score + public static Angle kL1CoralSetpoint = Rotations.of(12.7); + public static Angle kL2CoralSetpoint = kBiscuitStowSetpoint; + public static Angle kL3CoralSetpoint = kBiscuitStowSetpoint; + public static Angle kL4CoralSetpoint = kBiscuitStowSetpoint; + + // Algae obtaining + public static Angle kFloorAlgaeSetpoint = Rotations.of(49.627 / 2); + public static Angle kMicAlgaeSetpoint = Rotations.of(51.61872 / 2); + public static Angle kHpAlgaeSetpoint = Rotations.of(16.97559 / 2); + + // Algae scoring + public static Angle kProcessorSetpoint = Rotations.of(41.193 / 2); + public static Angle kBargeSetpoint = Rotations.of(12.3489 / 2); + // public static Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // 9.089 + + // Elevator + public static Angle kElevatorFunnelSetpoint = Rotations.of(2.03125); + public static Angle kElevatorL1LoadSetpoint = Rotations.of(1.76); + public static Angle kElevatorStowSetpoint = kElevatorFunnelSetpoint; + public static Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5)); + public static Angle kMaxRev = kPrestageSetpoint.minus(Rotations.of(5)); + + public static MotionMagicConfigs getAlgaeMotionConfig() { + MotionMagicConfigs algaeConfig = + new MotionMagicConfigs() + .withMotionMagicAcceleration(300) + .withMotionMagicCruiseVelocity(50) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(600); + return algaeConfig; + } + + public static MotionMagicConfigs getAlgaeRemovalMotionConfig() { + MotionMagicConfigs algaeConfig = + new MotionMagicConfigs() + .withMotionMagicAcceleration(300) + .withMotionMagicCruiseVelocity(30) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(1500); + return algaeConfig; + } + + public static MotionMagicConfigs getNoAlgaeMotionConfig() { + MotionMagicConfigs algaeConfig = + new MotionMagicConfigs() + .withMotionMagicAcceleration(300) + .withMotionMagicCruiseVelocity(80) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(1800); + return algaeConfig; + } + + public static TalonFXSConfiguration getFXSConfig() { + TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(0) + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimit(20) + .withSupplyCurrentLowerLimit(5) + .withSupplyCurrentLowerTime(2) + .withSupplyCurrentLimitEnable(true); + fxsConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxsConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kMaxFwd) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kMaxRev); + fxsConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(2) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0) + .withKS(0) + .withKV(0.1) + .withKA(0); + fxsConfig.Slot0 = slot0; + + fxsConfig.MotionMagic = getNoAlgaeMotionConfig(); + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Brake); + fxsConfig.MotorOutput = motorOut; + + CommutationConfigs commutation = + new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST); + fxsConfig.Commutation = commutation; + + ExternalFeedbackConfigs feedBack = + new ExternalFeedbackConfigs() + .withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation); + fxsConfig.ExternalFeedback = feedBack; + + return fxsConfig; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java new file mode 100644 index 0000000..90e7a13 --- /dev/null +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -0,0 +1,34 @@ +package frc.robot.constants; + +import edu.wpi.first.units.measure.Angle; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class TurretConstants { + +public static final Angle leftTurretLimit = null; +public static final Angle rightTurretLimit = null; + +public static TalonFXConfiguration turretFXConfig() { + TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(70) + .withMotionMagicAcceleration(300) // was 300 + .withMotionMagicJerk(1500); + fxConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive); + fxConfig.MotorOutput = motorOut; + return fxConfig; +} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java new file mode 100644 index 0000000..183f7df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -0,0 +1,417 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.DriveConstants; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import java.util.Set; +import java.util.function.BooleanSupplier; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class DriveSubsystem extends MeasurableSubsystem { + private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class); + private final SwerveIO io; + private SwerveIOInputsAutoLogged inputs = new SwerveIOInputsAutoLogged(); + private final HolonomicDriveController holonomicController; + + private final ProfiledPIDController omegaController; + private final PIDController xController; + private final PIDController yController; + + public DriveStates currDriveState = DriveStates.IDLE; + private ChassisSpeeds holoContOutput = new ChassisSpeeds(); + + private double trajectoryActive = 0.0; + + private double driveMultiplier = 1.0; + + private int gyroDifferentCount = 0; + private int gyroCorrectionCount = 0; + + private boolean ignoreSticks = false; + + private RobotStateSubsystem robotStateSubsystem; + + public DriveSubsystem(SwerveIO io) { + this.io = io; + // Setup Holonomic Controller + omegaController = + new ProfiledPIDController( + DriveConstants.kPOmega, + DriveConstants.kIOmega, + DriveConstants.kDOmega, + new TrapezoidProfile.Constraints( + DriveConstants.kMaxAutoOmega, DriveConstants.kMaxAccelOmega)); + omegaController.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); + + xController = + new PIDController( + DriveConstants.kPHolonomic, DriveConstants.kIHolonomic, DriveConstants.kDHolonomic); + // xController.setIntegratorRange(DriveConstants.kIMin, DriveConstants.kIMax); + yController = + new PIDController( + DriveConstants.kPHolonomic, DriveConstants.kIHolonomic, DriveConstants.kDHolonomic); + // yController.setIntegratorRange(DriveConstants.kIMin, DriveConstants.kIMax); + holonomicController = new HolonomicDriveController(xController, yController, omegaController); + // Disabling the holonomic controller makes the robot directly follow the + // trajectory output (no + // closing the loop on x,y,theta errors) + holonomicController.setEnabled(true); + + setAutoDebugMsg("Nothing"); + } + + public boolean hasZeroed() { + return inputs.didZero; + } + + public void zeroModules() { + io.zeroModules(); + } + + // Open-Loop Swerve Movements + public void drive(double vXmps, double vYmps, double vOmegaRadps) { + if (!ignoreSticks) { + io.drive(vXmps * driveMultiplier, vYmps * driveMultiplier, vOmegaRadps, true); + } + } + + public void stopDriving() { + this.move(0, 0, 0, false); + io.drive(0, 0, 0, false); + } + + public void setAzimuthVel(double vel) { + io.setAzimuthVel(vel); + } + + // Closed-Loop (Velocity Controlled) Swerve Movement + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Move X", vXmps); + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Move Y", vYmps); + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Move Omega", vOmegaRadps); + + io.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + public double getHolonomicControllerOmegaErrorRadians() { + return omegaController.getPositionError(); + } + + public void resetOdometry(Pose2d pose) { + io.resetOdometry(pose); + logger.info("reset odometry with: {}", pose); + } + + public void setAutoDebugMsg(String msg) { + org.littletonrobotics.junction.Logger.recordOutput("Swerve/Auto Drive Info", msg); + } + + public void setIgnoreSticks(boolean ignore) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Ignoring Sticks", ignore); + this.ignoreSticks = ignore; + } + + public void addVisionMeasurement(Pose2d pose, double timestamp) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000 - timestamp); + io.addVisionMeasurement(pose, timestamp); + } + + public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDevvs) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/stdDevvs", stdDevvs.get(0, 0)); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000.0 - timestamp); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/FPGA Seconds", RobotController.getFPGATime() / 1_000_000.0); + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Result Seconds", timestamp); + + io.addVisionMeasurement(pose, timestamp, stdDevvs); + } + + public void resetHolonomicController(double yaw) { + xController.reset(); + yController.reset(); + omegaController.reset(yaw); + holonomicController.getThetaController().reset(yaw); + logger.info("Holonomic Controller Reset: {}", yaw); + } + + public void resetHolonomicController() { + xController.reset(); + yController.reset(); + omegaController.reset(inputs.gyroRotation2d.getRadians()); + holonomicController.getThetaController().reset(inputs.gyroRotation2d.getRadians()); + logger.info("Holonomic Controller Reset: {}", inputs.gyroRotation2d.getRadians()); + } + + public void resetOmegaController() { + omegaController.reset(inputs.gyroRotation2d.getRadians()); + } + + // Getters/Setters + public Pose2d getPoseMeters() { + return inputs.poseMeters; + } + + public Rotation2d getGyroRotation2d() { + return inputs.gyroRotation2d; + } + + public ChassisSpeeds getFieldRelSpeed() { + return inputs.fieldRelSpeed; + } + + public ChassisSpeeds getRobotRelSpeed() { + return inputs.robotRelSpeed; + } + + public void setDriveState(DriveStates state) { + currDriveState = state; + } + + public BooleanSupplier getAzimuth1FwdLimitSupplier() { + return io.getAzimuth1FwdLimitSwitch(); + } + + public boolean isDriveStill() { + ChassisSpeeds cs = inputs.fieldRelSpeed; + double vX = cs.vxMetersPerSecond; + double vY = cs.vyMetersPerSecond; + + // Take fieldRel Speed and get the magnitude of the vector + double wheelSpeed = Math.sqrt(FastMath.pow2(vX) + FastMath.pow2(vY)); + + boolean velStill = Math.abs(wheelSpeed) <= DriveConstants.kSpeedStillThreshold; + boolean gyroStill = isGyroStill(); + + return velStill && gyroStill; + } + + public boolean isGyroStill() { + return Math.abs(inputs.gyroRate) <= DriveConstants.kGyroRateStillThreshold; + } + + public void setGyroOffset(Rotation2d rotation) { + io.setBothGyroOffset(apply(rotation)); + Logger.recordOutput("DriveSubsystem/gyroOffset", rotation); + } + + public void setEnableHolo(boolean enabled) { + holonomicController.setEnabled(enabled); + logger.info("Holonomic Controller Enabled: {}", enabled); + } + + public void prepClimb() { + io.setDriveCoast(true); + io.setSwerveModuleAngles( + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90)); + } + + public void teleResetGyro() { + setAutoDebugMsg("Reset Gyro"); + logger.info("Driver Joystick: Reset Gyro"); + double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0; + io.setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); + Logger.recordOutput("DriveSubsystem/gyroOffset", Rotation2d.fromDegrees(gyroResetDegs)); + io.resetGyro(); + io.resetOdometry( + new Pose2d(inputs.poseMeters.getTranslation(), Rotation2d.fromDegrees(gyroResetDegs))); + } + + // Make whether a trajectory is currently active obvious on grapher + public void grapherTrajectoryActive(boolean active) { + if (active) trajectoryActive = 1.0; + else trajectoryActive = 0.0; + } + + // Field flipping stuff + public boolean shouldFlip() { + return robotStateSubsystem.getAllianceColor() == Alliance.Red; + } + + public Translation2d apply(Translation2d translation) { + if (shouldFlip()) { + return new Translation2d(DriveConstants.kFieldMaxX - translation.getX(), translation.getY()); + } else { + return translation; + } + } + + public Pose2d apply(Pose2d pose) { + if (shouldFlip()) { + return new Pose2d( + DriveConstants.kFieldMaxX - pose.getX(), + pose.getY(), + new Rotation2d(-pose.getRotation().getCos(), pose.getRotation().getSin())); + } else { + return pose; + } + } + + public Rotation2d apply(Rotation2d rotation) { + logger.info( + "initial target yaw: {}, cos: {}, sin: {}", rotation, rotation.getCos(), rotation.getSin()); + if (shouldFlip()) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } else return rotation; + } + + public double apply(double x) { + logger.info("initial x: {}", x); + if (shouldFlip()) { + return DriveConstants.kFieldMaxX - x; + } else return x; + } + + // Control Methods + public void lockZero() { + Rotation2d rot = Rotation2d.fromDegrees(0.0); + io.setSwerveModuleAngles(rot, rot, rot, rot); + } + + public void toSafeHold() { + setDriveState(DriveStates.SAFE_HOLD); + io.configDriveCurrents(DriveConstants.getSafeDriveLimits()); + } + + public void toIdle() { + setDriveState(DriveStates.IDLE); + io.configDriveCurrents(DriveConstants.getNormDriveLimits()); + } + + public PIDController getxController() { + return xController; + } + + public PIDController getyController() { + return yController; + } + + public ProfiledPIDController getomegaController() { + return omegaController; + } + + public PIDController getomegaControllerNonProfiled() { + return new PIDController( + omegaController.getP(), omegaController.getI(), omegaController.getD()); + } + + public double getAvgDriveCurrent() { + return FastMath.abs(inputs.avgDriveCurrent); + } + + public double getAvgRearDriveVel() { + return inputs.avgRearDriveVel; + } + + public void setDriveMultiplier(double multiplier) { + driveMultiplier = multiplier; + } + + public double getDriveMultiplier() { + return driveMultiplier; + } + + public void removeDriveMultiplier() { + driveMultiplier = 1.0; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Swerve Pose", inputs.swervePose); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Gyro Correction Count", gyroCorrectionCount); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Gyro Disagreement", + inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees()); + + if (Math.abs(inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees()) + > DriveConstants.kGyroDifferentThreshold) { + gyroDifferentCount++; + } else { + gyroDifferentCount = 0; + } + if (gyroDifferentCount > DriveConstants.kGyroDifferentCount && isDriveStill()) { + io.setPigeonGyroOffset( + inputs.navxRotation2d.minus(inputs.gyroRotation2d).plus(io.getPigeonGyroOffset())); + logger.info( + "NavX gyro correction degs {} -> {}", + inputs.gyroRotation2d.getDegrees(), + inputs.navxRotation2d.getDegrees()); + gyroDifferentCount = 0; + gyroCorrectionCount++; + } + + switch (currDriveState) { + case IDLE -> {} + case SAFE -> {} + case SAFE_HOLD -> {} + default -> {} + } + } + + public enum DriveStates { + IDLE, + SAFE, + SAFE_HOLD + } + + @Override + public void registerWith(TelemetryService telemetryService) { + io.registerWith(telemetryService); + super.registerWith(telemetryService); + } + + @Override + public Set getMeasures() { + return Set.of( + new Measure("State", () -> currDriveState.ordinal()), + new Measure("Gyro roll", () -> inputs.gyroRoll), + new Measure("Gyro pitch", () -> inputs.gyroPitch), + new Measure("Gyro Rotation2D(deg)", () -> inputs.gyroRotation2d.getDegrees()), + new Measure("Odometry X", () -> inputs.poseMeters.getX()), + new Measure("Odometry Y", () -> inputs.poseMeters.getY()), + new Measure("Odometry Rotation2D(deg)", () -> inputs.poseMeters.getRotation().getDegrees()), + new Measure("Holonomic Cont Vx", () -> holoContOutput.vxMetersPerSecond), + new Measure("Holonomic Cont Vy", () -> holoContOutput.vyMetersPerSecond), + new Measure("Holonomic Cont Vomega", () -> holoContOutput.omegaRadiansPerSecond), + new Measure( + "Holo Controller Omega Err", + () -> holonomicController.getThetaController().getPositionError()), + new Measure( + "Holo Controller Omega Setpoint", + () -> holonomicController.getThetaController().getSetpoint().position), + new Measure("Trajectory Active", () -> trajectoryActive)); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java new file mode 100644 index 0000000..906b598 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -0,0 +1,384 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.Matrix; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.RobotConstants; +import frc.robot.constants.VisionConstants; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.strykeforce.gyro.SF_AHRS; +import org.strykeforce.gyro.SF_PIGEON2; +import org.strykeforce.healthcheck.Checkable; +import org.strykeforce.swerve.OdometryStrategy; +import org.strykeforce.swerve.PoseEstimatorOdometryStrategy; +import org.strykeforce.swerve.SwerveDrive; +import org.strykeforce.swerve.SwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule.ClosedLoopUnits; +import org.strykeforce.telemetry.TelemetryService; + +public class Swerve implements SwerveIO, Checkable { + private final SwerveDrive swerveDrive; + + // Grapher stuff + private PoseEstimatorOdometryStrategy odometryStrategy; + + private SF_PIGEON2 pigeon; + private SF_AHRS navx; + private Rotation2d navxOffset = new Rotation2d(); + + private TalonFXConfigurator configurator; + + private TalonSRX[] azimuths = new TalonSRX[4]; + private TalonFX[] drives = new TalonFX[4]; + + private V6TalonSwerveModule[] swerveModules; + private SwerveDriveKinematics kinematics; + private double fieldY = 0.0; + private double fieldX = 0.0; + + private StatusSignal drive10StatorCurrent; + private StatusSignal drive11StatorCurrent; + private StatusSignal drive12StatorCurrent; + private StatusSignal drive13StatorCurrent; + private StatusSignal drive10Velocity; + private StatusSignal drive11Velocity; + private StatusSignal drive12Velocity; + private StatusSignal drive13Velocity; + private boolean didZero = false; + + public Swerve() { + + var moduleBuilder = + new V6TalonSwerveModule.V6Builder() + .driveGearRatio(DriveConstants.kDriveGearRatio) + .wheelDiameterInches(DriveConstants.kWheelDiameterInches) + .driveMaximumMetersPerSecond(DriveConstants.kMaxSpeedMetersPerSecond) + .latencyCompensation(true); + + swerveModules = new V6TalonSwerveModule[4]; + Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters(); + + for (int i = 0; i < 4; i++) { + var azimuthTalon = new TalonSRX(i); + azimuths[i] = azimuthTalon; + azimuthTalon.configFactoryDefault(RobotConstants.kTalonConfigTimeout); + azimuthTalon.configAllSettings( + DriveConstants.getAzimuthTalonConfig(), RobotConstants.kTalonConfigTimeout); + azimuthTalon.enableCurrentLimit(true); + azimuthTalon.enableVoltageCompensation(true); + azimuthTalon.setNeutralMode(NeutralMode.Coast); + + var driveTalon = new TalonFX(i + 10); + drives[i] = driveTalon; + configurator = driveTalon.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); // factory default + configurator.apply(DriveConstants.getDriveTalonConfig()); + driveTalon.getSupplyVoltage().setUpdateFrequency(100); + driveTalon.getSupplyCurrent().setUpdateFrequency(100); + driveTalon.getClosedLoopReference().setUpdateFrequency(200); + + swerveModules[i] = + moduleBuilder + .azimuthTalon(azimuthTalon) + .driveTalon(driveTalon) + .wheelLocationMeters(wheelLocations[i]) + .closedLoopUnits(ClosedLoopUnits.VOLTAGE) + .build(); + swerveModules[i].loadAndSetAzimuthZeroReference(); + } + + drive10StatorCurrent = drives[0].getStatorCurrent(); + drive11StatorCurrent = drives[1].getStatorCurrent(); + drive12StatorCurrent = drives[2].getStatorCurrent(); + drive13StatorCurrent = drives[3].getStatorCurrent(); + drive10Velocity = drives[0].getVelocity(); + drive11Velocity = drives[1].getVelocity(); + drive12Velocity = drives[2].getVelocity(); + drive13Velocity = drives[3].getVelocity(); + didZero = true; + + pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "rio"); + pigeon.applyConfig(DriveConstants.getPigeon2Configuration()); + navx = new SF_AHRS(); + swerveDrive = new SwerveDrive(false, 0.02, pigeon, swerveModules); + swerveDrive.resetGyro(); + swerveDrive.setGyroOffset(Rotation2d.fromDegrees(0)); + + kinematics = swerveDrive.getKinematics(); + + odometryStrategy = + new PoseEstimatorOdometryStrategy( + swerveDrive.getHeading(), + new Pose2d(), + swerveDrive.getKinematics(), + VisionConstants.kStateStdDevs, + VisionConstants.kLocalMeasurementStdDevs, + VisionConstants.kVisionMeasurementStdDevs, + getSwerveModulePositions()); + + swerveDrive.setOdometry(odometryStrategy); + } + + // Getters/Setter + @Override + public String getName() { + return "Swerve"; + } + + private SwerveModule[] getSwerveModules() { + return swerveDrive.getSwerveModules(); + } + + public void setSwerveModuleAngles(Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) { + swerveModules[0].setAzimuthRotation2d(FL); + swerveModules[1].setAzimuthRotation2d(FR); + swerveModules[2].setAzimuthRotation2d(BL); + swerveModules[3].setAzimuthRotation2d(BR); + } + + private SwerveModulePosition[] getSwerveModulePositions() { + SwerveModule[] swerveModules = getSwerveModules(); + SwerveModulePosition[] temp = {null, null, null, null}; + for (int i = 0; i < 4; ++i) { + temp[i] = swerveModules[i].getPosition(); + } + return temp; + } + + public SwerveModuleState[] getSwerveModuleStates() { + V6TalonSwerveModule[] swerveModules = (V6TalonSwerveModule[]) swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return swerveModuleStates; + } + + private ChassisSpeeds getRobotRelSpeed() { + SwerveDriveKinematics kinematics = swerveDrive.getKinematics(); + SwerveModule[] swerveModules = swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; ++i) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return kinematics.toChassisSpeeds(swerveModuleStates); + } + + private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { + Rotation2d heading = swerveDrive.getHeading().unaryMinus(); + fieldX = + roboRelSpeed.vxMetersPerSecond * heading.getCos() + + roboRelSpeed.vyMetersPerSecond * heading.getSin(); + fieldY = + -roboRelSpeed.vxMetersPerSecond * heading.getSin() + + roboRelSpeed.vyMetersPerSecond * heading.getCos(); + + return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond); + } + + public Rotation2d getPigeonGyroOffset() { + return swerveDrive.getGyroOffset(); + } + + @Override + public void setOdometry(OdometryStrategy odom) { + swerveDrive.setOdometry(odom); + } + + @Override + public void setPigeonGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + } + + @Override + public void setBothGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + navxOffset = rotation; + } + + @Override + public void setDriveCoast(boolean coast) { + for (int i = 0; i < 4; i++) { + drives[i] + .getConfigurator() + .apply( + DriveConstants.getDriveTalonConfig() + .MotorOutput + .withNeutralMode(coast ? NeutralModeValue.Coast : NeutralModeValue.Brake)); + } + } + + public void disableNoMotionCal() { + pigeon.applyConfig(DriveConstants.getPigeon2NoMotionDisabledConfiguration()); + } + + @Override + public void resetGyro() { + swerveDrive.resetGyro(); + navx.reset(); + } + + @Override + public void prepForAuto(Pose2d pose2d, double offsetDegrees, Alliance alliance) { + double gyroResetDegs = alliance == Alliance.Blue ? 0.0 : 180.0; + setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); + resetGyro(); + setBothGyroOffset(Rotation2d.fromDegrees(offsetDegrees)); + resetOdometry(pose2d); + } + + @Override + public void resetOdometry(Pose2d pose) { + swerveDrive.resetOdometry(pose); + // navx.reset(); + } + + @Override + public void addVisionMeasurement(Pose2d pose, double timestamp) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose, timestamp); + } + + @Override + public void addVisionMeasurement(Pose2d pose2d, double timestamp, Matrix stdDevs) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose2d, timestamp, stdDevs); + } + + @Override + public void drive(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.drive(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void setAzimuthVel(double vel) { + for (int i = 0; i < 4; i++) { + azimuths[i].set(TalonSRXControlMode.PercentOutput, vel); + } + } + + @Override + public void configDriveCurrents(CurrentLimitsConfigs config) { + for (int i = 0; i < 4; i++) { + drives[i].getConfigurator().apply(config); + } + } + + public double getAvgDriveCurrent() { + double sum = 0; + + for (int i = 0; i < 4; i++) { + sum += drives[i].getStatorCurrent().getValueAsDouble(); + } + + return sum / 4.0; + } + + public double getRearDriveAvgVel() { + double sum = 0; + + for (int i = 2; i < 4; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + + @Override + public void zeroModules() { + for (int i = 0; i < 4; i++) { + swerveModules[i].loadAndSetAzimuthZeroReference(); + } + } + + @Override + public void updateInputs(SwerveIOInputs inputs) { + swerveDrive.updateInputs(); // Call before swerveDrive.periodic() + swerveDrive.periodic(); + + inputs.odometryX = swerveDrive.getPoseMeters().getX(); + inputs.odometryY = swerveDrive.getPoseMeters().getY(); + inputs.swervePose = odometryStrategy.getPoseMeters(); + inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees(); + inputs.gyroRotation2d = swerveDrive.getHeading(); + inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset); + inputs.normalizedGyroRotation = + FastMath.normalizeMinusPiPi(swerveDrive.getHeading().getRadians()); + inputs.gyroPitch = pigeon.getPitch(); + inputs.gyroRoll = pigeon.getRoll(); + inputs.gyroRate = swerveDrive.getGyroRate(); + inputs.isConnected = pigeon.getPigeon2().getUpTime().hasUpdated(); + inputs.poseMeters = swerveDrive.getPoseMeters(); + inputs.pigeonTemp = pigeon.getPigeon2().getTemperature().getValueAsDouble(); + inputs.robotRelSpeed = getRobotRelSpeed(); + inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); + inputs.fieldY = fieldY; + inputs.fieldX = fieldX; + inputs.didZero = didZero; + + for (int i = 0; i < 4; ++i) { + inputs.azimuthVels[i] = azimuths[i].getSelectedSensorVelocity(); + inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent(); + } + BaseStatusSignal.refreshAll( + drive10StatorCurrent, + drive11StatorCurrent, + drive12StatorCurrent, + drive13StatorCurrent, + drive10Velocity, + drive11Velocity, + drive12Velocity, + drive13Velocity); + // inputs.avgDriveCurrent = getAvgDriveCurrent(); + // inputs.avgRearDriveVel = getRearDriveAvgVel(); + + inputs.avgDriveCurrent = + (drive10StatorCurrent.getValueAsDouble() + + drive11StatorCurrent.getValueAsDouble() + + drive12StatorCurrent.getValueAsDouble() + + drive13StatorCurrent.getValueAsDouble()) + / 4; + inputs.avgRearDriveVel = + (Math.abs(drive12Velocity.getValueAsDouble()) + + Math.abs(drive13Velocity.getValueAsDouble())) + / 2; + inputs.avgRearDriveVel = + (Math.abs(drive10Velocity.getValueAsDouble()) + + Math.abs(drive11Velocity.getValueAsDouble())) + / 2; + } + + @Override + public void registerWith(TelemetryService telemetryService) { + swerveDrive.registerWith(telemetryService); + pigeon.registerWith(telemetryService); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java new file mode 100644 index 0000000..ed56aff --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -0,0 +1,336 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.TalonFXS; +import edu.wpi.first.math.Matrix; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.VisionConstants; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.strykeforce.gyro.SF_AHRS; +import org.strykeforce.gyro.SF_PIGEON2; +import org.strykeforce.healthcheck.Checkable; +import org.strykeforce.healthcheck.HealthCheck; +import org.strykeforce.swerve.FXSwerveModule; +import org.strykeforce.swerve.OdometryStrategy; +import org.strykeforce.swerve.PoseEstimatorOdometryStrategy; +import org.strykeforce.swerve.SwerveDrive; +import org.strykeforce.swerve.SwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule.ClosedLoopUnits; +import org.strykeforce.telemetry.TelemetryService; + +public class SwerveFXS implements SwerveIO, Checkable { + @HealthCheck private final SwerveDrive swerveDrive; + + // Grapher stuff + private PoseEstimatorOdometryStrategy odometryStrategy; + + private SF_PIGEON2 pigeon; + private SF_AHRS navx; + private Rotation2d navxOffset = new Rotation2d(); + + private TalonFXSConfigurator configuratorFXS; + private TalonFXConfigurator configurator; + + private TalonFXS[] azimuths = new TalonFXS[4]; + private TalonFX[] drives = new TalonFX[4]; + + private FXSwerveModule[] swerveModules; + private SwerveDriveKinematics kinematics; + private double fieldY = 0.0; + private double fieldX = 0.0; + private boolean didZero = false; + + public SwerveFXS() { + + var moduleBuilder = + new FXSwerveModule.FXBuilder() + .driveGearRatio(DriveConstants.kDriveGearRatio) + .wheelDiameterInches(DriveConstants.kWheelDiameterInches) + .driveMaximumMetersPerSecond(DriveConstants.kMaxSpeedMetersPerSecond) + .latencyCompensation(true) + .encoderOpposed(false); + swerveModules = new FXSwerveModule[4]; + Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters(); + didZero = true; + for (int i = 0; i < 4; i++) { + var azimuthFXS = new TalonFXS(i, "*"); + configuratorFXS = azimuthFXS.getConfigurator(); + configuratorFXS.apply(new TalonFXSConfiguration()); // factory default + configuratorFXS.apply(DriveConstants.getAzimuthFXSConfig()); + + azimuthFXS.getRawPulseWidthPosition().setUpdateFrequency(200); + + azimuths[i] = azimuthFXS; + + var driveTalon = new TalonFX(i + 10, "*"); + configurator = driveTalon.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); // factory default + configurator.apply(DriveConstants.getDriveTalonConfig()); + drives[i] = driveTalon; + + swerveModules[i] = + moduleBuilder + .azimuthTalon(azimuthFXS) + .driveTalon(driveTalon) + .wheelLocationMeters(wheelLocations[i]) + .closedLoopUnits(ClosedLoopUnits.VOLTAGE) + .build(); + boolean zeroCheck = swerveModules[i].zeroAndCheck(); + didZero = zeroCheck && didZero; + } + + pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "*"); + pigeon.applyConfig(DriveConstants.getPigeon2Configuration()); + navx = new SF_AHRS(); + swerveDrive = new SwerveDrive(false, 0.02, pigeon, swerveModules); + swerveDrive.resetGyro(); + swerveDrive.setGyroOffset(Rotation2d.fromDegrees(0)); + + kinematics = swerveDrive.getKinematics(); + + odometryStrategy = + new PoseEstimatorOdometryStrategy( + swerveDrive.getHeading(), + new Pose2d(), + swerveDrive.getKinematics(), + VisionConstants.kStateStdDevs, + VisionConstants.kLocalMeasurementStdDevs, + VisionConstants.kVisionMeasurementStdDevs, + getSwerveModulePositions()); + + swerveDrive.setOdometry(odometryStrategy); + } + + // Getters/Setter + @Override + public String getName() { + return "Swerve"; + } + + @Override + public void zeroModules() { + didZero = true; + for (int i = 0; i < 4; i++) { + boolean zeroCheck = swerveModules[i].zeroAndCheck(); + didZero = zeroCheck && didZero; + } + } + + private SwerveModule[] getSwerveModules() { + return swerveDrive.getSwerveModules(); + } + + public void setSwerveModuleAngles(Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) { + swerveModules[0].setAzimuthRotation2d(FL); + swerveModules[1].setAzimuthRotation2d(FR); + swerveModules[2].setAzimuthRotation2d(BL); + swerveModules[3].setAzimuthRotation2d(BR); + } + + private SwerveModulePosition[] getSwerveModulePositions() { + SwerveModule[] swerveModules = getSwerveModules(); + SwerveModulePosition[] temp = {null, null, null, null}; + for (int i = 0; i < 4; ++i) { + temp[i] = swerveModules[i].getPosition(); + } + return temp; + } + + public SwerveModuleState[] getSwerveModuleStates() { + V6TalonSwerveModule[] swerveModules = (V6TalonSwerveModule[]) swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return swerveModuleStates; + } + + private ChassisSpeeds getRobotRelSpeed() { + SwerveDriveKinematics kinematics = swerveDrive.getKinematics(); + SwerveModule[] swerveModules = swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; ++i) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return kinematics.toChassisSpeeds(swerveModuleStates); + } + + private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { + Rotation2d heading = swerveDrive.getHeading().unaryMinus(); + fieldX = + roboRelSpeed.vxMetersPerSecond * heading.getCos() + + roboRelSpeed.vyMetersPerSecond * heading.getSin(); + fieldY = + -roboRelSpeed.vxMetersPerSecond * heading.getSin() + + roboRelSpeed.vyMetersPerSecond * heading.getCos(); + + return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond); + } + + public double getAvgDriveCurrent() { + double sum = 0; + + for (int i = 0; i < 4; i++) { + sum += drives[i].getStatorCurrent().getValueAsDouble(); + } + + return sum / 4.0; + } + + public double getFrontDriveAvgVel() { + double sum = 0; + + for (int i = 0; i < 2; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + + public double getRearDriveAvgVel() { + double sum = 0; + + for (int i = 2; i < 4; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + + public Rotation2d getPigeonGyroOffset() { + return swerveDrive.getGyroOffset(); + } + + @Override + public void setOdometry(OdometryStrategy odom) { + swerveDrive.setOdometry(odom); + } + + @Override + public void setPigeonGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + } + + @Override + public void setBothGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + navxOffset = rotation; + } + + public void disableNoMotionCal() { + pigeon.applyConfig(DriveConstants.getPigeon2NoMotionDisabledConfiguration()); + } + + @Override + public void resetGyro() { + swerveDrive.resetGyro(); + navx.reset(); + } + + @Override + public void resetOdometry(Pose2d pose) { + swerveDrive.resetOdometry(pose); + navx.reset(); + } + + @Override + public void addVisionMeasurement(Pose2d pose, double timestamp) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose, timestamp); + } + + @Override + public void addVisionMeasurement(Pose2d pose2d, double timestamp, Matrix stdDevs) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose2d, timestamp, stdDevs); + } + + @Override + public void drive(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.drive(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void setAzimuthVel(double vel) { + for (int i = 0; i < 4; i++) { + azimuths[i].set(vel); + } + } + + @Override + public void configDriveCurrents(CurrentLimitsConfigs config) { + for (int i = 0; i < 4; i++) { + drives[i].getConfigurator().apply(config); + } + } + + @Override + public void prepForAuto(Pose2d pose2d, double offsetDegrees, Alliance alliance) { + double gyroResetDegs = alliance == Alliance.Blue ? 0.0 : 180.0; + setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); + resetGyro(); + setBothGyroOffset(Rotation2d.fromDegrees(offsetDegrees)); + resetOdometry(pose2d); + } + + @Override + public void updateInputs(SwerveIOInputs inputs) { + swerveDrive.updateInputs(); // Call before swerveDrive.periodic() + swerveDrive.periodic(); + + inputs.odometryX = swerveDrive.getPoseMeters().getX(); + inputs.odometryY = swerveDrive.getPoseMeters().getY(); + inputs.swervePose = odometryStrategy.getPoseMeters(); + inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees(); + inputs.gyroRotation2d = swerveDrive.getHeading(); + inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset); + inputs.normalizedGyroRotation = + FastMath.normalizeMinusPiPi(swerveDrive.getHeading().getRadians()); + inputs.gyroPitch = pigeon.getPitch(); + inputs.gyroRoll = pigeon.getRoll(); + inputs.gyroRate = swerveDrive.getGyroRate(); + inputs.isConnected = pigeon.getPigeon2().getUpTime().hasUpdated(); + inputs.poseMeters = swerveDrive.getPoseMeters(); + inputs.pigeonTemp = pigeon.getPigeon2().getTemperature().getValueAsDouble(); + for (int i = 0; i < 4; ++i) { + inputs.azimuthVels[i] = azimuths[i].getVelocity().getValueAsDouble(); + inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent().getValueAsDouble(); + } + inputs.avgDriveCurrent = getAvgDriveCurrent(); + inputs.avgRearDriveVel = getRearDriveAvgVel(); + inputs.avgFrontDriveVel = getFrontDriveAvgVel(); + inputs.robotRelSpeed = getRobotRelSpeed(); + inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); + inputs.fieldY = fieldY; + inputs.fieldX = fieldX; + inputs.didZero = didZero; + } + + @Override + public void registerWith(TelemetryService telemetryService) { + swerveDrive.registerWith(telemetryService); + pigeon.registerWith(telemetryService); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java new file mode 100644 index 0000000..384e624 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.swerve.OdometryStrategy; +import org.strykeforce.swerve.SwerveModule; +import org.strykeforce.telemetry.TelemetryService; + +public interface SwerveIO { + + @AutoLog + public static class SwerveIOInputs { + public double odometryX = 0.0; + public double odometryY = 0.0; + public Pose2d swervePose; + public double odometryRotation2D = 0.0; + public Rotation2d gyroRotation2d = new Rotation2d(); + public Rotation2d navxRotation2d = new Rotation2d(); + public double normalizedGyroRotation = 0.0; + public double gyroPitch = 0.0; + public double gyroRoll = 0.0; + public double gyroRate = 0.0; + public boolean isConnected = false; + public Pose2d poseMeters = new Pose2d(); + public double pigeonTemp = 0; + public double fieldX = 0; + public double fieldY = 0; + public ChassisSpeeds robotRelSpeed = new ChassisSpeeds(); + public ChassisSpeeds fieldRelSpeed = new ChassisSpeeds(); + public double[] azimuthVels = {0, 0, 0, 0}; + public double[] azimuthCurrent = {0, 0, 0, 0}; + public double avgDriveCurrent = 0; + public double avgRearDriveVel = 0; + public double avgFrontDriveVel = 0; + public boolean didZero = false; + } + + private SwerveModule[] getSwerveModules() { + return null; + } + + public default void setSwerveModuleAngles( + Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) {} + + private SwerveModulePosition[] getSwerveModulePositions() { + return null; + } + + private SwerveModuleState[] getSwerveModuleStates() { + return null; + } + + private ChassisSpeeds getRobotRelSpeed() { + return null; + } + + public default SwerveDriveKinematics getKinematics() { + return null; + } + + public default Rotation2d getPigeonGyroOffset() { + return null; + } + + public default void setDriveCoast(boolean coast) {} + + public default void setOdometry(OdometryStrategy Odom) {} + + public default void setPigeonGyroOffset(Rotation2d rotation) {} + + public default void setBothGyroOffset(Rotation2d rotation) {} + + public default void resetGyro() {} + + public default void updateSwerve() {} + + public default void resetOdometry(Pose2d pose) {} + + public default void addVisionMeasurement(Pose2d pose, double timestamp) {} + + public default void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDevs) {} + + public default void drive( + double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) {} + + public default void move( + double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) {} + + public default void setAzimuthVel(double vel) {} + + public default void updateInputs(SwerveIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} + + public default void configDriveCurrents(CurrentLimitsConfigs config) {} + + public default BooleanSupplier getAzimuth1FwdLimitSwitch() { + return () -> false; + } + + public default void zeroModules() {} + + public default void prepForAuto(Pose2d pose2d, double offsetDegrees, Alliance alliance) {} +} diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java new file mode 100644 index 0000000..694f6d0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.laser; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class LaserSubsystem { + public LaserSubsystem(){ + } +public Rotation2d getLaserPos(){ + return null; +} + +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java index 3a114a4..fcd31cc 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -2,20 +2,32 @@ import edu.wpi.first.math.geometry.Pose2d; 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.math.Matrix; + +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; public class TurretSubsystem { + private MotionMagicVoltage positionMain = + new MotionMagicVoltage(0); //Update MotionMagic constants + private Follower positionFollower = + new Follower(0, false); public TurretSubsystem(){ - + } //Stubbed functions until the turret subsystem is done. public Rotation2d getGyroRotation2d(){ return null; } -public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDev ){ +public void zeroTurret(){ +} + +public boolean turretOnTarget(){ + return false; +} + +public void goToPos(Pose2d target){ } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 580593b..e7a71e7 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -487,7 +487,7 @@ public void periodic() { stdMatrix.set(2, 0, VisionConstants.kTrustYawStdDev); } - turretSubsystem.addVisionMeasurement( + robotState.addVisionMeasurement( robotPose, result.getTimeStamp() / 1_000_000.0, stdMatrix); stdMatrix.set(2, 0, VisionConstants.kIgnoreYawStdDev); } From 3640753ec169a1dcae1d7371e429cec87b33b907 Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 2 Sep 2025 19:57:36 -0400 Subject: [PATCH 04/11] Finished motion magic configs and almost finished turret's IOFX --- build.gradle | 1 + .../frc/robot/constants/RobotConstants.java | 101 +++++++++--------- .../frc/robot/constants/TurretConstants.java | 68 ++++++++++-- .../subsystems/drive/DriveSubsystem.java | 27 +++-- .../subsystems/laser/LaserSubsystem.java | 9 +- .../subsystems/turret/TurretSubsystem.java | 33 ------ .../subsystems/turret/TurretSubsystemIO.java | 17 +++ .../turret/TurretSubsystemIOFX.java | 50 +++++++++ .../subsystems/vision/VisionSubsystem.java | 31 +++--- 9 files changed, 208 insertions(+), 129 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java create mode 100644 src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java diff --git a/build.gradle b/build.gradle index 855dbc7..ec0b1b8 100644 --- a/build.gradle +++ b/build.gradle @@ -82,6 +82,7 @@ dependencies { implementation("com.opencsv:opencsv:5.6") implementation 'org.strykeforce:wallEYE:25.1.0' implementation 'net.jafama:jafama:2.3.2' //fastMath + implementation 'ch.qos.logback:logback-classic:1.3.5' } test { diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 1bba738..5afce1e 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -20,7 +20,6 @@ import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.RobotController; import org.slf4j.LoggerFactory; public class RobotConstants { @@ -90,55 +89,55 @@ public class RobotConstants { public static final int kMaxAutoSwitchID = 9; public RobotConstants() { - // Proto constants - kElevatorFunnelSetpoint = ProtoConstants.kElevatorFunnelSetpoint; - kElevatorL1LoadSetpoint = ProtoConstants.kElevatorL1LoadSetpoint; - kElevatorStowSetpoint = ProtoConstants.kElevatorStowSetpoint; - talonFXSConfig = ProtoConstants.getFXSConfig(); - algaeMotionConfig = ProtoConstants.getAlgaeMotionConfig(); - algaeRemovalMotionConfig = algaeMotionConfig; - noAlgaeMotionConfig = ProtoConstants.getNoAlgaeMotionConfig(); - kTicksPerRot = 160; - logger.info("Using Proto Constants"); - - // Biscuit - - kBiscuitZero = ProtoConstants.kZero; - kSafeToStowUpper = ProtoConstants.kSafeToStowUpper; - kSafeToStowLower = ProtoConstants.kSafeToStowLower; - - // Setpoints - // Idle - kStowSetpoint = ProtoConstants.kBiscuitStowSetpoint; - kFunnelSetpoint = ProtoConstants.kFunnelSetpoint; - kL1CoralLoadSetpoint = ProtoConstants.kL1CoralLoadSetpoint; - kPrestageSetpoint = ProtoConstants.kPrestageSetpoint; - kPrestageAlgaeSetpoint = ProtoConstants.kPrestageAlgaeSetpoint; - - // Algae removal - kL2AlgaeSetpoint = ProtoConstants.kL2AlgaeSetpoint; - kL3AlgaeSetpoint = ProtoConstants.kL3AlgaeSetpoint; - - kL2AlgaeRemovalSetpoint = ProtoConstants.kL2AlgaeRemovalSetpoint; - kL3AlgaeRemovalSetpoint = ProtoConstants.kL3AlgaeRemovalSetpoint; - - // Coral score - kL1CoralSetpoint = ProtoConstants.kL1CoralSetpoint; - kL2CoralSetpoint = ProtoConstants.kL2CoralSetpoint; - kL3CoralSetpoint = ProtoConstants.kL3CoralSetpoint; - kL4CoralSetpoint = ProtoConstants.kL4CoralSetpoint; - - // Algae obtaining - kFloorAlgaeSetpoint = ProtoConstants.kFloorAlgaeSetpoint; - kMicAlgaeSetpoint = ProtoConstants.kMicAlgaeSetpoint; - kHpAlgaeSetpoint = ProtoConstants.kHpAlgaeSetpoint; - - // Algae scoring - kProcessorSetpoint = ProtoConstants.kProcessorSetpoint; - kBargeSetpoint = ProtoConstants.kBargeSetpoint; - // kBargeBackwardSetpoint = ProtoConstants.kBargeBackwardSetpoint; - - kTagAlignThreshold = ProtoConstants.kTagAlignThreshold; + // Proto constants + kElevatorFunnelSetpoint = ProtoConstants.kElevatorFunnelSetpoint; + kElevatorL1LoadSetpoint = ProtoConstants.kElevatorL1LoadSetpoint; + kElevatorStowSetpoint = ProtoConstants.kElevatorStowSetpoint; + talonFXSConfig = ProtoConstants.getFXSConfig(); + algaeMotionConfig = ProtoConstants.getAlgaeMotionConfig(); + algaeRemovalMotionConfig = algaeMotionConfig; + noAlgaeMotionConfig = ProtoConstants.getNoAlgaeMotionConfig(); + kTicksPerRot = 160; + logger.info("Using Proto Constants"); + + // Biscuit + + kBiscuitZero = ProtoConstants.kZero; + kSafeToStowUpper = ProtoConstants.kSafeToStowUpper; + kSafeToStowLower = ProtoConstants.kSafeToStowLower; + + // Setpoints + // Idle + kStowSetpoint = ProtoConstants.kBiscuitStowSetpoint; + kFunnelSetpoint = ProtoConstants.kFunnelSetpoint; + kL1CoralLoadSetpoint = ProtoConstants.kL1CoralLoadSetpoint; + kPrestageSetpoint = ProtoConstants.kPrestageSetpoint; + kPrestageAlgaeSetpoint = ProtoConstants.kPrestageAlgaeSetpoint; + + // Algae removal + kL2AlgaeSetpoint = ProtoConstants.kL2AlgaeSetpoint; + kL3AlgaeSetpoint = ProtoConstants.kL3AlgaeSetpoint; + + kL2AlgaeRemovalSetpoint = ProtoConstants.kL2AlgaeRemovalSetpoint; + kL3AlgaeRemovalSetpoint = ProtoConstants.kL3AlgaeRemovalSetpoint; + + // Coral score + kL1CoralSetpoint = ProtoConstants.kL1CoralSetpoint; + kL2CoralSetpoint = ProtoConstants.kL2CoralSetpoint; + kL3CoralSetpoint = ProtoConstants.kL3CoralSetpoint; + kL4CoralSetpoint = ProtoConstants.kL4CoralSetpoint; + + // Algae obtaining + kFloorAlgaeSetpoint = ProtoConstants.kFloorAlgaeSetpoint; + kMicAlgaeSetpoint = ProtoConstants.kMicAlgaeSetpoint; + kHpAlgaeSetpoint = ProtoConstants.kHpAlgaeSetpoint; + + // Algae scoring + kProcessorSetpoint = ProtoConstants.kProcessorSetpoint; + kBargeSetpoint = ProtoConstants.kBargeSetpoint; + // kBargeBackwardSetpoint = ProtoConstants.kBargeBackwardSetpoint; + + kTagAlignThreshold = ProtoConstants.kTagAlignThreshold; } public static class ProtoConstants { @@ -295,4 +294,4 @@ public static TalonFXSConfiguration getFXSConfig() { return fxsConfig; } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index 90e7a13..261cf44 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -1,21 +1,76 @@ package frc.robot.constants; -import edu.wpi.first.units.measure.Angle; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.Angle; public class TurretConstants { -public static final Angle leftTurretLimit = null; -public static final Angle rightTurretLimit = null; + public static final Angle leftTurretLimit = null; + public static final Angle rightTurretLimit = null; + + public static final int kTurretFx = 0; + + public static final double kFxForwardMax = 0; + public static final double kFxReverseMax = 0; -public static TalonFXConfiguration turretFXConfig() { + public static TalonFXConfiguration turretFXConfig() { TalonFXConfiguration fxConfig = new TalonFXConfiguration(); - MotionMagicConfigs motionMagic = + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(70) + .withSupplyCurrentLowerLimit(10) + .withSupplyCurrentLowerTime(2); + fxConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kFxForwardMax) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kFxReverseMax); + fxConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(2) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0.36) + .withKS(0) + .withKV(0.13) + .withKA(0); + fxConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = new MotionMagicConfigs() .withMotionMagicCruiseVelocity(70) .withMotionMagicAcceleration(300) // was 300 @@ -29,6 +84,5 @@ public static TalonFXConfiguration turretFXConfig() { .withInverted(InvertedValue.CounterClockwise_Positive); fxConfig.MotorOutput = motorOut; return fxConfig; + } } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 183f7df..e7d1d94 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -11,10 +11,9 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.DriveConstants; -import frc.robot.subsystems.robotState.RobotStateSubsystem; +// import frc.robot.subsystems.robotState.RobotStateSubsystem; import java.util.Set; import java.util.function.BooleanSupplier; import net.jafama.FastMath; @@ -46,7 +45,7 @@ public class DriveSubsystem extends MeasurableSubsystem { private boolean ignoreSticks = false; - private RobotStateSubsystem robotStateSubsystem; + // private RobotStateSubsystem robotStateSubsystem; public DriveSubsystem(SwerveIO io) { this.io = io; @@ -233,16 +232,16 @@ public void prepClimb() { Rotation2d.fromDegrees(90)); } - public void teleResetGyro() { - setAutoDebugMsg("Reset Gyro"); - logger.info("Driver Joystick: Reset Gyro"); - double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0; - io.setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); - Logger.recordOutput("DriveSubsystem/gyroOffset", Rotation2d.fromDegrees(gyroResetDegs)); - io.resetGyro(); - io.resetOdometry( - new Pose2d(inputs.poseMeters.getTranslation(), Rotation2d.fromDegrees(gyroResetDegs))); - } + // public void teleResetGyro() { + // setAutoDebugMsg("Reset Gyro"); + // logger.info("Driver Joystick: Reset Gyro"); + // double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0; + // io.setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs)); + // Logger.recordOutput("DriveSubsystem/gyroOffset", Rotation2d.fromDegrees(gyroResetDegs)); + // io.resetGyro(); + // io.resetOdometry( + // new Pose2d(inputs.poseMeters.getTranslation(), Rotation2d.fromDegrees(gyroResetDegs))); + // } // Make whether a trajectory is currently active obvious on grapher public void grapherTrajectoryActive(boolean active) { @@ -252,7 +251,7 @@ public void grapherTrajectoryActive(boolean active) { // Field flipping stuff public boolean shouldFlip() { - return robotStateSubsystem.getAllianceColor() == Alliance.Red; + return true; // robotStateSubsystem.getAllianceColor() == Alliance.Red; } public Translation2d apply(Translation2d translation) { diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index 694f6d0..b42d91b 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -3,10 +3,9 @@ import edu.wpi.first.math.geometry.Rotation2d; public class LaserSubsystem { - public LaserSubsystem(){ - } -public Rotation2d getLaserPos(){ + public LaserSubsystem() {} + + public Rotation2d getLaserPos() { return null; -} - + } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java deleted file mode 100644 index fcd31cc..0000000 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.subsystems.turret; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; - -public class TurretSubsystem { - private MotionMagicVoltage positionMain = - new MotionMagicVoltage(0); //Update MotionMagic constants - private Follower positionFollower = - new Follower(0, false); - public TurretSubsystem(){ - - } -//Stubbed functions until the turret subsystem is done. -public Rotation2d getGyroRotation2d(){ - return null; -} - -public void zeroTurret(){ -} - -public boolean turretOnTarget(){ - return false; -} - -public void goToPos(Pose2d target){ -} - -} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java new file mode 100644 index 0000000..99b380d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.turret; + +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +public interface TurretSubsystemIO { + + @AutoLog + public class TurretIOInputs { + public double position = 0.0; + public double velocity = 0.0; + } + + public default void updateInputs(TurretIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java new file mode 100644 index 0000000..6368eaa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java @@ -0,0 +1,50 @@ +package frc.robot.subsystems.turret; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.TurretConstants; +import frc.robot.subsystems.turret.TurretSubsystemIO.TurretIOInputs; +import org.strykeforce.telemetry.TelemetryService; + +public class TurretSubsystemIOFX implements TurretSubsystemIO { + + private TalonFX talonFX; + private TalonFXConfigurator talonConfigurator; + + StatusSignal curPosition; + StatusSignal curVelocity; + + private MotionMagicVoltage positionMain = + // Update MotionMagic constants + new MotionMagicVoltage(0).withEnableFOC(false).withSlot(0); + + public TurretSubsystemIOFX() { + talonFX = new TalonFX(TurretConstants.kTurretFx); + talonConfigurator = talonFX.getConfigurator(); + talonConfigurator.apply(TurretConstants.turretFXConfig()); + + curPosition = talonFX.getPosition(); + curVelocity = talonFX.getVelocity(); + } + + public void zeroTurret() {} + + public void setPosition(Angle position) { + talonFX.setControl(positionMain.withPosition(position)); + } + + @Override + public void updateInputs(TurretIOInputs inputs) { + BaseStatusSignal.refreshAll(curPosition, curVelocity); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFX, true); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index e7a71e7..acb7cb4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -11,15 +11,12 @@ 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.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.*; import edu.wpi.first.util.CircularBuffer; import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.VisionConstants; -//import frc.robot.subsystems.drive.DriveSubsystem; -import java.io.IOException; +import frc.robot.subsystems.turret.TurretSubsystemIOFX; import java.util.ArrayList; import java.util.Set; import net.jafama.FastMath; @@ -28,7 +25,6 @@ import org.strykeforce.telemetry.TelemetryService; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; -import frc.robot.subsystems.turret.TurretSubsystem; public class VisionSubsystem extends MeasurableSubsystem { @@ -43,9 +39,7 @@ public class VisionSubsystem extends MeasurableSubsystem { }; private double[] camHeights = { - camPositions[0].getZ(), - camPositions[1].getZ(), - camPositions[2].getZ(), + camPositions[0].getZ(), camPositions[1].getZ(), camPositions[2].getZ(), }; // Array of camera rotations @@ -57,14 +51,12 @@ public class VisionSubsystem extends MeasurableSubsystem { // Array of camera names private String[] camNames = { - VisionConstants.kCam1Name, - VisionConstants.kCam2Name, - VisionConstants.kCam3Name, + VisionConstants.kCam1Name, VisionConstants.kCam2Name, VisionConstants.kCam3Name, }; private int trustedCameraYawIdx = -1; - //private DriveSubsystem driveSubsystem; + // private DriveSubsystem driveSubsystem; /*Because we use two seperate loggers we can import one and then define the other here.*/ private org.slf4j.Logger textLogger; @@ -81,14 +73,14 @@ public class VisionSubsystem extends MeasurableSubsystem { private WallEyeTagResult[] lastResult = new WallEyeTagResult[VisionConstants.kNumCams]; private Matrix adativeMatrix; private Matrix stdMatrix; - private TurretSubsystem turretSubsystem; + private TurretSubsystemIOFX turretSubsystem; private boolean[] acceptUpdates = new boolean[VisionConstants.kNumCams]; private boolean ignoreRearCams = false; private boolean isAuto = false; - public VisionSubsystem(TurretSubsystem turretSubsystem) { + public VisionSubsystem(TurretSubsystemIOFX turretSubsystem) { this.turretSubsystem = turretSubsystem; - //this.driveSubsystem = driveSubsystem; + // this.driveSubsystem = driveSubsystem; textLogger = LoggerFactory.getLogger("Vision"); cams = new WallEyeCam[VisionConstants.kNumCams]; @@ -98,7 +90,7 @@ public VisionSubsystem(TurretSubsystem turretSubsystem) { // We then copy the adaptive matrix because they will differ later stdMatrix = adaptiveMatrix.copy(); // I'm not sure why we put this in a try catch maybe could be removed - field = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + field = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); // Fill our camera array for (int i = 0; i < VisionConstants.kNumCams; i++) { cams[i] = new WallEyeCam(camNames[i], -1); @@ -384,7 +376,8 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn @Override public void periodic() { Logger.recordOutput("Vision/Vision Updates On", visionUpdating); - double gyroData = FastMath.normalizeMinusPiPi(turretSubsystem.getGyroRotation2d().getRadians()); + double gyroData = 0.0; + // FastMath.normalizeMinusPiPi(turretSubsystem.getGyroRotation2d().getRadians()); gyroBuffer.addFirst(gyroData); Logger.recordOutput("Vision/Gyro Buffer", gyroData); @@ -487,8 +480,8 @@ public void periodic() { stdMatrix.set(2, 0, VisionConstants.kTrustYawStdDev); } - robotState.addVisionMeasurement( - robotPose, result.getTimeStamp() / 1_000_000.0, stdMatrix); + // robotState.addVisionMeasurement( + // robotPose, result.getTimeStamp() / 1_000_000.0, stdMatrix); stdMatrix.set(2, 0, VisionConstants.kIgnoreYawStdDev); } } else { From e8e4cc40b87c73a15cf8df676e31391db8b3882c Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 16 Sep 2025 20:08:15 -0400 Subject: [PATCH 05/11] Added chinese reminder therom --- .../frc/robot/constants/TurretConstants.java | 20 +++++- .../subsystems/turret/TurretSubsystem.java | 71 +++++++++++++++++++ .../subsystems/turret/TurretSubsystemIO.java | 5 ++ .../turret/TurretSubsystemIOFX.java | 10 ++- 4 files changed, 102 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index 261cf44..3486858 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -17,14 +17,28 @@ import edu.wpi.first.units.measure.Angle; public class TurretConstants { - - public static final Angle leftTurretLimit = null; - public static final Angle rightTurretLimit = null; + // TODO Add real constants + public static final Angle kLeftTurretLimit = null; + public static final Angle kRightTurretLimit = null; + public static final double kTurretCloseEnough = 0; public static final int kTurretFx = 0; + public static final int kCanCoder1Id = 1; + public static final int kCanCoder2Id = 2; + public static final double kCanCoder1Gr = 20 / 150; + public static final double kCanCoder2Gr = 19 / 150; + public static final double kBigGr = 1 / 100; + + public static final double kCanCoder1Zero = 0; + public static final double kCanCoder2Zero = 0; public static final double kFxForwardMax = 0; public static final double kFxReverseMax = 0; + // List of gear rations gr, 2gr, 3gr and so on + public static final double[] kCanCoder1PosRot = {}; + public static final double[] kCanCoder2PosRot = {}; + + public static final double kZeroDifferenceTol = 100; public static TalonFXConfiguration turretFXConfig() { TalonFXConfiguration fxConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java new file mode 100644 index 0000000..6cd5921 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems.turret; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.TurretConstants; +import java.util.ArrayList; +import java.util.Set; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class TurretSubsystem extends MeasurableSubsystem { + + private TurretSubsystemIO io; + private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private Angle setPoint; + private ArrayList possiblePos1 = new ArrayList(); + private ArrayList possiblePos2 = new ArrayList(); + + public TurretSubsystem() { + this.io = io; + } + + public void setPosition(Angle setPos) { + setPoint = setPos; + io.setPosition(setPoint); + } + + public Angle getPosition() { + return Rotations.of(inputs.position); + } + + private boolean withInTolerence(double pos1, double pos2) { + return (pos1 - pos2) < TurretConstants.kZeroDifferenceTol; + } + + public double zero() { + + for (int i = 0; i < 8; i++) { + possiblePos1.add( + ((inputs.canCoder1Pos.abs(Rotations) - TurretConstants.kCanCoder1Zero)) + * TurretConstants.kCanCoder1Gr + + TurretConstants.kCanCoder1PosRot[i]); + } + for (int i = 0; i < 8; i++) { + possiblePos1.add( + (inputs.canCoder2Pos.abs(Rotations) - TurretConstants.kCanCoder2Zero) + * TurretConstants.kCanCoder2Gr + + TurretConstants.kCanCoder2PosRot[i]); + } + + for (int i = 0; i < 8; i++) { + for (int j = 0; j < 8; j++) { + if (withInTolerence(possiblePos1.get(i), possiblePos2.get(j))) { + return possiblePos1.get(i); + } + } + } + return 2767; + } + + public boolean isFinished() { + return Math.abs(getPosition().minus(setPoint).in(Rotations)) + < TurretConstants.kTurretCloseEnough; + } + + @Override + public Set getMeasures() { + return null; + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java index 99b380d..dec8f96 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIO.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.turret; +import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; @@ -9,9 +10,13 @@ public interface TurretSubsystemIO { public class TurretIOInputs { public double position = 0.0; public double velocity = 0.0; + public Angle canCoder1Pos; + public Angle canCoder2Pos; } public default void updateInputs(TurretIOInputs inputs) {} public default void registerWith(TelemetryService telemetryService) {} + + public default void setPosition(Angle setPos) {} } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java index 6368eaa..ef67377 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -15,6 +16,8 @@ public class TurretSubsystemIOFX implements TurretSubsystemIO { private TalonFX talonFX; private TalonFXConfigurator talonConfigurator; + private CANcoder canCoder1; // left + private CANcoder canCoder2; // right StatusSignal curPosition; StatusSignal curVelocity; @@ -27,6 +30,8 @@ public TurretSubsystemIOFX() { talonFX = new TalonFX(TurretConstants.kTurretFx); talonConfigurator = talonFX.getConfigurator(); talonConfigurator.apply(TurretConstants.turretFXConfig()); + canCoder1 = new CANcoder(TurretConstants.kCanCoder1Id); + canCoder2 = new CANcoder(TurretConstants.kCanCoder2Id); curPosition = talonFX.getPosition(); curVelocity = talonFX.getVelocity(); @@ -40,11 +45,14 @@ public void setPosition(Angle position) { @Override public void updateInputs(TurretIOInputs inputs) { - BaseStatusSignal.refreshAll(curPosition, curVelocity); + BaseStatusSignal.refreshAll( + curPosition, curVelocity, canCoder1.getPosition(), canCoder2.getPosition()); } @Override public void registerWith(TelemetryService telemetryService) { telemetryService.register(talonFX, true); + telemetryService.register(canCoder1); + telemetryService.register(canCoder2); } } From 5b5d69615486a68950471f89e61acbbf1ee770b5 Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 23 Sep 2025 19:57:04 -0400 Subject: [PATCH 06/11] Finished Turretsubsystem. Made TODO list in LaserSubsystem and VisionSubsystem. Started the dual buffers. --- .../frc/robot/constants/TurretConstants.java | 6 ++--- .../subsystems/laser/LaserSubsystem.java | 1 + .../subsystems/turret/TurretSubsystem.java | 8 ++++-- .../turret/TurretSubsystemIOFX.java | 2 +- .../subsystems/vision/VisionSubsystem.java | 27 ++++++++++--------- 5 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index 3486858..a0e1d75 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -22,9 +22,9 @@ public class TurretConstants { public static final Angle kRightTurretLimit = null; public static final double kTurretCloseEnough = 0; - public static final int kTurretFx = 0; - public static final int kCanCoder1Id = 1; - public static final int kCanCoder2Id = 2; + public static final int kTurretFxId = 0; + public static final int kCanCoder1Id = 21; + public static final int kCanCoder2Id = 22; public static final double kCanCoder1Gr = 20 / 150; public static final double kCanCoder2Gr = 19 / 150; public static final double kBigGr = 1 / 100; diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index b42d91b..c79ee09 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -6,6 +6,7 @@ public class LaserSubsystem { public LaserSubsystem() {} public Rotation2d getLaserPos() { + //TODO See whiteboard return null; } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java index 6cd5921..e8b40e8 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -24,6 +24,7 @@ public TurretSubsystem() { public void setPosition(Angle setPos) { setPoint = setPos; io.setPosition(setPoint); + } public Angle getPosition() { @@ -31,7 +32,7 @@ public Angle getPosition() { } private boolean withInTolerence(double pos1, double pos2) { - return (pos1 - pos2) < TurretConstants.kZeroDifferenceTol; + return Math.abs(pos1 - pos2) < TurretConstants.kZeroDifferenceTol; } public double zero() { @@ -66,6 +67,9 @@ public boolean isFinished() { @Override public Set getMeasures() { - return null; + return Set.of( + new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), + new Measure("Turret Setpoint", () -> setPoint.in(Rotations)), + new Measure("Turret Zero Point", () -> zero())); } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java index ef67377..7c656b7 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystemIOFX.java @@ -27,7 +27,7 @@ public class TurretSubsystemIOFX implements TurretSubsystemIO { new MotionMagicVoltage(0).withEnableFOC(false).withSlot(0); public TurretSubsystemIOFX() { - talonFX = new TalonFX(TurretConstants.kTurretFx); + talonFX = new TalonFX(TurretConstants.kTurretFxId); talonConfigurator = talonFX.getConfigurator(); talonConfigurator.apply(TurretConstants.turretFXConfig()); canCoder1 = new CANcoder(TurretConstants.kCanCoder1Id); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index acb7cb4..47d3c51 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -16,7 +16,7 @@ import edu.wpi.first.util.CircularBuffer; import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.VisionConstants; -import frc.robot.subsystems.turret.TurretSubsystemIOFX; +import frc.robot.subsystems.turret.TurretSubsystem; import java.util.ArrayList; import java.util.Set; import net.jafama.FastMath; @@ -25,6 +25,9 @@ import org.strykeforce.telemetry.TelemetryService; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; +import frc.robot.subsystems.drive.DriveSubsystem; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.geometry.Translation2d; public class VisionSubsystem extends MeasurableSubsystem { @@ -66,6 +69,8 @@ public class VisionSubsystem extends MeasurableSubsystem { private int minTags; private CircularBuffer gyroBuffer = new CircularBuffer(VisionConstants.kCircularBufferSize); + private CircularBuffer turretBuffer = + new CircularBuffer(VisionConstants.kCircularBufferSize); private double timeSinceLastUpdate; private int updatesToWheels; private Matrix adaptiveMatrix; @@ -73,14 +78,15 @@ public class VisionSubsystem extends MeasurableSubsystem { private WallEyeTagResult[] lastResult = new WallEyeTagResult[VisionConstants.kNumCams]; private Matrix adativeMatrix; private Matrix stdMatrix; - private TurretSubsystemIOFX turretSubsystem; + private TurretSubsystem turretSubsystem; private boolean[] acceptUpdates = new boolean[VisionConstants.kNumCams]; private boolean ignoreRearCams = false; private boolean isAuto = false; + private DriveSubsystem driveSubsystem; - public VisionSubsystem(TurretSubsystemIOFX turretSubsystem) { + public VisionSubsystem(TurretSubsystem turretSubsystem) { this.turretSubsystem = turretSubsystem; - // this.driveSubsystem = driveSubsystem; + this.driveSubsystem = driveSubsystem; textLogger = LoggerFactory.getLogger("Vision"); cams = new WallEyeCam[VisionConstants.kNumCams]; @@ -192,7 +198,6 @@ private double averageTagDistance(WallEyePoseResult result) { } // Filters - /* Commented until a Drive Subsystem private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) { ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed(); @@ -204,16 +209,12 @@ private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) Math.sqrt(FastMath.pow2(vel.vxMetersPerSecond) + FastMath.pow2(vel.vyMetersPerSecond)); double dispMagnitude = Math.sqrt(FastMath.pow2(disp.getX()) + FastMath.pow2(disp.getY())); - - /*This gets our displacement and compares it to who much we could - have moved.It does this by getting the velocity and plotting it on a - graph. The graph will be in the docs. return result.getNumTags() >= minTags && dispMagnitude <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter + VisionConstants.kOffsetOnVelFilter + FastMath.pow2(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter)); - }*/ + } private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) { return (result.getNumTags() >= 2 || result.getAmbiguity() < VisionConstants.kMaxAmbig) @@ -362,6 +363,7 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn } // See what pose is closer the the gyro at the time of the photo's capture. + //TODO If Birds nest camera then user turret plus gyro buffer. Else use gyro buffer. double rotation = gyroBuffer.get( FastMath.floorToInt( @@ -376,9 +378,10 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn @Override public void periodic() { Logger.recordOutput("Vision/Vision Updates On", visionUpdating); - double gyroData = 0.0; - // FastMath.normalizeMinusPiPi(turretSubsystem.getGyroRotation2d().getRadians()); + double gyroData = FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians()); gyroBuffer.addFirst(gyroData); + double turretData = FastMath.normalizeMinusPiPi(turretSubsystem.getPosition().in(Radians)); + turretBuffer.addFirst(turretData); Logger.recordOutput("Vision/Gyro Buffer", gyroData); From 348bbd822c6d4e0e26f0b9c91cdb89f2f6fc3336 Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 23 Sep 2025 19:58:09 -0400 Subject: [PATCH 07/11] Finished Turretsubsystem. Made TODO list in LaserSubsystem and VisionSubsystem. Started the dual buffers. --- .../frc/robot/subsystems/laser/LaserSubsystem.java | 2 +- .../frc/robot/subsystems/turret/TurretSubsystem.java | 7 +++---- .../frc/robot/subsystems/vision/VisionSubsystem.java | 10 +++++----- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index c79ee09..2d7f241 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -6,7 +6,7 @@ public class LaserSubsystem { public LaserSubsystem() {} public Rotation2d getLaserPos() { - //TODO See whiteboard + // TODO See whiteboard return null; } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java index e8b40e8..7997ed9 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -24,7 +24,6 @@ public TurretSubsystem() { public void setPosition(Angle setPos) { setPoint = setPos; io.setPosition(setPoint); - } public Angle getPosition() { @@ -68,8 +67,8 @@ public boolean isFinished() { @Override public Set getMeasures() { return Set.of( - new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), - new Measure("Turret Setpoint", () -> setPoint.in(Rotations)), - new Measure("Turret Zero Point", () -> zero())); + new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), + new Measure("Turret Setpoint", () -> setPoint.in(Rotations)), + new Measure("Turret Zero Point", () -> zero())); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 47d3c51..5952212 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -11,11 +11,14 @@ 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.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.*; import edu.wpi.first.util.CircularBuffer; import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.turret.TurretSubsystem; import java.util.ArrayList; import java.util.Set; @@ -25,9 +28,6 @@ import org.strykeforce.telemetry.TelemetryService; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; -import frc.robot.subsystems.drive.DriveSubsystem; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.geometry.Translation2d; public class VisionSubsystem extends MeasurableSubsystem { @@ -69,7 +69,7 @@ public class VisionSubsystem extends MeasurableSubsystem { private int minTags; private CircularBuffer gyroBuffer = new CircularBuffer(VisionConstants.kCircularBufferSize); - private CircularBuffer turretBuffer = + private CircularBuffer turretBuffer = new CircularBuffer(VisionConstants.kCircularBufferSize); private double timeSinceLastUpdate; private int updatesToWheels; @@ -363,7 +363,7 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn } // See what pose is closer the the gyro at the time of the photo's capture. - //TODO If Birds nest camera then user turret plus gyro buffer. Else use gyro buffer. + // TODO If Birds nest camera then user turret plus gyro buffer. Else use gyro buffer. double rotation = gyroBuffer.get( FastMath.floorToInt( From 65a37c059044613edd5901ba8ed71e361621ade3 Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 30 Sep 2025 19:57:19 -0400 Subject: [PATCH 08/11] Still need to finish laser subsystem but will work on finishing it next time and hope to start the robot state --- .../frc/robot/constants/LaserConstants.java | 89 +++++++++++++++++++ .../frc/robot/constants/TurretConstants.java | 2 +- .../frc/robot/constants/VisionConstants.java | 12 ++- .../subsystems/laser/LaserSubsystem.java | 43 +++++++-- .../subsystems/laser/LaserSubsystemIO.java | 19 ++++ .../subsystems/laser/LaserSubsystemIOFX.java | 40 +++++++++ .../subsystems/turret/TurretSubsystem.java | 5 +- .../subsystems/vision/VisionSubsystem.java | 36 +++++--- 8 files changed, 220 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/constants/LaserConstants.java create mode 100644 src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java create mode 100644 src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java new file mode 100644 index 0000000..0cd8e61 --- /dev/null +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -0,0 +1,89 @@ +package frc.robot.constants; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; +import edu.wpi.first.units.measure.Angle; + +public class LaserConstants { + public static final Angle kLUpperTurretLimit = null; + public static final Angle kLowerTurretLimit = null; + public static final double kLaserCloseEnough = 100; + + public static final int kLaserFxId = 1; + public static final double kLaserGr = 20 / 150; + public static final double kFxForwardMax = 0; + public static final double kFxReverseMax = 0; + + // TODO Update Talon Constants + public static TalonFXConfiguration turretFXConfig() { + TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(70) + .withSupplyCurrentLowerLimit(10) + .withSupplyCurrentLowerTime(2); + fxConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + fxConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kFxForwardMax) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kFxReverseMax); + fxConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(2) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0.36) + .withKS(0) + .withKV(0.13) + .withKA(0); + fxConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(70) + .withMotionMagicAcceleration(300) // was 300 + .withMotionMagicJerk(1500); + fxConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive); + fxConfig.MotorOutput = motorOut; + return fxConfig; + } +} diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index a0e1d75..ee7dddd 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -20,7 +20,7 @@ public class TurretConstants { // TODO Add real constants public static final Angle kLeftTurretLimit = null; public static final Angle kRightTurretLimit = null; - public static final double kTurretCloseEnough = 0; + public static final double kTurretCloseEnough = 0.001; public static final int kTurretFxId = 0; public static final int kCanCoder1Id = 21; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 697c1ae..4687941 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -67,13 +67,13 @@ public final class VisionConstants { public static final double kCamErrorZThreshold = 0.3; // Constants for cameras - public static final int kNumCams = 5; + public static final int kNumCams = 3; public static final int kNumPis = 3; public static final int[] kUdpIndex = {0, 1, 2}; // Camera Ports public static final int[] kCamPorts = {5802, 5804, 5803, 5804, 5804}; - + // TODO Update Vision Constants // Names public static final String kCam1Name = "Left Servo"; public static final String kCam2Name = "Rear Right"; // when looking out the back @@ -86,11 +86,9 @@ public final class VisionConstants { // public static final String kPi3Name = "Rear"; // Indexs - // public static final int kCam1Idx = 0; - // public static final int kCam2Idx = 0; - // public static final int kCam3Idx = 0; - // public static final int kCam4Idx = 0; - // public static final int kCam5Idx = 2; + public static final int kCam1Idx = 0; + public static final int kCam2Idx = 0; + public static final int kCam3Idx = 0; public static final double kLoopTime = 0.02; public static final int kCircularBufferSize = 1000; diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index 2d7f241..99fd04d 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -1,12 +1,43 @@ package frc.robot.subsystems.laser; -import edu.wpi.first.math.geometry.Rotation2d; +import static edu.wpi.first.units.Units.Rotations; -public class LaserSubsystem { - public LaserSubsystem() {} +import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.LaserConstants; +import java.util.Set; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; - public Rotation2d getLaserPos() { - // TODO See whiteboard - return null; +public class LaserSubsystem extends MeasurableSubsystem { + private LaserSubsystemIO io; + private LaserIOInputsAutoLogged inputs = new LaserIOInputsAutoLogged(); + private Angle setPoint; + + public LaserSubsystem() { + this.io = io; + } + + public Angle getLaserPos() { + return Rotations.of(inputs.position); + } + + public void setPosition(Angle pos) { + setPoint = pos; + io.setPosition(pos); + } + + public void zero() { + // TODO Add kraken encoder and make zero function + } + + public boolean isFinished() { + return Math.abs(getLaserPos().minus(setPoint).in(Rotations)) < LaserConstants.kLaserCloseEnough; + } + + @Override + public Set getMeasures() { + return Set.of( + new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), + new Measure("Turret Setpoint", () -> setPoint.in(Rotations))); } } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java new file mode 100644 index 0000000..98688dc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.laser; + +import edu.wpi.first.units.measure.Angle; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +public interface LaserSubsystemIO { + + @AutoLog + public class LaserIOInputs { + public double position; + } + + public default void updateInputs(LaserIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} + + public default void setPosition(Angle setPos) {} +} diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java new file mode 100644 index 0000000..b62184a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.laser; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.LaserConstants; +import org.strykeforce.telemetry.TelemetryService; + +public class LaserSubsystemIOFX implements LaserSubsystemIO { + + private MotionMagicVoltage motionMagic = + new MotionMagicVoltage(0).withEnableFOC(false).withSlot(1); + private TalonFX talonFX; + private TalonFXConfigurator talonFXConfigurator; + private StatusSignal position; + + public LaserSubsystemIOFX() { + talonFX = new TalonFX(LaserConstants.kLaserFxId); + talonFXConfigurator = talonFX.getConfigurator(); + talonFXConfigurator.apply(LaserConstants.turretFXConfig()); + + position = talonFX.getPosition(); + } + + public void setPosition(Angle setPos) { + talonFX.setControl(motionMagic.withPosition(setPos)); + } + + @Override + public void updateInputs(LaserIOInputs input) { + BaseStatusSignal.refreshAll(position); + } + + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFX, true); + } +} diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java index 7997ed9..cb7f3a6 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -6,6 +6,7 @@ import frc.robot.constants.TurretConstants; import java.util.ArrayList; import java.util.Set; +import net.jafama.FastMath; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; @@ -38,13 +39,13 @@ public double zero() { for (int i = 0; i < 8; i++) { possiblePos1.add( - ((inputs.canCoder1Pos.abs(Rotations) - TurretConstants.kCanCoder1Zero)) + (FastMath.abs(inputs.canCoder1Pos.in(Rotations) - TurretConstants.kCanCoder1Zero)) * TurretConstants.kCanCoder1Gr + TurretConstants.kCanCoder1PosRot[i]); } for (int i = 0; i < 8; i++) { possiblePos1.add( - (inputs.canCoder2Pos.abs(Rotations) - TurretConstants.kCanCoder2Zero) + FastMath.abs(inputs.canCoder2Pos.in(Rotations) - TurretConstants.kCanCoder2Zero) * TurretConstants.kCanCoder2Gr + TurretConstants.kCanCoder2PosRot[i]); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 5952212..3251c2a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -363,16 +363,32 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn } // See what pose is closer the the gyro at the time of the photo's capture. - // TODO If Birds nest camera then user turret plus gyro buffer. Else use gyro buffer. - double rotation = - gyroBuffer.get( - FastMath.floorToInt( - (((RobotController.getFPGATime() - time) / 1_000_000.0) - / VisionConstants.kLoopTime))); - Logger.recordOutput( - "Vision/Gyro Queried Loop Count", - FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); - return getCloserPose(pose1, pose2, rotation, camIndex); + if (camIndex == 1) { + double rotation = + (gyroBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime)))) + + turretBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime))); + Logger.recordOutput( + "Vision/Gyro Queried Loop Count", + FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); + return getCloserPose(pose1, pose2, rotation, camIndex); + + } else { + double rotation = + gyroBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime))); + Logger.recordOutput( + "Vision/Gyro Queried Loop Count", + FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); + return getCloserPose(pose1, pose2, rotation, camIndex); + } } @Override From 60b4540c46c1301f75abc71bfcb4b4472f8debbd Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 7 Oct 2025 19:50:26 -0400 Subject: [PATCH 09/11] Finshed Laser. Need to check on the overrides on Laser and Turret. Next time ROBOT STATE. --- .../frc/robot/constants/LaserConstants.java | 17 +++++++++-- .../subsystems/laser/LaserSubsystem.java | 30 ++++++++++++++++++- .../subsystems/laser/LaserSubsystemIO.java | 10 +++++++ .../subsystems/laser/LaserSubsystemIOFX.java | 30 ++++++++++++++++++- .../subsystems/turret/TurretSubsystem.java | 4 +++ 5 files changed, 87 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java index 0cd8e61..dfdca2b 100644 --- a/src/main/java/frc/robot/constants/LaserConstants.java +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -25,9 +25,10 @@ public class LaserConstants { public static final double kLaserGr = 20 / 150; public static final double kFxForwardMax = 0; public static final double kFxReverseMax = 0; - + public static final double kZeroedThreshhold = 0.025; + public static final int kZeroCounter = 2; // TODO Update Talon Constants - public static TalonFXConfiguration turretFXConfig() { + public static TalonFXConfiguration laserFXConfig() { TalonFXConfiguration fxConfig = new TalonFXConfiguration(); CurrentLimitsConfigs current = @@ -86,4 +87,16 @@ public static TalonFXConfiguration turretFXConfig() { fxConfig.MotorOutput = motorOut; return fxConfig; } + + public static CurrentLimitsConfigs getZeroingCurrentLimitsConfigs() { + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(false) + .withStatorCurrentLimit(20) + .withSupplyCurrentLimit(10) + .withSupplyCurrentLowerLimit(8) + .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimitEnable(true); + return current; + } } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index 99fd04d..ccfa10e 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -12,6 +12,8 @@ public class LaserSubsystem extends MeasurableSubsystem { private LaserSubsystemIO io; private LaserIOInputsAutoLogged inputs = new LaserIOInputsAutoLogged(); private Angle setPoint; + private LaserStates curState; + private int zeroCounter = 0; public LaserSubsystem() { this.io = io; @@ -29,7 +31,7 @@ public void setPosition(Angle pos) { public void zero() { // TODO Add kraken encoder and make zero function } - + // TODO Find whats up with these functions not needing @Overide public boolean isFinished() { return Math.abs(getLaserPos().minus(setPoint).in(Rotations)) < LaserConstants.kLaserCloseEnough; } @@ -40,4 +42,30 @@ public Set getMeasures() { new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), new Measure("Turret Setpoint", () -> setPoint.in(Rotations))); } + + public void perodic() { + io.updateInputs(inputs); + + switch (curState) { + case ZEROING: + if (Math.abs(inputs.velocity) < LaserConstants.kLaserCloseEnough) { + zeroCounter++; + if (zeroCounter <= LaserConstants.kZeroCounter) { + io.zero(); + zeroCounter = 0; + io.setLimitConfig(LaserConstants.laserFXConfig().CurrentLimits); + setPosition(Rotations.of(0)); + } + } + break; + + case ZEROED: + break; + } + } + + public enum LaserStates { + ZEROING, + ZEROED + } } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java index 98688dc..1dbb06b 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.laser; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; @@ -9,6 +10,7 @@ public interface LaserSubsystemIO { @AutoLog public class LaserIOInputs { public double position; + public double velocity; } public default void updateInputs(LaserIOInputs inputs) {} @@ -16,4 +18,12 @@ public default void updateInputs(LaserIOInputs inputs) {} public default void registerWith(TelemetryService telemetryService) {} public default void setPosition(Angle setPos) {} + + public default void zero() {} + + public default void setOpenLoopVoltage(double output) {} + + public default void setOpenLoopVelocity(double output) {} + + public default void setLimitConfig(CurrentLimitsConfigs config) {} } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java index b62184a..60fb267 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java @@ -1,11 +1,17 @@ package frc.robot.subsystems.laser; +import static edu.wpi.first.units.Units.Rotations; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.constants.LaserConstants; import org.strykeforce.telemetry.TelemetryService; @@ -16,25 +22,47 @@ public class LaserSubsystemIOFX implements LaserSubsystemIO { private TalonFX talonFX; private TalonFXConfigurator talonFXConfigurator; private StatusSignal position; + private StatusSignal velocity; + private DutyCycleOut openLoopVelocity = new DutyCycleOut(0); + private VoltageOut openLoopVoltage = new VoltageOut(0); public LaserSubsystemIOFX() { talonFX = new TalonFX(LaserConstants.kLaserFxId); talonFXConfigurator = talonFX.getConfigurator(); - talonFXConfigurator.apply(LaserConstants.turretFXConfig()); + talonFXConfigurator.apply(LaserConstants.laserFXConfig()); position = talonFX.getPosition(); + velocity = talonFX.getVelocity(); } public void setPosition(Angle setPos) { talonFX.setControl(motionMagic.withPosition(setPos)); } + public void zero() { + setPosition(Angle.ofBaseUnits(0.0, Rotations)); + setOpenLoopVelocity(0.0); + } + @Override public void updateInputs(LaserIOInputs input) { BaseStatusSignal.refreshAll(position); + BaseStatusSignal.refreshAll(velocity); + } + + public void setLimitConfig(CurrentLimitsConfigs config) { + talonFXConfigurator.apply(config); } public void registerWith(TelemetryService telemetryService) { telemetryService.register(talonFX, true); } + + public void setOpenLoopVoltage(double output) { + talonFX.setControl(openLoopVoltage.withOutput(output)); + } + + public void setOpenLoopVelocity(double output) { + talonFX.setControl(openLoopVelocity.withOutput(output)); + } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java index cb7f3a6..3ae6dc6 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -65,6 +65,10 @@ public boolean isFinished() { < TurretConstants.kTurretCloseEnough; } + public void perodic() { + io.updateInputs(inputs); + } + @Override public Set getMeasures() { return Set.of( From 0c37b8bbfc0ceaffe132e17adaf02db125d0f5ef Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 14 Oct 2025 19:14:49 -0400 Subject: [PATCH 10/11] Finished Laser Subsystem --- .../java/frc/robot/constants/LaserConstants.java | 10 ++++++++++ .../frc/robot/subsystems/laser/LaserSubsystem.java | 12 ++++++------ .../frc/robot/subsystems/laser/LaserSubsystemIO.java | 4 ++++ .../robot/subsystems/laser/LaserSubsystemIOFX.java | 4 ++++ .../frc/robot/subsystems/turret/TurretSubsystem.java | 2 +- 5 files changed, 25 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java index dfdca2b..c1acfc6 100644 --- a/src/main/java/frc/robot/constants/LaserConstants.java +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -27,6 +27,7 @@ public class LaserConstants { public static final double kFxReverseMax = 0; public static final double kZeroedThreshhold = 0.025; public static final int kZeroCounter = 2; + public static final int kZeroingVolts = 3; // TODO Update Talon Constants public static TalonFXConfiguration laserFXConfig() { TalonFXConfiguration fxConfig = new TalonFXConfiguration(); @@ -99,4 +100,13 @@ public static CurrentLimitsConfigs getZeroingCurrentLimitsConfigs() { .withSupplyCurrentLimitEnable(true); return current; } + public static SoftwareLimitSwitchConfigs getZeroingSoftLimitConfigs() { + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); + + return swLimit; + } } + diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index ccfa10e..5edcfcd 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.laser; - import static edu.wpi.first.units.Units.Rotations; - import edu.wpi.first.units.measure.Angle; import frc.robot.constants.LaserConstants; import java.util.Set; @@ -15,7 +13,7 @@ public class LaserSubsystem extends MeasurableSubsystem { private LaserStates curState; private int zeroCounter = 0; - public LaserSubsystem() { + public LaserSubsystem(LaserSubsystemIO io) { this.io = io; } @@ -29,11 +27,14 @@ public void setPosition(Angle pos) { } public void zero() { - // TODO Add kraken encoder and make zero function + io.setOpenLoopVoltage(LaserConstants.kZeroingVolts); + io.setSoftLimitConfig(LaserConstants.getZeroingSoftLimitConfigs()); + curState = LaserStates.ZEROING; } // TODO Find whats up with these functions not needing @Overide public boolean isFinished() { - return Math.abs(getLaserPos().minus(setPoint).in(Rotations)) < LaserConstants.kLaserCloseEnough; + return Math.abs(getLaserPos().minus(setPoint).in(Rotations)) < LaserConstants.kLaserCloseEnough && + curState != LaserStates.ZEROING; } @Override @@ -42,7 +43,6 @@ public Set getMeasures() { new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), new Measure("Turret Setpoint", () -> setPoint.in(Rotations))); } - public void perodic() { io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java index 1dbb06b..7a4bb59 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.laser; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; + import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; @@ -26,4 +28,6 @@ public default void setOpenLoopVoltage(double output) {} public default void setOpenLoopVelocity(double output) {} public default void setLimitConfig(CurrentLimitsConfigs config) {} + + public default void setSoftLimitConfig(SoftwareLimitSwitchConfigs config){} } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java index 60fb267..71df031 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -65,4 +66,7 @@ public void setOpenLoopVoltage(double output) { public void setOpenLoopVelocity(double output) { talonFX.setControl(openLoopVelocity.withOutput(output)); } + public void setSoftLimitConfig(SoftwareLimitSwitchConfigs config){ + talonFXConfigurator.apply(config); + } } diff --git a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java index 3ae6dc6..f4345fb 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretSubsystem.java @@ -18,7 +18,7 @@ public class TurretSubsystem extends MeasurableSubsystem { private ArrayList possiblePos1 = new ArrayList(); private ArrayList possiblePos2 = new ArrayList(); - public TurretSubsystem() { + public TurretSubsystem(TurretSubsystemIO io) { this.io = io; } From c1b0125b5980df681349265b0af0c09c59cb9b08 Mon Sep 17 00:00:00 2001 From: Huck Date: Tue, 14 Oct 2025 19:36:04 -0400 Subject: [PATCH 11/11] Made sure it was spotless and built for a push to main --- src/main/java/frc/robot/constants/LaserConstants.java | 2 +- .../java/frc/robot/subsystems/laser/LaserSubsystem.java | 7 +++++-- .../java/frc/robot/subsystems/laser/LaserSubsystemIO.java | 3 +-- .../frc/robot/subsystems/laser/LaserSubsystemIOFX.java | 3 ++- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/LaserConstants.java b/src/main/java/frc/robot/constants/LaserConstants.java index c1acfc6..061eb7f 100644 --- a/src/main/java/frc/robot/constants/LaserConstants.java +++ b/src/main/java/frc/robot/constants/LaserConstants.java @@ -100,6 +100,7 @@ public static CurrentLimitsConfigs getZeroingCurrentLimitsConfigs() { .withSupplyCurrentLimitEnable(true); return current; } + public static SoftwareLimitSwitchConfigs getZeroingSoftLimitConfigs() { SoftwareLimitSwitchConfigs swLimit = new SoftwareLimitSwitchConfigs() @@ -109,4 +110,3 @@ public static SoftwareLimitSwitchConfigs getZeroingSoftLimitConfigs() { return swLimit; } } - diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java index 5edcfcd..025e13a 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.laser; + import static edu.wpi.first.units.Units.Rotations; + import edu.wpi.first.units.measure.Angle; import frc.robot.constants.LaserConstants; import java.util.Set; @@ -33,8 +35,8 @@ public void zero() { } // TODO Find whats up with these functions not needing @Overide public boolean isFinished() { - return Math.abs(getLaserPos().minus(setPoint).in(Rotations)) < LaserConstants.kLaserCloseEnough && - curState != LaserStates.ZEROING; + return Math.abs(getLaserPos().minus(setPoint).in(Rotations)) < LaserConstants.kLaserCloseEnough + && curState != LaserStates.ZEROING; } @Override @@ -43,6 +45,7 @@ public Set getMeasures() { new Measure("Turret Finished?", () -> isFinished() ? 1 : 0), new Measure("Turret Setpoint", () -> setPoint.in(Rotations))); } + public void perodic() { io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java index 7a4bb59..c5086d8 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIO.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; - import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; @@ -29,5 +28,5 @@ public default void setOpenLoopVelocity(double output) {} public default void setLimitConfig(CurrentLimitsConfigs config) {} - public default void setSoftLimitConfig(SoftwareLimitSwitchConfigs config){} + public default void setSoftLimitConfig(SoftwareLimitSwitchConfigs config) {} } diff --git a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java index 71df031..c8bae4b 100644 --- a/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java +++ b/src/main/java/frc/robot/subsystems/laser/LaserSubsystemIOFX.java @@ -66,7 +66,8 @@ public void setOpenLoopVoltage(double output) { public void setOpenLoopVelocity(double output) { talonFX.setControl(openLoopVelocity.withOutput(output)); } - public void setSoftLimitConfig(SoftwareLimitSwitchConfigs config){ + + public void setSoftLimitConfig(SoftwareLimitSwitchConfigs config) { talonFXConfigurator.apply(config); } }