From 538cc02eb413cc3e848c6e50b496de1b80105313 Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Sun, 18 Jan 2026 20:31:05 -0600 Subject: [PATCH 01/20] First vision implementation (no simulation vision implementation) --- .vscode/settings.json | 8 +- src/main/java/frc/robot/Constants.java | 44 +++-- .../frc/robot/subsystems/DriveSubsystem.java | 32 ++-- .../frc/robot/subsystems/VisionSubsystem.java | 153 ++++++++++++++++++ .../frc/robot/utils/VisionEstimation.java | 18 +++ 5 files changed, 232 insertions(+), 23 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/VisionSubsystem.java create mode 100644 src/main/java/frc/robot/utils/VisionEstimation.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e6ede8..9ab7bfa 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,11 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.dependency.enableDependencyCheckup": false + "java.dependency.enableDependencyCheckup": false, + "cSpell.words": [ + "feedforwards", + "Holonomic", + "Photonvision", + "Pidgey" + ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1cc3a86..4406cdf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,18 +7,19 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +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.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +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.math.util.Units; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearAcceleration; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.*; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -137,11 +138,34 @@ public static final class NeoMotorConstants { public static final class VisionConstants { - public static final Transform3d kRobotToCam - = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + public static final String kCameraName1 = "Photonvision"; + public static final String kCameraName2 = "Photonvision2"; - public static final AprilTagFieldLayout kTagLayout - = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + // Distance to fill pose3d z value assuming robot is on the ground + public static Distance kEncoderZOffset = Inches.of(5.0); + + // Confidence of encoder readings for vision; should be tuned + public static final double kEncoderConfidence = 0.15; + + public static final Transform3d kRobotToCamOne = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); + public static final Transform3d kRobotToCamTwo = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); + + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout + .loadField(AprilTagFields.kDefaultField); + + // Placeholder numbers + public static final Pose3d kTurretAxisOfRotation = new Pose3d(Meters.of(0.2), Meters.of(0.3), Meters.of(0.3), + new Rotation3d(0.0, 0.0, 0.0)); + public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.6); + public static final Angle kCameraTwoPitch = Radians.of(0.0); + public static final Angle kCameraTwoRoll = Radians.of(0.0); + + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + + public static final Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); + public static final Matrix kVisionStdDevs = VecBuilder.fill(1, 1, 1); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e1c77fc..fdd4dd2 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -6,6 +6,7 @@ import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -16,6 +17,8 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; 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.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; @@ -24,11 +27,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; import frc.robot.utils.ShuffleboardPid; +import frc.robot.utils.VisionEstimation; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.VisionConstants; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.sim.Pigeon2SimState; @@ -40,6 +46,8 @@ import static edu.wpi.first.units.Units.*; +import org.photonvision.PhotonCamera; + public class DriveSubsystem extends SubsystemBase { // Create MAXSwerveModules @@ -77,6 +85,8 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); + private final VisionSubsystem m_vision = new VisionSubsystem(() -> Radians.of(0.0), this::addVisionMeasurement); + // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, new Rotation2d(pidgey.getYaw().getValue()), @@ -87,7 +97,7 @@ public class DriveSubsystem extends SubsystemBase { m_rearRight.getPosition() }); - SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( + SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( DriveConstants.kDriveKinematics, new Rotation2d(pidgey.getYaw().getValue()), new SwerveModulePosition[] { @@ -101,6 +111,7 @@ public class DriveSubsystem extends SubsystemBase { * Creates a new DriveSubsystem. */ public DriveSubsystem() { + // Usage reporting for MAXSwerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_MaxSwerve); @@ -211,8 +222,8 @@ public void periodic() { // System.out.println("Current rotation: " + // getPose().getRotation().getRadians()); - poseEstimator.update(new Rotation2d(getHeading()), getModulePositions()); - m_field.setRobotPose(poseEstimator.getEstimatedPosition()); + m_poseEstimator.update(new Rotation2d(getHeading()), getModulePositions()); + m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); SmartDashboard.putData(m_field); @@ -344,26 +355,23 @@ public Angle getHeading() { return pidgey.getYaw().getValue(); } - public Angle getNonContinuousHeading() { - if (getHeading().in(Radians) > 2.0 * Math.PI) { - return getHeading(); - } - - double rotations = Math.floor(getHeading().in(Radians) / (2 * Math.PI)); - return Radians.of(getHeading().in(Radians) - rotations); + public void addVisionMeasurement(VisionEstimation estimation) { + m_poseEstimator.addVisionMeasurement(estimation.m_pose, estimation.m_timestamp, estimation.m_stdDevs); } private Angle getOptimalAngle(Angle target) { Angle robotHeading = getHeading(); // Full robot rotations in radians - Angle robotRotations = Radians.of(Radians.convertFrom(Math.floor(robotHeading.in(Radians) / (2 * Math.PI)), Rotations)); + Angle robotRotations = Radians + .of(Radians.convertFrom(Math.floor(robotHeading.in(Radians) / (2 * Math.PI)), Rotations)); // Both are the same angle, just one is negative and one is positive Angle pTargetAngle = robotRotations.plus(target); Angle nTargetAngle = robotRotations.plus(target.minus(Radians.of(2 * Math.PI))); - // If either angle is less than 180 degrees relative to the robot's current angle, it is the most optimal path + // If either angle is less than 180 degrees relative to the robot's current + // angle, it is the most optimal path if (robotHeading.minus(pTargetAngle).abs(Radians) < Math.PI) { return pTargetAngle; } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java new file mode 100644 index 0000000..1694673 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -0,0 +1,153 @@ +package frc.robot.subsystems; + +import java.util.List; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.function.Supplier; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.robot.Constants.VisionConstants; +import frc.robot.utils.VisionEstimation; +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.units.Units.*; + +public class VisionSubsystem extends SubsystemBase { + + PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); + PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); + + Supplier m_turretAngleSupplier; + + PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(null, VisionConstants.kRobotToCamOne); + PhotonPoseEstimator m_poseEstimatorTwo = new PhotonPoseEstimator(null, VisionConstants.kRobotToCamTwo); + + Consumer m_visionConsumer; + private Matrix curStdDevs; + + public VisionSubsystem(Supplier turretAngleSupplier, Consumer visionConsumer) { + m_turretAngleSupplier = turretAngleSupplier; + m_visionConsumer = visionConsumer; + } + + @Override + public void periodic() { + Optional visionEstimationCameraOne = Optional.empty(); + + for (var result : m_camera1.getAllUnreadResults()) { + visionEstimationCameraOne = m_poseEstimatorOne.estimateCoprocMultiTagPose(result); + + if (visionEstimationCameraOne.isEmpty()) { + visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + } + + updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), m_poseEstimatorOne); + + visionEstimationCameraOne.ifPresent(estimation -> { + m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), + Timer.getFPGATimestamp(), getCurrentStdDevs())); + }); + } + + Optional visionEstimationCameraTwo = Optional.empty(); + m_poseEstimatorTwo.setRobotToCameraTransform(getTurretCameraTransform()); + + for (var result : m_camera1.getAllUnreadResults()) { + visionEstimationCameraTwo = m_poseEstimatorTwo.estimateCoprocMultiTagPose(result); + + if (visionEstimationCameraTwo.isEmpty()) { + visionEstimationCameraTwo = m_poseEstimatorTwo.estimateLowestAmbiguityPose(result); + } + + updateEstimationStdDevs(visionEstimationCameraTwo, result.getTargets(), m_poseEstimatorTwo); + + visionEstimationCameraTwo.ifPresent(estimation -> { + m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), + Timer.getFPGATimestamp(), getCurrentStdDevs())); + }); + } + } + + private void updateEstimationStdDevs( + Optional estimatedPose, List targets, + PhotonPoseEstimator poseEstimator) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) + continue; + numTags++; + avgDist += tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + private Transform3d getTurretCameraTransform() { + Angle turretAngle = m_turretAngleSupplier.get(); + + Distance cameraXOffset = Meters + .of(VisionConstants.kTurretCameraDistanceToCenter.in(Meters) * Math.cos(turretAngle.in(Radians))); + Distance cameraYOffset = Meters + .of(VisionConstants.kTurretCameraDistanceToCenter.in(Meters) * Math.sin(turretAngle.in(Radians))); + + Distance cameraX = VisionConstants.kTurretAxisOfRotation.getMeasureX().plus(cameraXOffset); + Distance cameraY = VisionConstants.kTurretAxisOfRotation.getMeasureY().plus(cameraYOffset); + Distance cameraZ = VisionConstants.kTurretAxisOfRotation.getMeasureZ(); + + return new Transform3d(cameraX, cameraY, cameraZ, + new Rotation3d(VisionConstants.kCameraTwoPitch, VisionConstants.kCameraTwoRoll, turretAngle)); + } + + private Matrix getCurrentStdDevs() { + return curStdDevs; + } +} diff --git a/src/main/java/frc/robot/utils/VisionEstimation.java b/src/main/java/frc/robot/utils/VisionEstimation.java new file mode 100644 index 0000000..695e5e4 --- /dev/null +++ b/src/main/java/frc/robot/utils/VisionEstimation.java @@ -0,0 +1,18 @@ +package frc.robot.utils; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public class VisionEstimation { + public Pose2d m_pose; + public double m_timestamp; + public Matrix m_stdDevs; + + public VisionEstimation(Pose2d pose, double timestamp, Matrix stdDevs) { + m_pose = pose; + m_timestamp = timestamp; + m_stdDevs = stdDevs; + } +} From fb374f5a0189b60170b10d0aa5f323453967b0fd Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Sun, 18 Jan 2026 20:35:12 -0600 Subject: [PATCH 02/20] Camera variable fix --- src/main/java/frc/robot/subsystems/VisionSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 1694673..37220d9 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -69,7 +69,7 @@ public void periodic() { Optional visionEstimationCameraTwo = Optional.empty(); m_poseEstimatorTwo.setRobotToCameraTransform(getTurretCameraTransform()); - for (var result : m_camera1.getAllUnreadResults()) { + for (var result : m_camera2.getAllUnreadResults()) { visionEstimationCameraTwo = m_poseEstimatorTwo.estimateCoprocMultiTagPose(result); if (visionEstimationCameraTwo.isEmpty()) { From bb014a1ff47486cb468aafb5bd2d1610dbf95e56 Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Sun, 18 Jan 2026 20:40:16 -0600 Subject: [PATCH 03/20] Make turret angle supplier optional in case turret is fixed --- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/VisionSubsystem.java | 9 ++++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index fdd4dd2..d2433f1 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -85,7 +85,7 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); - private final VisionSubsystem m_vision = new VisionSubsystem(() -> Radians.of(0.0), this::addVisionMeasurement); + private final VisionSubsystem m_vision = new VisionSubsystem(null, this::addVisionMeasurement); // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 37220d9..6c30836 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -34,7 +34,7 @@ public class VisionSubsystem extends SubsystemBase { PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); - Supplier m_turretAngleSupplier; + Optional> m_turretAngleSupplier; PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(null, VisionConstants.kRobotToCamOne); PhotonPoseEstimator m_poseEstimatorTwo = new PhotonPoseEstimator(null, VisionConstants.kRobotToCamTwo); @@ -42,7 +42,7 @@ public class VisionSubsystem extends SubsystemBase { Consumer m_visionConsumer; private Matrix curStdDevs; - public VisionSubsystem(Supplier turretAngleSupplier, Consumer visionConsumer) { + public VisionSubsystem(Optional> turretAngleSupplier, Consumer visionConsumer) { m_turretAngleSupplier = turretAngleSupplier; m_visionConsumer = visionConsumer; } @@ -132,7 +132,10 @@ private void updateEstimationStdDevs( } private Transform3d getTurretCameraTransform() { - Angle turretAngle = m_turretAngleSupplier.get(); + if (m_turretAngleSupplier.isEmpty()) + return VisionConstants.kRobotToCamTwo; + + Angle turretAngle = m_turretAngleSupplier.get().get(); Distance cameraXOffset = Meters .of(VisionConstants.kTurretCameraDistanceToCenter.in(Meters) * Math.cos(turretAngle.in(Radians))); From c4409918add72fcbc9e3ddf80d817a3e016e59ee Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Mon, 19 Jan 2026 09:23:40 -0600 Subject: [PATCH 04/20] Use pose estimation --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/Constants.java | 20 +++++++++++++++---- .../frc/robot/subsystems/DriveSubsystem.java | 6 ++++-- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 9ab7bfa..b6e2b76 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -62,6 +62,7 @@ "feedforwards", "Holonomic", "Photonvision", - "Pidgey" + "Pidgey", + "teleop" ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4406cdf..891ec0d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,8 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.*; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; @@ -19,7 +17,21 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.*; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -72,7 +84,7 @@ public static final class DriveConstants { public static final int kFrontRightTurningCanId = 5; public static final int kRearRightTurningCanId = 8; - // Auxillary Device Can IDs + // Auxiliary Device Can IDs public static final int kPidgeyCanId = 13; public static final boolean kGyroReversed = false; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d2433f1..b6089cf 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -46,6 +46,8 @@ import static edu.wpi.first.units.Units.*; +import java.util.Optional; + import org.photonvision.PhotonCamera; public class DriveSubsystem extends SubsystemBase { @@ -85,7 +87,7 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); - private final VisionSubsystem m_vision = new VisionSubsystem(null, this::addVisionMeasurement); + private final VisionSubsystem m_vision = new VisionSubsystem(Optional.empty(), this::addVisionMeasurement); // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, @@ -236,7 +238,7 @@ public void periodic() { * @return The pose. */ public Pose2d getPose() { - return m_odometry.getPoseMeters(); + return m_poseEstimator.getEstimatedPosition(); } public SwerveModulePosition[] getModulePositions() { From 6be9c870fb8fb77d262dee87e0c851383a1de30f Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 20 Jan 2026 18:36:29 -0600 Subject: [PATCH 05/20] Turret and shooter subsystem implementation --- src/main/java/frc/robot/Constants.java | 15 ++++- src/main/java/frc/robot/RobotContainer.java | 6 ++ .../robot/subsystems/ShooterSubsystem.java | 43 +++++++++++++ .../frc/robot/subsystems/TurretSubsystem.java | 60 +++++++++++++++++++ 4 files changed, 123 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/TurretSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d5d42e0..dc76355 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,6 +5,8 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; @@ -14,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearAcceleration; @@ -148,4 +151,14 @@ public static final class VisionConstants { public static final class NumericalConstants { public static final double kEpsilon = 1e-6; } -} + + public static final class TurretConstants { + public static final int kMotorId = 1; + public static final Angle kMinAngle = Radians.of(0.0); + public static final Angle kMaxAngle = Radians.of(2.0 * Math.PI); + } + + public static final class ShooterConstants { + public static final int kMotorId = 1; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c671577..99fc45d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,8 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.TurretSubsystem; public class RobotContainer { @@ -25,7 +27,11 @@ public class RobotContainer { private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); + private TurretSubsystem m_turret; + public RobotContainer() { + m_turret = new TurretSubsystem(m_drive::getHeading); + m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..06b213f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.RPM; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants; + +public class ShooterSubsystem extends SubsystemBase { + + SparkMax m_shooterMotor = new SparkMax(ShooterConstants.kMotorId, MotorType.kBrushless); + + AbsoluteEncoder m_absoluteEncoder = m_shooterMotor.getAbsoluteEncoder(); + + private final SparkClosedLoopController m_shooterClosedLoopController = m_shooterMotor.getClosedLoopController(); + + public ShooterSubsystem() { + + } + + public void Spin(AngularVelocity shootSpeedVelocity) { + //activates shooter to shoot balls at specific velocity + + m_shooterClosedLoopController.setSetpoint(shootSpeedVelocity.in(RPM), ControlType.kVelocity); + } + + public void Stop() { + Spin(RPM.of(0)); + } + + @Override + public void periodic() { + + } +} diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java new file mode 100644 index 0000000..11c8884 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Rotations; + +import java.util.function.Supplier; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.TurretConstants; + + +public class TurretSubsystem extends SubsystemBase { + + private final SparkMax m_turretMotor = new SparkMax(TurretConstants.kMotorId, MotorType.kBrushless); + + private final AbsoluteEncoder m_absoluteEncoder = m_turretMotor.getAbsoluteEncoder(); + + private final SparkClosedLoopController m_turretClosedLoopController = m_turretMotor.getClosedLoopController(); + + private Supplier m_robotAngleSupplier; + + public TurretSubsystem(Supplier robotAngleSupplier) { + m_robotAngleSupplier = robotAngleSupplier; + } + + public Command moveToAngle(Angle angle) { + + + + return new InstantCommand(() -> { + if (angle.gt(TurretConstants.kMaxAngle)) { + System.out.println("Angle " + angle + "is bigger than maximum angle " + TurretConstants.kMaxAngle + "."); + return; + + } else if (angle.lt(TurretConstants.kMinAngle)) { + System.out.println("Angle " + angle + "is to smaller than minimum angle " + TurretConstants.kMinAngle + "."); + return; + } + + m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); + }); + } + + public Angle getRotation() { + return Rotations.of(m_absoluteEncoder.getPosition()); + } + + @Override + public void periodic() { + + } +} \ No newline at end of file From aa9290a30b56f88b3e73bfe9388b78ef8c41f8a6 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 22 Jan 2026 17:18:09 -0600 Subject: [PATCH 06/20] Simulation utility implementation (not finished) --- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/TurretSubsystem.java | 14 +++++++-- .../frc/robot/utils/SubsystemSimulation.java | 29 +++++++++++++++++++ 3 files changed, 41 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/utils/SubsystemSimulation.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dc76355..80ab77f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -153,7 +153,7 @@ public static final class NumericalConstants { } public static final class TurretConstants { - public static final int kMotorId = 1; + public static final int kMotorId = 20; public static final Angle kMinAngle = Radians.of(0.0); public static final Angle kMaxAngle = Radians.of(2.0 * Math.PI); } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 11c8884..84d0358 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -5,11 +5,14 @@ import java.util.function.Supplier; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.sim.SparkAbsoluteEncoderSim; +import com.revrobotics.sim.SparkMaxSim; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -20,8 +23,10 @@ public class TurretSubsystem extends SubsystemBase { private final SparkMax m_turretMotor = new SparkMax(TurretConstants.kMotorId, MotorType.kBrushless); + private final SparkMaxSim m_simTurretMotor = new SparkMaxSim(m_turretMotor, DCMotor.getNEO(1)); private final AbsoluteEncoder m_absoluteEncoder = m_turretMotor.getAbsoluteEncoder(); + private final SparkAbsoluteEncoderSim m_simAbsoluteEncoder = m_simTurretMotor.getAbsoluteEncoderSim(); private final SparkClosedLoopController m_turretClosedLoopController = m_turretMotor.getClosedLoopController(); @@ -32,9 +37,6 @@ public TurretSubsystem(Supplier robotAngleSupplier) { } public Command moveToAngle(Angle angle) { - - - return new InstantCommand(() -> { if (angle.gt(TurretConstants.kMaxAngle)) { System.out.println("Angle " + angle + "is bigger than maximum angle " + TurretConstants.kMaxAngle + "."); @@ -57,4 +59,10 @@ public Angle getRotation() { public void periodic() { } + + @Override + public void simulationPeriodic() { + m_simTurretMotor.setVelocity(1.0); + System.out.println(m_simAbsoluteEncoder.getPosition()); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/SubsystemSimulation.java b/src/main/java/frc/robot/utils/SubsystemSimulation.java new file mode 100644 index 0000000..8418112 --- /dev/null +++ b/src/main/java/frc/robot/utils/SubsystemSimulation.java @@ -0,0 +1,29 @@ +package frc.robot.utils; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; + +public class SubsystemSimulation { + private Mechanism2d m_mech; + private MechanismRoot2d m_root; + + // Simulation for elevator + public SubsystemSimulation(String name, Distance width, Distance height, Distance startDistance) { + m_mech = new Mechanism2d(width.in(Meters), height.in(Meters)); + m_root = m_mech.getRoot(name, width.in(Meters) / 2, 0.0); + m_root.append(new MechanismLigament2d(name, startDistance.in(Meters), Math.PI)); + } + + // Simulation for arm + public SubsystemSimulation(String name, Distance width, Distance height, Distance length, Angle startAngle) { + m_mech = new Mechanism2d(width.in(Meters), height.in(Meters)); + m_root = m_mech.getRoot(name, width.in(Meters) / 2, 0.0); + m_root.append(new MechanismLigament2d(name, length.in(Meters), startAngle.in(Radians))); + } +} \ No newline at end of file From 583853a4b68b78931ce2684e0e39c45b63ea862a Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 27 Jan 2026 19:03:13 -0600 Subject: [PATCH 07/20] Add vision IPs --- README.md | 1 + src/main/java/frc/robot/subsystems/MAXSwerveModule.java | 9 +++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 6bcc477..919f6da 100644 --- a/README.md +++ b/README.md @@ -47,6 +47,7 @@ All devices connected to the robot's local network along with each device's assi | ------- | ---------- | | Gateway | 10.56.90.1 | | RoboRio | 10.56.90.2 | +| Vision Coprocessor | 10.56.90.10 | ## Button Bindings diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 9988f5e..c92206f 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -38,7 +38,7 @@ public class MAXSwerveModule { private final SparkClosedLoopController m_drivingClosedLoopController; private final SparkClosedLoopController m_turningClosedLoopController; - + private LinearVelocity m_simDriverEncoderVelocity = MetersPerSecond.of(0.0); private Distance m_simDriverEncoderPosition = Meters.of(0.0); private Angle m_simCurrentAngle = Radians.of(0.0); @@ -83,7 +83,8 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular public void updateSimDriverPosition(SwerveModuleState desiredState) { m_simDriverEncoderVelocity = MetersPerSecond.of(desiredState.speedMetersPerSecond); - m_simDriverEncoderPosition = m_simDriverEncoderPosition.plus(m_simDriverEncoderVelocity.times(Constants.DriveConstants.kPeriodicInterval)); + m_simDriverEncoderPosition = m_simDriverEncoderPosition + .plus(m_simDriverEncoderVelocity.times(Constants.DriveConstants.kPeriodicInterval)); } public SwerveModuleState getState() { @@ -94,7 +95,7 @@ public SwerveModuleState getState() { return new SwerveModuleState(m_drivingEncoder.getVelocity(), new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); - return new SwerveModuleState(m_simDriverEncoderVelocity, + return new SwerveModuleState(m_simDriverEncoderVelocity, new Rotation2d(m_simCurrentAngle.in(Radian) - m_chassisAngularOffset)); } @@ -111,7 +112,7 @@ public SwerveModulePosition getPosition() { return new SwerveModulePosition( m_drivingEncoder.getPosition(), new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); - + return new SwerveModulePosition(m_simDriverEncoderPosition, new Rotation2d(m_simCurrentAngle.in(Radian) - m_chassisAngularOffset)); } From 3aa617f799cd82a4f38da239e6fbcde8969381e0 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 29 Jan 2026 18:26:30 -0600 Subject: [PATCH 08/20] Tested vision implementation --- .../frc/robot/subsystems/DriveSubsystem.java | 8 ++++++- .../Vision.java} | 22 +++++++++---------- 2 files changed, 18 insertions(+), 12 deletions(-) rename src/main/java/frc/robot/{subsystems/VisionSubsystem.java => utils/Vision.java} (91%) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index b6089cf..832e300 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -28,6 +28,7 @@ import frc.robot.Robot; import frc.robot.utils.ShuffleboardPid; import frc.robot.utils.VisionEstimation; +import frc.robot.utils.Vision; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.VisionConstants; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -87,7 +88,7 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); - private final VisionSubsystem m_vision = new VisionSubsystem(Optional.empty(), this::addVisionMeasurement); + private final Vision m_vision = new Vision(Optional.empty(), this::addVisionMeasurement); // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, @@ -227,6 +228,10 @@ public void periodic() { m_poseEstimator.update(new Rotation2d(getHeading()), getModulePositions()); m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + // System.out.println(m_poseEstimator.getEstimatedPosition()); + + m_vision.periodic(); + SmartDashboard.putData(m_field); m_pidController.periodic(); @@ -358,6 +363,7 @@ public Angle getHeading() { } public void addVisionMeasurement(VisionEstimation estimation) { + System.out.println(estimation.m_pose); m_poseEstimator.addVisionMeasurement(estimation.m_pose, estimation.m_timestamp, estimation.m_stdDevs); } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/utils/Vision.java similarity index 91% rename from src/main/java/frc/robot/subsystems/VisionSubsystem.java rename to src/main/java/frc/robot/utils/Vision.java index 6c30836..b388119 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems; +package frc.robot.utils; import java.util.List; import java.util.Optional; @@ -22,37 +22,37 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.robot.Constants.VisionConstants; -import frc.robot.utils.VisionEstimation; import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static edu.wpi.first.units.Units.*; -public class VisionSubsystem extends SubsystemBase { +public class Vision { PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); Optional> m_turretAngleSupplier; - PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(null, VisionConstants.kRobotToCamOne); - PhotonPoseEstimator m_poseEstimatorTwo = new PhotonPoseEstimator(null, VisionConstants.kRobotToCamTwo); + PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(VisionConstants.kTagLayout, + VisionConstants.kRobotToCamOne); + PhotonPoseEstimator m_poseEstimatorTwo = new PhotonPoseEstimator(VisionConstants.kTagLayout, + VisionConstants.kRobotToCamTwo); Consumer m_visionConsumer; private Matrix curStdDevs; - public VisionSubsystem(Optional> turretAngleSupplier, Consumer visionConsumer) { + public Vision(Optional> turretAngleSupplier, Consumer visionConsumer) { m_turretAngleSupplier = turretAngleSupplier; m_visionConsumer = visionConsumer; } - @Override public void periodic() { Optional visionEstimationCameraOne = Optional.empty(); for (var result : m_camera1.getAllUnreadResults()) { - visionEstimationCameraOne = m_poseEstimatorOne.estimateCoprocMultiTagPose(result); + visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); if (visionEstimationCameraOne.isEmpty()) { visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); @@ -62,7 +62,7 @@ public void periodic() { visionEstimationCameraOne.ifPresent(estimation -> { m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), - Timer.getFPGATimestamp(), getCurrentStdDevs())); + estimation.timestampSeconds, getCurrentStdDevs())); }); } @@ -80,7 +80,7 @@ public void periodic() { visionEstimationCameraTwo.ifPresent(estimation -> { m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), - Timer.getFPGATimestamp(), getCurrentStdDevs())); + estimation.timestampSeconds, getCurrentStdDevs())); }); } } @@ -134,7 +134,7 @@ private void updateEstimationStdDevs( private Transform3d getTurretCameraTransform() { if (m_turretAngleSupplier.isEmpty()) return VisionConstants.kRobotToCamTwo; - + Angle turretAngle = m_turretAngleSupplier.get().get(); Distance cameraXOffset = Meters From 66add86276055bdc310a7364a74b2822a9c91f73 Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Sun, 1 Feb 2026 13:41:37 -0600 Subject: [PATCH 09/20] Precise turret position function (untested) --- src/main/java/frc/robot/Constants.java | 17 +++-- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 9 ++- .../frc/robot/subsystems/TurretSubsystem.java | 39 +++++++---- .../java/frc/robot/utils/PositionBuffer.java | 68 +++++++++++++++++++ src/main/java/frc/robot/utils/RingBuffer.java | 67 ++++++++++++++++++ 6 files changed, 183 insertions(+), 19 deletions(-) create mode 100644 src/main/java/frc/robot/utils/PositionBuffer.java create mode 100644 src/main/java/frc/robot/utils/RingBuffer.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 80ab77f..3502f3d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,10 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; @@ -16,6 +12,12 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -146,6 +148,8 @@ public static final class VisionConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + public static final int kMaxTimeStamps = 2000; } public static final class NumericalConstants { @@ -156,6 +160,11 @@ public static final class TurretConstants { public static final int kMotorId = 20; public static final Angle kMinAngle = Radians.of(0.0); public static final Angle kMaxAngle = Radians.of(2.0 * Math.PI); + + public static final int kPositionBufferLength = 4000; + public static final Time kEncoderReadingDelay = Seconds.of(0.005); + + public static final Time kEncoderReadInterval = Seconds.of(0.01); } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cea1ffc..9bd5e50 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -15,6 +16,7 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); + addPeriodic(m_robotContainer.pushTurretEncoderReading(), Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 99fc45d..38ae911 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; public class RobotContainer { @@ -27,7 +26,7 @@ public class RobotContainer { private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); - private TurretSubsystem m_turret; + private final TurretSubsystem m_turret; public RobotContainer() { m_turret = new TurretSubsystem(m_drive::getHeading); @@ -63,4 +62,10 @@ public Command getAutonomousCommand() { return new PathPlannerAuto(m_autoSelected); } + + public Runnable pushTurretEncoderReading() { + return () -> { + m_turret.pushCurrentEncoderReading(); + }; + } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 84d0358..6e7a617 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,27 +1,27 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Rotations; - import java.util.function.Supplier; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.sim.SparkAbsoluteEncoderSim; import com.revrobotics.sim.SparkMaxSim; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.TurretConstants; - +import frc.robot.utils.PositionBuffer; public class TurretSubsystem extends SubsystemBase { - + private final SparkMax m_turretMotor = new SparkMax(TurretConstants.kMotorId, MotorType.kBrushless); private final SparkMaxSim m_simTurretMotor = new SparkMaxSim(m_turretMotor, DCMotor.getNEO(1)); @@ -30,8 +30,10 @@ public class TurretSubsystem extends SubsystemBase { private final SparkClosedLoopController m_turretClosedLoopController = m_turretMotor.getClosedLoopController(); + private final PositionBuffer m_positionBuffer = new PositionBuffer(TurretConstants.kPositionBufferLength); + private Supplier m_robotAngleSupplier; - + public TurretSubsystem(Supplier robotAngleSupplier) { m_robotAngleSupplier = robotAngleSupplier; } @@ -39,12 +41,14 @@ public TurretSubsystem(Supplier robotAngleSupplier) { public Command moveToAngle(Angle angle) { return new InstantCommand(() -> { if (angle.gt(TurretConstants.kMaxAngle)) { - System.out.println("Angle " + angle + "is bigger than maximum angle " + TurretConstants.kMaxAngle + "."); - return; - + System.out + .println("Angle " + angle + "is bigger than maximum angle " + TurretConstants.kMaxAngle + "."); + return; + } else if (angle.lt(TurretConstants.kMinAngle)) { - System.out.println("Angle " + angle + "is to smaller than minimum angle " + TurretConstants.kMinAngle + "."); - return; + System.out.println( + "Angle " + angle + "is to smaller than minimum angle " + TurretConstants.kMinAngle + "."); + return; } m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); @@ -55,9 +59,18 @@ public Angle getRotation() { return Rotations.of(m_absoluteEncoder.getPosition()); } + public Angle getRotationAtTime(double timestamp) { + return m_positionBuffer.getAngleAtTime(timestamp); + } + @Override public void periodic() { - + } + + // Connected to another periodic loop that runs quicker than 0.02 seconds + public void pushCurrentEncoderReading() { + m_positionBuffer.pushElement(Rotations.of(m_absoluteEncoder.getPosition()), + TurretConstants.kEncoderReadingDelay.in(Seconds)); } @Override diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java new file mode 100644 index 0000000..6e94e41 --- /dev/null +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -0,0 +1,68 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Timer; + +public class PositionBuffer { + + private record TimePosition(Angle angle, double timestamp) { + } + + private final RingBuffer m_positions; + + public PositionBuffer(int length) { + m_positions = new RingBuffer<>(length); + } + + public void pushElement(Angle angle, double delay) { + m_positions.push(new TimePosition(angle, Timer.getFPGATimestamp() - delay)); + } + + public Angle getAngleAtTime(double time) { + // A fun little binary search algo + int high = m_positions.getLength(); + int low = 0; + + // Avoiding overflow (though isn't really necessary here) + int midpoint = low + (high - low) / 2; + + while (low < high) { + double timeAtMidpoint = m_positions.get(midpoint).timestamp; + + if (time > timeAtMidpoint) { + low = midpoint + 1; + } else if (time < timeAtMidpoint) { + high = midpoint; + } + + midpoint = low + (high - low) / 2; + } + + Angle angleAtTimeStamp; + + // Linearly interpolate velocity if we aren't on the first/last timestamp + if (midpoint != 0 && midpoint != m_positions.getLength() - 1) { + TimePosition closestTimestamp = m_positions.get(midpoint); + + double dt = m_positions.get(midpoint).timestamp - time; + TimePosition nextTimeStamp = dt > 0 ? m_positions.get(midpoint + 1) : m_positions.get(midpoint - 1); + + Angle daStamped = nextTimeStamp.angle.minus(closestTimestamp.angle); + + // Absolute since in order to preserve the sign, only one part of the fraction + // can keep its original sign. In this case we always keep the denominator + // positive + double dtStamped = Math.abs(nextTimeStamp.timestamp - closestTimestamp.timestamp); + + double percentageTimeTraveled = dt / dtStamped; + Angle da = daStamped.times(percentageTimeTraveled); + + angleAtTimeStamp = closestTimestamp.angle.plus(da); + } else { + angleAtTimeStamp = m_positions.get(midpoint).angle; + } + + return angleAtTimeStamp; + + } +} diff --git a/src/main/java/frc/robot/utils/RingBuffer.java b/src/main/java/frc/robot/utils/RingBuffer.java new file mode 100644 index 0000000..d7738ee --- /dev/null +++ b/src/main/java/frc/robot/utils/RingBuffer.java @@ -0,0 +1,67 @@ +package frc.robot.utils; + +public class RingBuffer { + private int m_tail; + private int m_head; + private int m_length; + + private Object[] m_elements; + + public RingBuffer(int maxLength) { + + if (maxLength < 1) { + System.out.println("Cannot create ring buffer with a max length of < 1."); + return; + } + + m_elements = new Object[maxLength]; + + m_head = 0; + m_tail = 0; + m_length = maxLength; + } + + public void push(T element) { + if (m_head - m_tail == m_length) { + pop(); + } + + m_elements[getTrueIndex(m_head++)] = element; + } + + public T pop() { + if (m_tail == m_head) { + System.out.println("Cannot pop from empty ring buffer."); + return null; + } + + return (T) m_elements[getTrueIndex(m_tail++)]; + } + + public T get(int index) { + int trueIndex = m_tail + index; + if (trueIndex > m_head) { + System.out.println("Index " + index + " is out of bounds of the ring buffer."); + return null; + } + + return (T) m_elements[getTrueIndex(trueIndex)]; + } + + public int getLength() { + return m_head - m_tail; + } + + @Override + public String toString() { + String str = "{ "; + for (int i = m_tail; i < m_head; i++) { + str += m_elements[getTrueIndex(i)] + " "; + } + return str + "}"; + } + + private int getTrueIndex(int index) { + return index % m_elements.length; + } +} \ No newline at end of file From 070fd47e8963e704b88bcc23ae93f5fde854aa58 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 2 Feb 2026 19:18:09 -0600 Subject: [PATCH 10/20] Fix drivetrain pose issues and attempt heading fix (did not work, very destructive) --- src/main/java/frc/robot/Constants.java | 44 +++++++++++++++--- src/main/java/frc/robot/RobotContainer.java | 13 ++++-- .../frc/robot/subsystems/DriveSubsystem.java | 12 ++--- .../robot/subsystems/ShooterSubsystem.java | 45 +++++++++++++++---- .../frc/robot/subsystems/TurretSubsystem.java | 36 +++++++++++++-- 5 files changed, 122 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3502f3d..23498bc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,8 +3,13 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; @@ -142,12 +147,11 @@ public static final class NeoMotorConstants { public static final class VisionConstants { - public static final Transform3d kRobotToCam - = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); - public static final AprilTagFieldLayout kTagLayout - = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout + .loadField(AprilTagFields.kDefaultField); public static final int kMaxTimeStamps = 2000; } @@ -159,15 +163,41 @@ public static final class NumericalConstants { public static final class TurretConstants { public static final int kMotorId = 20; public static final Angle kMinAngle = Radians.of(0.0); - public static final Angle kMaxAngle = Radians.of(2.0 * Math.PI); + public static final Angle kMaxAngle = Radians.of(1.0 * Math.PI); public static final int kPositionBufferLength = 4000; public static final Time kEncoderReadingDelay = Seconds.of(0.005); public static final Time kEncoderReadInterval = Seconds.of(0.01); + + public static final double kP = 1.5; + public static final double kI = 0.0; + public static final double kD = 0.0; } public static final class ShooterConstants { - public static final int kMotorId = 1; + public static final int kShooterMotorId = 30; + public static final int kHoodMotorId = 31; + + public static final double kHoodP = 0.1; + public static final double kHoodI = 0.0; + public static final double kHoodD = 0.0; + + public static final double kShooterP = 0.1; + public static final double kShooterI = 0.0; + public static final double kShooterD = 0.0; + + // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part + public static final double kHoodGearRatio = (62 / 25) * (14 / 218); + } + + public static final class Fixtures { + public static final Pose2d kRedAllianceHub = new Pose2d(); + public static final Pose2d kBlueAllianceHub = new Pose2d(); + + // From a top down perspective of the field with the red alliance on the left + // side + public static final Pose2d kTopFeedPose = new Pose2d(); + public static final Pose2d kBottomFeedPose = new Pose2d(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 38ae911..176d1b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,6 +3,8 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import static edu.wpi.first.units.Units.Degrees; + import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; @@ -10,6 +12,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; @@ -21,7 +24,8 @@ public class RobotContainer { private final DriveSubsystem m_drive = new DriveSubsystem(); - private final CommandXboxController m_driverController = new CommandXboxController(OIConstants.kDriverControllerPort); + private final CommandXboxController m_driverController = new CommandXboxController( + OIConstants.kDriverControllerPort); private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); @@ -29,7 +33,7 @@ public class RobotContainer { private final TurretSubsystem m_turret; public RobotContainer() { - m_turret = new TurretSubsystem(m_drive::getHeading); + m_turret = new TurretSubsystem(m_drive::getPose); m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); @@ -51,8 +55,9 @@ public RobotContainer() { } private void configureBindings() { - m_driverController.x().whileTrue(m_drive.enableFacePose(new Pose2d())); - m_driverController.x().whileFalse(m_drive.disableFacePose()); + m_driverController.a() + .whileTrue( + m_turret.pointToHeading(Degrees.of(90.0))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 7d9088e..150268d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -178,7 +178,7 @@ public Command enableFacePose(Pose2d fixture) { // Floating point value correction if (Math.abs(totalDistance) < Constants.NumericalConstants.kEpsilon) return; - + m_targetAutoAngle = Radians.of(Math.atan2(yFixtureDist, xFixtureDist)); m_isManualRotate = false; @@ -234,7 +234,7 @@ public void periodic() { * @return The pose. */ public Pose2d getPose() { - return m_odometry.getPoseMeters(); + return poseEstimator.getEstimatedPosition(); } public SwerveModulePosition[] getModulePositions() { @@ -275,13 +275,15 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ // Convert the commanded speeds into the correct units for the drivetrain if (!m_isManualRotate) - System.out.println("Setpoint: " + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians) + ", Current: " - + getHeading().in(Radians)); + System.out + .println("Setpoint: " + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians) + ", Current: " + + getHeading().in(Radians)); double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeed.magnitude(); double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeed.magnitude(); double rotDelivered = (m_isManualRotate) ? rot * DriveConstants.kMaxAngularSpeed.magnitude() - : m_pidController.calculate(getHeading().in(Radians), getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians)); + : m_pidController.calculate(getHeading().in(Radians), + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians)); var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 06b213f..1b260a3 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,43 +1,72 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.*; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +import frc.robot.utils.ShuffleboardPid; public class ShooterSubsystem extends SubsystemBase { - - SparkMax m_shooterMotor = new SparkMax(ShooterConstants.kMotorId, MotorType.kBrushless); - AbsoluteEncoder m_absoluteEncoder = m_shooterMotor.getAbsoluteEncoder(); + SparkMax m_shooterMotor = new SparkMax(ShooterConstants.kShooterMotorId, MotorType.kBrushless); + SparkMax m_hoodMotor = new SparkMax(ShooterConstants.kHoodMotorId, MotorType.kBrushless); + + AbsoluteEncoder m_absoluteEncoder = m_hoodMotor.getAbsoluteEncoder(); private final SparkClosedLoopController m_shooterClosedLoopController = m_shooterMotor.getClosedLoopController(); + private final SparkClosedLoopController m_hoodClosedLoopController = m_hoodMotor.getClosedLoopController(); + + private final SparkMaxConfig m_shooterConfig = new SparkMaxConfig(); + private final SparkMaxConfig m_hoodConfig = new SparkMaxConfig(); public ShooterSubsystem() { + m_shooterConfig.closedLoop + .p(ShooterConstants.kShooterP) + .i(ShooterConstants.kShooterI) + .d(ShooterConstants.kShooterD); + + m_hoodConfig.closedLoop + .p(ShooterConstants.kHoodP) + .i(ShooterConstants.kHoodI) + .d(ShooterConstants.kHoodD); + m_hoodConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + + m_shooterMotor.configure(m_shooterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_hoodMotor.configure(m_hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } - public void Spin(AngularVelocity shootSpeedVelocity) { - //activates shooter to shoot balls at specific velocity + public void Aim(Angle angle) { + m_hoodClosedLoopController.setSetpoint(angle.in(Rotations) / ShooterConstants.kHoodGearRatio, + ControlType.kPosition); + } + public void Spin(AngularVelocity shootSpeedVelocity) { m_shooterClosedLoopController.setSetpoint(shootSpeedVelocity.in(RPM), ControlType.kVelocity); } - public void Stop() { + public void StopShooting() { Spin(RPM.of(0)); } @Override public void periodic() { - + } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6e7a617..77077fc 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,21 +1,30 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotations; + import java.util.function.Supplier; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; import com.revrobotics.sim.SparkAbsoluteEncoderSim; import com.revrobotics.sim.SparkMaxSim; import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.system.plant.DCMotor; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; @@ -32,10 +41,16 @@ public class TurretSubsystem extends SubsystemBase { private final PositionBuffer m_positionBuffer = new PositionBuffer(TurretConstants.kPositionBufferLength); - private Supplier m_robotAngleSupplier; + private Supplier m_robotPoseSupplier; + + private SparkMaxConfig m_pidConfig = new SparkMaxConfig(); - public TurretSubsystem(Supplier robotAngleSupplier) { - m_robotAngleSupplier = robotAngleSupplier; + public TurretSubsystem(Supplier robotPoseSupplier) { + m_robotPoseSupplier = robotPoseSupplier; + + m_pidConfig.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); + m_pidConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + m_turretMotor.configure(m_pidConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } public Command moveToAngle(Angle angle) { @@ -44,7 +59,6 @@ public Command moveToAngle(Angle angle) { System.out .println("Angle " + angle + "is bigger than maximum angle " + TurretConstants.kMaxAngle + "."); return; - } else if (angle.lt(TurretConstants.kMinAngle)) { System.out.println( "Angle " + angle + "is to smaller than minimum angle " + TurretConstants.kMinAngle + "."); @@ -55,6 +69,20 @@ public Command moveToAngle(Angle angle) { }); } + public Command pointToHeading(Angle heading) { + return new RunCommand(() -> { + // Very scary code, broke turret with a 2x on the angle + // Pose2d robotPose = m_robotPoseSupplier.get(); + + // double robotRotation = robotPose.getRotation().getRadians(); + + // Angle angle = Radians.of(robotRotation).minus(heading); + + // m_turretClosedLoopController.setSetpoint(angle.in(Rotations), + // ControlType.kPosition); + }, this); + } + public Angle getRotation() { return Rotations.of(m_absoluteEncoder.getPosition()); } From eac8da4005b07049a4effa6d3bc7da732fc25535 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 2 Feb 2026 19:21:11 -0600 Subject: [PATCH 11/20] Camera field calibration device cad --- cad/camera-holder.FCStd | Bin 0 -> 20767 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 cad/camera-holder.FCStd diff --git a/cad/camera-holder.FCStd b/cad/camera-holder.FCStd new file mode 100644 index 0000000000000000000000000000000000000000..7dbfb0eafa165d31463dbab45fad20c1aee9bed1 GIT binary patch literal 20767 zcmb@sbC4*pw&*>!ZQHi7$F^;Ik8RtY*<;(bZQHiKeeSvU+;>iWRqvnIRo&@yCFxqp z%5SZeL|zIQ1O)&900JN?Lr6O#j)unlZ?***5CG=yR}niS7aJ2>XS&}u)>ph%iJ4sW zI}g;Y-6|)Ci(pYCB>MNO8I#21P41IZ(u#MR?W{wq^;5%O^t;lgzDrddKl;9FsQT7X zLleVn&kV^Uev66~OXs!{GA|W9-&nChxr+`m8y;WWafdsgmM&L07O=WMd{6HM&mVd_ zJf2VZ1+UVr&J?`dyy(+c*Ur+kH^IA#R4}}D`#Fsp@bK`8xl5;5yko9lLOZTObyxCz zjjJ_A14g{!36Ih!8lDQs{Vs0eBf~A2K(xiA+MLh5jKdFVFeUWjVgl$nJKH7s#f&dgAApmV7*^h%ZR>2qE z>&&4DqfkNfFaH$8XJt;mLy%$dHjMC58#lE8#K=V7E?~%D_9|{%K`=G+gd6Z)SM!Hr z;azoYImBAKXEpdh%ODe!mkSM7q?-$1A}NGi&cK3@L-7IbHq%(%cybr^gCFF?M8NWQ z9MD-!Gm7XL%a`z=RlmfKEp)djp@q41(6R}zGjaIeLk!jLg!phrd#^!{prj7d;Ob>)l*@{694=~vWFfkVh$ZuL#zM{h2&Ye{3#A9 zT6cvaI@`o!CAL~@M4N=ZvW#Ba+a{S&>V`HJE)A1?AQ98bOljqBPd;9xoJgnsp#{)T ze${W+9ke5m_{gi)qqh85XRgfIdki``xP#ss@xyGR5AyxF!5@QD1>pa_%IHf1dh?)l z24;ahQTT1ENgp)l&A0e*N22~M6ze+0_|d~~xKYM=PwTm7c)r^IKsVs}dGqt|^QH!= zAM*6jP5z$U{&U)(BhYe#N&s&ny^X3Jax0#O0f)TOgLyC}wRgFoA%1b!%F9-TbnxPa zVRkI%M54{mnDbHk*wvi5(|AVn8N0{39nY>?jd=Jn&Y{)D!HzKS$`7NjZN>8eq6)WlgaMb+A0q7Pzsh1cL< z+AE2~uIiwAGk#hXXRGj@PAR@HUTXgp=&@z+D_G>CW*u6(VK&zV2iC6$e)nVRjDBIN zH!o3bkFBQvWqNCAwANfTd1I3bS2nreFb$T8D715NSn^7?Y`vw}a>hcT@!O{DJv-sa z42Htq^%3&jmpRMo+=gwc!UiR~57W3$;jMzKJVNWDrh600KaGNUdmI9(fNVnFi849T zMB{dnagwDUbs_6ykrjM-OoGC?Ds7k5^IH%uaqWubRiIGBa9R_|gOFhy_*?&dVo(za zfv{8&tg!(kV#05m&Od=?cCEh|DS#wMbwY55PS~nAj~mjQ7nti%={zeds3$mBK`QDr(ECwSU))Fkzg$b0w1&1gYo8<(d}r^_>A-R{m5B$KR=jfeT7uhw1~g?>)0 zkP5SjO#_lxhZ`UeyGzQ<9Nl=G1{&l5lu~<{BawrW!uYS<+&ZQpCM-Q03UeAK{_ec_ za@y>WX{~|c)THQv{Iil>%F$PoPMtTd-`bxU9E(rnDK@G=2E!Y$HOi7{ACg$*5~Gd4 zDIA5g!q#k^euavVmv+JRVK_Sl14$9o%>6A)iWN?ccZPY0NSTQFgW=3tG6E|3DZi4B zhzWDL8qpkU5nKTq&bVkoHvfc)K8szWs%&G0&9M1GYoM|IsqSETa->qt;MkKIum@Fa zeIHsbB_a3G>|Zh(|M0!v`AN;X$1Ca#rMywFKWM-IPSX2wf_l$Yn*I5dvwgspU3i*J zn*=SlYaBcd;|(6NQ0k|G*ejdxtPt}BEr~=g)OY2>LdMrIT?uLF`DRKAST6E3OQ4cL zo1M$P|9q``BBMg}k^fcraQ1t0G8uCrMDB`Rnqww&^a{9|Xk>f1rkI_`>b{=P!}Xx# ztAUDB8-EG;kJ8Se(uYS+Nr5NggpQ7~fE@D8k|%{BDmGI>hu{2%OlX|@i;h545k)i; zg%`8V!!}O(wo$wiuPC)4=ACS3!>uOoDfCWAic-x>3jS}p%(K{IU|jwZPZjsmR4MDB!;O2axMz%-CN0J z*%J3r`A$kjj;nkooV^xOULvsk5}ye+RoSDdH*Yq}L6|n9tm+WQ%Xv$DM6&qMO+kEp zQj8E)bqhR5(Hi)*GydxkUb3SzF=#rsirl5v>9r~TtcCG4*h4pK`i6zEZMHF;Y-tK$~8?pZ`~3v#3A!z2BbEC!r}%_%NTAEJbp=J?Lc$&fqNjxh8HfuxYG85Gm%J=nw;0Ml+OD;@$HQg zI(P2q!>W?QE0%;7-Gmm-y{gBcmocgBQ|cVrx0T?S+L9bI=TK@Nm$8)%$C5yE-eQMy zXL^~NkUBiz>-)Ex@q8U-(2u;SiFlje4|#xl{Qyg}QE?%QV~SRFw_udwWQN#&pQ~U_ z@;*_(q!S%f-#Q=CdIb;C;G}RO-D=3-6t4j_)?&>T?q}Qb7CY;9OzG^giO~V>lN)j( zW$CQ^dU1UK)QEltf!*C7n>QUSa+z%4zdCprG^MAXoSEuy>6eP{Jv?$>(ibZwFRcP_ zdW1f{Z|>xDx6XKXETP3vFAN(5a~Y?0Il;6#oZ#n&M)DN4M2jm8=vOj)iBl*Kju{&0 ztJ1-s?Dfk}HxlU7Gk&BN90mmZ`s~!2v3ax?s%@ThTSPDxq3{d)GxN`A#sf-9DQeqi zFsHlU@4Xv1b--LO{LVn@VBA%= z$2?;>1B#vk?K=ZPx6>?u;6cmiTV$|wGL=9}@1fdfbt;CDmCn8c%B7Oo3nKr{@=_H4 z=|#!FIa{YBF*{0Xaw_sFn6>~i*48b^dnyYcDFvvcl2<~_)2Feb7MUXnLa6X#QH3-J z+OrKSmm`w{4iih5CKFYgVHCNLnHw?7g>u>(vb&sjKMY& zQFt-?v_tR}o+jG~Z? zAfE<8ibX(m>w6@DG88LXMA<%0MUCOFLrox>N}!&B?wwglBot1GqM`t4KpX-hR4ar^ z!+2Fx%A^Qrd?;A1OB`RoN6RRMHW4bL4V7C|l>__8m|)y2z?y!FK`9PKwHFtp596FH zfO#gMRNHQqP&%2WxQ`Tkm;h2GhlD8JUT!Elf^jaO!h-2!UVs2Q7G=uyQCN|7nL$P| z;EF6rNZo^@l1E1mC9mW$cY zj-0XK3)+)2%C$Ph`UAu!u0E} zm;Ps7B-{y%u0l3`I0|b0M_0TFP`XHA)Cf5oK$^YXp2x18>NA2zv*$v+ckGTjPW#VA zzqM)}g#kwPS)fvTLM5UGG4+v75n`0JMX?~*@}Oo?<^iw4SMLF@z0eF&%5}u93JvJmZ!17HD}x>@L0KK>yk#o`1>GS! zTlWyBYzb)J%svPDsqF;SeF_gw+-S6zyx@RMp*%oLi+k1XgWbKfA{WFCA>gwe`8#W< zDy+-`phO~gOVIi(jH<4)@UUZXlf= z6S6AhrImPzA=b9XGX~0X-o%0dvw59|Q-``zMNc#pxLnbeg3H)s(HZFy?hF%llBN?` zNA*oiBsGpgzUioN6cU3*l=z_^M-GasSNu^4k(iG)1YRl`gDJUYX0~5D4N%+~!7Y^d zW}1PBU*D{c@amLbhL4l_Zd83B8!PE8OY0BK9;=rwyFnggWtny*;o3rTHzJp5pZ=^`xyDVOubqycM3+ao9qS4>~TYH-Fk(_vOYZh;gTlZm) zTM(rli!TkM6Y^xakQL|xK_od;ZP0*+y<|351LIYqUaw`lKHU1nhA`yUXL$PuM3lEHhvGSB>*O{drpHEgS$K9exq>M0FA8RZ@*#pvT;$Fb1wj3eb4j2lcdab$-Apo@>#C ztFRYikexrR#&A;4M%gs-ftamVTVaw@@v7T`Rz}+0vbL9t4@u(T$4WH$a$3QG1lBQM z^FR}^hjHvdxS{s1=kpA(dd?X2c4I1`6_+yi{$P*>nMrwBz!PMo=7~s+5|HafAiTE? z(O5+wu(sNP)_L7$gVr)`=8x9RZ=P?try$)t?l1QrQ>m%szCe{(rSx>gwabhoa8r-D zn-uBU+7u2O3zE0sP^7P(nk*7hx1l)YWQsSW*f1S7irsdl13{{mN zq(x&5(m4H~+RA(`S_0A`^FFb5A4xO#V2iTv95fkmD49iaX=K)5vT=v`qPYC}Dio7Y z*;Lx2#tLSgaWhNXQ^io{laHCzsELV9*hhpUhCAFI{{n=fF_7E&igG9c03pl@E_vm~ z!p1o0fs~gjT(xFD7SG}=187p6c$a8|AOvPz)?SHfC=yJgDLJBYN^zz9&IgfBt3y>4 zcJZq63M7yy3h@O%35hDI_s)U~tYIoLs^xG++!_{4LaY0+P{s4a*k~l7X}-rTo#pOB z^45-tt(JQiCl}05#|EltVxJ^YCg!qzK(jDmnUGd8YSl!->M?|s%HZtkVBskQHAh;8 z^o!TTBktKAoZi$G-wsF}7hUZ|pLiVq?%f1$w+S+C;xP|sa$-V>uBOqxV^x}~61O>Q zd8CI0EmHwXn(gku?Pg>))qBuhKoe{0`9l^WW+$GHX{P1F_BA)6&hfXzi&KYMnDxVo zHTFQqH;aa(mXVp2FKe)UcA+)@yxy>Vm z+H+p5(waXyyc_{G-fW4sf*UJYQt3X`IXA^)hS&IL;Z?Ar}MbJeT-dI+T19c6dH~<^OGo^7w`kE%EQtq&^@3 z0NUSF$=T7u)=b>M$=t+|PT9cF+Jw&ew=*;y<@lr&yoB_)1T38d4V3}~voZtI(LC!S zvphp>J=`-~UGqHy3+tRiaCmVbM#z}QFOLxQq+tKSgeMr(dDsaULTg}z4TxzrAwz89 zI#}2^7IlKUsrXe>69}m-oNU4r&Gge&e-RnK=X27&qpHm+VxfRM$;tSnqFqIx$HL+q zh5DT096}BVKWX4F`Y}^LfPWX`bU)8z0t5h{fg1n-?ynd^cE;{>O6CUkCUl05_E#Ai zb{iZBBRfx&udOyIV4)?C>LgbSx_*JrYvK(Segt%mD5otu>G9d>XrxL>v6UADgrL82 zFuzYmq*NWg4LrX0_6u9Hd~go*x+!(HlzEVmC%M-n{QdJ}sYv z&|`tT070ddAH+&4Qwp|{%-Y4}^Yt3=Y#Xh3z$pH9^BQ@!ke*^us&3`L^gPP}x-MbT;W=wr!B(j>o{`Mf-s`q8* zhfcl3mv}z5k$G=pJ$*VYUQ&$M>c~1CULD3rM>}+c6(SMeqk^BVj>Mj=zC}K_H}TWZ zmF$@AIQU{H-@Uybxqzzs6ot_g#|JlFRmmIU!UBGy-H2g^$_(@S(27j71Nc}0f~Zi| z=%7GnI=iDWAQ5^8ANlPb$ri_R&;BwqKLEBm(8z>+mRam}%N(Si6YO;?1kOAFc!28F z={@(pJ8~_<_^J_!oad&ig4mzL=Hc73$u*_0Wl0IKNoXXl;1)HN8*Iff{LRuv6^w!3 zQnN`$28o#*AU*ofTPsgaG|504Gv?r*=FpvsxurkcCOCPCl#Yv_WQuSAgIuT-R#yK9 zAjfD{28bbT3 z`|_tbkPqj8>|1gBb8sv!E~3>{k^o^YCV$;rJXwa2KoF-k&MYS0tb~3{S%z~Vkd-B@ zzF~$Am$G#IQ}R`INYw!dTi)N0S|A*TUMxK-Ts@7I#Jz;T`Z`FxTtxttDqM_dw{gM@ z!~fL~9tHwDVTwNkl`zELcKS0+VcIBg2`g>*`}6%7x}tdWk?U9k7OZ3Wo=WgOVC~p@ ziTXG@e#1)FN)oNY5QLb{;RN_YhL8k+TU*)-Qa3=Tvi4C4F~~7FDx`=o&?$C-q{0Tc z$Ls89D&;*wADcJla_Zu((0Mfr>PsYIhALMpP z)RXIHKpEP(DKRgIH9qU9GV)ZcH!x)R75}QKo=(CLCEGvZW<^hn{CmCaWToM$+3n?z zF8k21wafzF8GXH3@94yJiB1)K;+EY4(r{j>C2+AWR$AuGG7iDa_CxHNRSs{hx~Zjd z7U#osI$Rdg{k?5K@#Tas^n0rGS$OhuMND= zg))a98JwI8ZUy~V)0u)`DP3XqowJGM|`YIy?u=LEQ#eJIj#hpWxni92X zO?l?!w%uS2!yOhKELLA~B9N&fqpezoX0AL`Ay>NXnCQmN7&J2h=rKrzV+>sUk%rj+ zVOsx=2LRAN9)B;4zqgW=iL;S8orJBkiKDH7^}nnW@PF7cK3FmST|9&k_& z{u6fuAWdQDLxvILaeOhV&;RR=^7sY&-=Q)?juw&nSMON=g5@83+5fG#|J1+##!7Ik zAY1?gg6PW+3a`IK86XTdBy}OzlXYw5Li7kfJY+f|CRRYi;?9pwbthsVDq4w#uxsbzpwm&Ck3e+p7s!${dVeI{~tBknBuKWv12rLOy zp{izQIs>cIlyo{Kvl-P}{1NAm=I4Cj=X{dL!rXk?M8VOH)>y7 z^p}jo)Bpg;e+7{@Fs2hUHdb;m{NK5xp&_>+fZ(&ER~Jj`T$s)4#Zw@V>mecdxCR6j zFCW!Ucz4sm&1Mm`GQDaG6GXyLx17n@ZD-0;xAthx#>>UGSMp}#lc>jg$NYCHW&0lQ zG<=wl#WZ|eI{2{9pkHeFMm*dJp#!H)~kdz)HN>7E_&a`|*}XwNc>tO(7#U(Pgc2tCTZ+?i zw==@PHBKB;>wNzX&iR-+-CGNe$O4A8^}6lQ4nAY^ILvxla+UW?QTpZZdihAWOQ6T; z1`};yG$`wj*y;&N_4uUo9^PkJh{T<(W7cA;%mm5Zq0Q^+k1FdQDY=`V*fLzSa*g8u zVFZ487;u;2>XozKc8i}x&QrN~1`jXOuVd!Kv=8olGKAKt=G_@P`vK9llcxv%B<70D zg9IyXw)^tQiQ`K)%#2p-V2i_bw)L9P)*Bd;XGno+rHm=!MbayS-wOx`4zGK(LJ- zX;LE)=5$5s%u2+_Tgu4d)Ub`Lc;*q-092crM7C$}X6pRPRDOhJTMO!SvduA{J-7q) zA@CDcwYP6H&WCqK#QbPCSzXQP<%ld{x_7Q8@9N5EHC%1Ytu$fXy^80Nn98#W`?D3Y zFHe@23^~7x$=PbGXDz(?hd_n5u-VR$t#rxGk;-PEZ|`}iz!UJYH)Y*>e%Jc})Q>*5 z%eT#}oXsJY5@^%`|JeY)D*tRb|9NlE&9U>h_tVQR{slhJQ*5+k64C|!`UQX4Jw5P1 z(2NhiiDZ^{M0~;aZy=*EsLQ}!K8k17wlr03>Ca;IK?>p|wBoyazp3gK9ko;EsQS}z zN=X>tTZ~98lsQf%p${ZicdiM{z3ebvJqbqK)Y<8PNZKsPI3I zQ-aPv9H)?*nHuwg)IVn{%XK=Gm}wZ%3MAWTs$`Hbg^9NhIvc`~7}f7NCueS+W*aVQ zc9+YpUxjZZo^TO7H{Bmg?+n{t&b)g#=YN_Q7^R?{K9)9sm5;QZ_&m$TJ#)$wcNTv} zy$BZgje(X9U89N}k2Jv9$C9%1`dvJ{(}L&UlqUg0uJ+*oWo3KAiIDnp3_k0&S@O|= ze=})eL&XCIUQ|S(4K}seRir2#SWQdHCdXnWi%ZIj7Slr|F-e5iTdD+5#bDs78IEO` z!>$vJ5siaNW*5achBL`AiDBwq@2kMvT!4-44RS(C43ZcrGNz|Rq&Z8wOJ`Z;UdH{h zmHFgJbt~x8o61=bxunr*yP!R8@(y24AI(dcsuOOIb_xzESY$?xwi}V&7G8gcR(?{_VK6r z?`wv>>JdhUyze-AGE;nlz&}5@IPwnSKA{0p&ATMV3ts8yTWp*hPowJ7GnJ z>f3pQvV2n5_bSA z+x?EK>v1L;h#@kEp?SaSI&c>I2ZT|d7TC&pp)7r)aP3 zcfX_WgF>sE-zS8P+N>UBO+WpvJ68imcR=R5A&?ES%U%P5yinZjPh{W1ytfPvspdJ}7X(ya@W3xdh^QJ3eXvONd0 zFNL(15LTMe_q70cZ~Fh0+w@w<+$ z3Ggd;r4NKMzy4MYi7LOdny=~seu8ecFZQDncH6Kr;K6H)@axDqxaLy{@9Rrd?sShG zD)8&bd#J#dQ6Um__!IorlikJVL@&c$!;V}!l3A{0kvUsVFGc~-2SF`-xwj47F)Ml% zJ!5>nv;rALbSk*DYu9zVPziIAirl1vA%@?I`es4Ibnd;-N4ax27d2+!I5ZiG$Emgp z=iPs}m?5ry$p6D&#D7D={~P*Zx^H{GR6H4Uew=yqb1uqQ85pLZ9Y0sJfR~Q9oO?ej zCcp3~6?d2Y#J>sr@f`&zAG^YkIvQ_)vrVAn77V_4d}n|zeke-=hS?ax1;d@mL7bh1ngmwxJLKgQ>1z(nMG}?yl05jjrLO=2YRcR3@SnL{09dkRK-@9WPgb zEaxzE)($7NDquB_B#ERzr*KK&8Ydm+8N)I8-4?0F*ja;35Q=<4LV}nWB{5`dNM$(B zc*N*X?p-g8)y8`FpuHUq=F93Si@t{tV!GG-ydQ=ZCL$yy!jyl^5m!7$Q%{tSN`xD) zA9f$qDkkqXb=$Lx{_pht+q?gYLjT9yDY@9&JDNB-nHc|z!~YqC{-4#b|7QLf1TZi# z{@>$Qi6i*i-oFO69{Qhd_y1P?*WbF7=WGKQP`q!{9v!eN&Wce^3k7$Yt;2_TkzHa2 z{Qz0-ZdTBA3aUoeWeA?uQ=H&RUUZXS$=B>zh8HZAsw$QMXLd39C-LvBx~rU+##`RH z%_GW1=$CkQ#`F684teuV(ErFXm(GwG)+3_9z1q;Qr&DY!eVDYGGuRA(l|V4;ft6f6 zQ9I2#<=V(5cshKXwhiM7$_WlWOoZcirILu^2LqF-XU(H<#0udt5i%g7cOd(5&+Bm! zB^>f6#MBN3ZZcn79(F~)NOinedaJ9DVvnKON9~MT=o}tdw(va*Mv!&9Gw0H)m{;bX z9a8U%vLcu_f1b(M?+c`KUN2-LsO%zv9|q=JuKEJ=>JnJ%8^y6FpA`DglnS(kZxoa> z0QfKM6**qgZ&$k7J>{!(NjTH-E$ts%?2mabGPB3m!Jrr4qg-I^7-orZ0eAQbHv9y zj|ZVRztymHs{7QNVym)e=s(z$?rlMZ3;_Tj918#d|5q9@7mI&8x2>b)w9kR&GgC8L ztFl0kJfdc8mYG9DxW%irwN%#ohn`*s3SzdgPP<;b-}cH^4>A)Z4A6+10O>9cNGgR7 z|3llE5BKCM_oWbElL*TQiESGr;~OlmR?33|hh;lT<7d_L;iRLmr3V)u-~Rnq{ZGcK74bN*u z$BL;#wtg~1e0QOYe7xDcXCepv}XEi~lJXb|=oo9woO#IXA zwhgb_LVlM8$Knu!fXxhON-U_(A;`{E3y3@Q=D0CvLahw?aMY{+GzIK zALNl8{~?`a76ihnA7RU!{N>h#2K4f3~_(zfR#4TPSOt`rU$7O0UG4{-}-QrG5 z2aPVK5I~kM5pNvC3(*l+cmaAH;<+X+AFu_HxfQiQ3MR%2g0j=9L4u(0k#Dar&H=Ry zFGB%OGi+Kyz?`hP(S*1>_?y|f+2XW%;HiiyN=9aq zVTfTg=zTR!tFl1}0B4J&JE+iRk!R$=dt$i@>vYe@7pJgsPvM>*MqZ60u`w85kQis= z6^$1!Je1?G)Cf>?A(f9PYnP5DAZbJa+2AebxtEw@H#Oa-Ey$X}Cu$^Zgji+tf%>{` z&neB};cO@==!5h!?(_m7*oGvF{anI4h7M9TmoPl}8}abL{OkTzrB}s6FMC@Kbba_o zFf{)qynP@O^-j#6AT7LDleDxoIi8it65b$)0}teFU|G}(mAs^2#!b%UYmBA=L~x4A z7x3`9rglJWlWFT^I_unYzg?s;((z1Y$?X$sng|z5$>{Dh~->p>@X+r1k=|!VU zhPMO!U-u)ayl;teF$pBTUg1jA+yl+tmVb|5Z2eLF6qc-d9a5xD7I&UtUH;Y9eax!b zw<#%WN33LPV)ks_u{oa8*sX6%+Okd`t*3btNurjtZ-;dHJeKTD`8^?7eL0DSDp&nF zlt?|iT~FPnivBTHeR-ME6LdNLg3nJ6j8VYap_y`&UiZ!RXn0{~q1LE)1J41neqrJs zpPF63uICfuuE)vb`FQRJ_Bzx z(E%;59sX@woyxog}TQ?KXKi}HYItx$mo|A)M{VUD9l20oG9hA1_bl6jwnQ3L`EQ$>Vbv; zFMU|6Dh*(UWrQkJ!irRZh5LYWHQMg=I+^G(GK(IFRDT-_{Ds&yycW=rRZ9rjv-vrt z!plE+Scy5BdCS#p!G_gzSS%Nymqx(1wNodLbf+>W%)M*?XEj-ck(I~tOS(3@PdC(e z%(pOg7;tN8QEr)Y7fmh_e*qj5glBi(AE~A8v#*pp=7?nEPX!L2b#<@tML5mE{p2{g zNFG zml^uJ$DPqnfI^ZeXZS)Lv`_>?sZdp0(aw0#05DsSWaT96d#3FOy|ZP)C;cZ2x*L7&4@5EYN&Bq z3;}A&C&122`g0Ta=z6CP&6I{G*K8k%!Kl!R8d)W1Ee?kAOL$WT`{(0>ALc6;VDp}E zFlS_jAwuNMu;Ho3w}qR3CNr15I>RD2Y*3wtba&N-b;8AF1AHXJ!qR1cnRk~X!=SDC ztau9&bsa8|*?HHmv+UMEQ|Km zWr`7YAt!n(*NWLi&B<%PVV;eG3T!nw4FjX$y9hcklo>`xE*3tX!U}kw2u90o>TK^U z_ebU@g8TWoNVHvBROtEo^ylBbA}}1i(BJqMx8AL0)y=|&ZNYR9MemX9rlL%ZjWMN`joqbE>djz{B$yK}KQu?XS*VQV z6q_$;m7`$h%&i$y(z&mF?R`7=H}R+4r`?-p+;bjiP6jj6u^{dx?g0S^%)qg3M9M6|8}oRs9xT zEppkK`<1;T4EG|d5ZUM6R-XrVVUU&U;Ej0GA@9^dGH^`#GAm_^Snr6b-K8*^Juv~? z<${SU+=5GOs{oI8rQ*-+U9+;boyp%S-}QWJ+N$QiaeMPTl*~w_5)T~DxO)x)P)!g+ z;RZrLG78EQ4mO)11aJlcfguVEA%O=l@#eelD^^Kbftsn2tLe`D=wG*dwb9vPw{jo< zP%lX4*Ja>l=pht76gUVT3=Nj`MDTw2DyEKunNo$UTjd ze5t+V4O@AE-6Ixb0;st1P=q6nP7pib$IAIOnMRp_U2H57bth-s8Ds$tH+7H`GtK$= zh}lkxK7@Zj5yvUXvL*wkF|UQ23!>@gMx&!hHK8Nh_N-v&v<7(YP-|RJ0oP5KIhCe^ zaz!x6gHkjzNZP#C%xSDj9nTPZAqwiLof3}(=kU0LyjokNAA82icaf`GYI9E#jIk&2 zh9bhWT0ek_@iCElcgpA9%8kU4egDcDg8Q`0!xDH(1mF#*r5p zd13yCgi4KPgS2Ps*4*T4ko0LIe0+>2Br1yVD<-YEo+eGbFI_jW@%%vaL^{P<@5#R0 z!_6e04?GpFNZae;EAzqIz}A@GJs2M2yUaqZ4*}7o2Wd_&ro@Y_;Pm`a&|gjqTtyd5a=ePTN>+Km_W;IA(Pt*SH;_Zvu_;jp# zzUP@^>1l*YT^Hc@edww7CRaXJOxJ{fvEn^my(_2C*K1Vvkm^TW;m4!2w0%$@JUNdY z(HwW!Wsl0p&$~-M_JgHv^|G9ewYNWHyqN5?**#Y{mp`ApA1TAC4`+SqgB`uR@%W-O zA}I{O=RkdS5%DbE7=J*59KG1jLow(VM$ppXMGtYufD!FBzXk@A!AfZpe@bgOPjGe8@t&keDGeN1 z%VTT~^#a)|c8gfAf^)OdE7P!TN(`lkH~O1X#Ke?%4si56e=#msUFG8BT9W5@9>H;n zr|n#{>#KN`(zsL$2vojv*jh3-<1GN^EE-_q1&3^4Jstga$K@e%@%s98L_ax!b$8XwbjjbJfjDBwaYSY{{i51Y2nF`%xt7we`=~Yj)-KJTuy| zm)p&@j*q66?C#oZp#<4A^&x|@t`c4`K6X|{0Hhv-&WLWBUNH5PU%V5w6{!d?aG($w z_yB;R9SMCxQn$Y1kAn#{%tvRL#N~_zdQlC1TNbW(@FeFk{&eJI(&>`+NgxUn>Gl_a z;|TJT$;tLhwg=F})Xc#H$9ba3c0Q!Ij?UXN!&tcU9Y{n3t+gF6e_>Y#cy$F3K0~$` znj8q7XPSnhxz};B@dh%DbtW5c9hw@>i)`afoY;CLu~sd24?U6! zP)@|n)gJ7OH!@-{4r{dj_=%AgaIQHvm zA5zIWiEO8XQBhGP?WR&@RJ`x-kOTwG5r&ZMs=UuK2xnXy$Z~@FxW2HVMBlDU{%DpEuZTR24-nGib;D5e88H} zmP@*RCZ#ygQuhayyPG1@57(3yF`_PLosv_f&#jn+^z2IA_hy3Q$c^3i`5(o5J|crV z+Ln7{F&}ES3lioly@NY-rEm0n&p=a?S!waXmvR-L8M!bHgm@Tf?MMe2n_6t z6_989waC_h+IyJ8a9E6I{$B&D@TeY}fs(d6@UUQ)N_7(-tEvJPGDd{pH2hT^Y0 zjl=Zdr{$nH4bPMiqGQ$|`5d2&otiI!sYzGt=vL86+3WqfX1{=)mJP<4Kojj_c3&uK6Q_g>zFK8 zVlOZi?;WUh@e=6r@$|l<0Rp1ACHm^2ePmOHuIRSkQ_WNNyG|hDyLOM6VKG?QKWiz`!I5VTDV;Ykt zx7jYGcm(W>fN*RmmWsg~43zo^)r-gyx#f{`<1m!0e0?dv^9Ol&3`sXZZNzQ{-O7pQ zNlbzP;O59*OAUq#YNucU8RQzcF8q)F4`;Jo8e!A-P3Dwq$wkiIM$2Tq{(N#zcx((& zw_R%(=;-WxIlnZ!PgO0WxRk3tzkwC9GuApLT;Xo%9$YviC=DWQVB#w0>#2>QFD}fq zB4pd@ur|uLmR#lWK~bzMD_uUeE@RnfcSg^R$@usW*%C1mOIb+=U@^rfYr!loJCndRJ`fS;oT!W{;Fz5VYl0XV@}B6 zSGJCNx5p<3EzV`k9Y+tBq7R0mhZB~D2A9+8^i;X3sw*phWN9?NLj9wQ0h{f0Ap59R zJ9957`7tx%zw0V*A8!l}4_Zba4qtb+U-bn2!JET_fyX@aGJIykTI>xMAR8TCP|SNwP42oIU!W0XyaJ?B|M~Z zl!!@q%O}P$497k|4$t^^exL@tVnZ}))8U01$U>hwVVxQgyLMCvdHlT4N=^7b=x}Xm=_M0b19Hz?~dgKq3lg z$08!iJuwl<5|2W?`_R9V>oHi!F8eoVQ!Nvr>aqJ5uLXMtg zX9Kh;5TR9(kYqLBu{RM|Ku$LjNlZYZCh)YGh&d;y#NyS6ZyZ~LSg>7zuyAb=!@UTOmW}LMAS{ep46BM@)Vf44UhMhCu@(e_ zuqj#?x0r50G>Ds`;o?O4UZCkb7`Zf^Yo+?mhcw(gkiMG`4R%V2U~$?*2nI!?0+%k@ zJRP_`QQzXxK2r<>Pnfkh@D2onz$scf@K=ZiqiukJ!~G1Yc)pFI3m`2$+81dWxpKs- yKHR^MzC$!^gf!f?5L$VHirnNV*`&lz Date: Tue, 3 Feb 2026 17:33:51 -0600 Subject: [PATCH 12/20] Turret fix maybe --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../frc/robot/subsystems/TurretSubsystem.java | 19 +++++-------------- 2 files changed, 8 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 176d1b5..83519cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radian; +import static edu.wpi.first.units.Units.Radians; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -55,9 +57,7 @@ public RobotContainer() { } private void configureBindings() { - m_driverController.a() - .whileTrue( - m_turret.pointToHeading(Degrees.of(90.0))); + m_driverController.a().whileTrue(new RunCommand(() -> m_turret.moveToAngle(Radians.of(Math.PI)))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 77077fc..ad91d5a 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -53,8 +53,7 @@ public TurretSubsystem(Supplier robotPoseSupplier) { m_turretMotor.configure(m_pidConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } - public Command moveToAngle(Angle angle) { - return new InstantCommand(() -> { + public void moveToAngle(Angle angle) { if (angle.gt(TurretConstants.kMaxAngle)) { System.out .println("Angle " + angle + "is bigger than maximum angle " + TurretConstants.kMaxAngle + "."); @@ -66,21 +65,13 @@ public Command moveToAngle(Angle angle) { } m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); - }); } - public Command pointToHeading(Angle heading) { - return new RunCommand(() -> { - // Very scary code, broke turret with a 2x on the angle - // Pose2d robotPose = m_robotPoseSupplier.get(); + public void pointToHeading(Angle heading) { + Angle robotHeading = Radians.of(m_robotPoseSupplier.get().getRotation().getRadians()); + Angle targetAngle = heading.minus(robotHeading); - // double robotRotation = robotPose.getRotation().getRadians(); - - // Angle angle = Radians.of(robotRotation).minus(heading); - - // m_turretClosedLoopController.setSetpoint(angle.in(Rotations), - // ControlType.kPosition); - }, this); + moveToAngle(targetAngle); } public Angle getRotation() { From c56ec5c6e3eca5f073f0d5a36913343cb4321ab7 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 3 Feb 2026 18:21:54 -0600 Subject: [PATCH 13/20] Working move to heading --- src/main/java/frc/robot/Constants.java | 5 ++- src/main/java/frc/robot/Robot.java | 30 +++++++++----- src/main/java/frc/robot/RobotContainer.java | 5 ++- .../frc/robot/subsystems/TurretSubsystem.java | 39 ++++++++++++------- 4 files changed, 52 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 23498bc..07044c3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -163,13 +163,16 @@ public static final class NumericalConstants { public static final class TurretConstants { public static final int kMotorId = 20; public static final Angle kMinAngle = Radians.of(0.0); - public static final Angle kMaxAngle = Radians.of(1.0 * Math.PI); + public static final Angle kMaxAngle = Radians.of(2.0 * Math.PI); public static final int kPositionBufferLength = 4000; public static final Time kEncoderReadingDelay = Seconds.of(0.005); public static final Time kEncoderReadInterval = Seconds.of(0.01); + public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); + public static final Angle kNoRotation = Radians.of(0.0); + public static final double kP = 1.5; public static final double kI = 0.0; public static final double kD = 0.0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9bd5e50..6cd1383 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,7 +16,8 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); - addPeriodic(m_robotContainer.pushTurretEncoderReading(), Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); + addPeriodic(m_robotContainer.pushTurretEncoderReading(), + Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); } @Override @@ -25,13 +26,16 @@ public void robotPeriodic() { } @Override - public void disabledInit() {} + public void disabledInit() { + } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } @Override - public void disabledExit() {} + public void disabledExit() { + } @Override public void autonomousInit() { @@ -43,10 +47,12 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override - public void autonomousExit() {} + public void autonomousExit() { + } @Override public void teleopInit() { @@ -56,10 +62,12 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override - public void teleopExit() {} + public void teleopExit() { + } @Override public void testInit() { @@ -67,8 +75,10 @@ public void testInit() { } @Override - public void testPeriodic() {} + public void testPeriodic() { + } @Override - public void testExit() {} + public void testExit() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 83519cb..9fb8209 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -54,10 +54,13 @@ public RobotContainer() { -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), true), m_drive)); + + // TODO: Get rid of this + m_turret.setDefaultCommand(new RunCommand(() -> m_turret.pointToHeading(Radians.of(Math.PI / 4)), m_turret)); } private void configureBindings() { - m_driverController.a().whileTrue(new RunCommand(() -> m_turret.moveToAngle(Radians.of(Math.PI)))); + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index ad91d5a..d3a0043 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -54,24 +54,33 @@ public TurretSubsystem(Supplier robotPoseSupplier) { } public void moveToAngle(Angle angle) { - if (angle.gt(TurretConstants.kMaxAngle)) { - System.out - .println("Angle " + angle + "is bigger than maximum angle " + TurretConstants.kMaxAngle + "."); - return; - } else if (angle.lt(TurretConstants.kMinAngle)) { - System.out.println( - "Angle " + angle + "is to smaller than minimum angle " + TurretConstants.kMinAngle + "."); - return; - } - - m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); + + if (angle.gt(TurretConstants.kFullRotation)) { + angle = angle.minus(TurretConstants.kFullRotation); + } else if (angle.lt(TurretConstants.kNoRotation)) { + angle = angle.plus(TurretConstants.kFullRotation); + } + + if (angle.gt(TurretConstants.kMaxAngle)) { + System.out + .println("Angle " + angle + "is bigger than maximum angle " + + TurretConstants.kMaxAngle + "."); + return; + } else if (angle.lt(TurretConstants.kMinAngle)) { + System.out.println( + "Angle " + angle + "is to smaller than minimum angle " + + TurretConstants.kMinAngle + "."); + return; + } + + m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); } public void pointToHeading(Angle heading) { - Angle robotHeading = Radians.of(m_robotPoseSupplier.get().getRotation().getRadians()); - Angle targetAngle = heading.minus(robotHeading); - - moveToAngle(targetAngle); + // moveToAngle( + // heading.minus(Radians.of(m_robotPoseSupplier.get().getRotation().getRadians() + // - heading.magnitude()))); + moveToAngle(Radians.of(m_robotPoseSupplier.get().getRotation().getRadians() - heading.in(Radians))); } public Angle getRotation() { From d7c7163eec258bae27f00d42097998937b9745f6 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 3 Feb 2026 18:35:05 -0600 Subject: [PATCH 14/20] Make build --- src/main/java/frc/robot/Constants.java | 39 +++++++++++++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 3 +- 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ad3beab..403872c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,6 +11,7 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -185,6 +186,44 @@ public static final class VisionConstants { public static final Matrix kVisionStdDevs = VecBuilder.fill(1, 1, 1); } + public static final class NumericalConstants { + public static final double kEpsilon = 1e-6; + } + + public static final class TurretConstants { + public static final int kMotorId = 20; + public static final Angle kMinAngle = Radians.of(0.0); + public static final Angle kMaxAngle = Radians.of(2.0 * Math.PI); + + public static final int kPositionBufferLength = 4000; + public static final Time kEncoderReadingDelay = Seconds.of(0.005); + + public static final Time kEncoderReadInterval = Seconds.of(0.01); + + public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); + public static final Angle kNoRotation = Radians.of(0.0); + + public static final double kP = 1.5; + public static final double kI = 0.0; + public static final double kD = 0.0; + } + + public static final class ShooterConstants { + public static final int kShooterMotorId = 30; + public static final int kHoodMotorId = 31; + + public static final double kHoodP = 0.1; + public static final double kHoodI = 0.0; + public static final double kHoodD = 0.0; + + public static final double kShooterP = 0.1; + public static final double kShooterI = 0.0; + public static final double kShooterD = 0.0; + + // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part + public static final double kHoodGearRatio = (62 / 25) * (14 / 218); + } + public static final class Fixtures { public static final Pose2d kRedAllianceHub = new Pose2d(); public static final Pose2d kBlueAllianceHub = new Pose2d(); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 27f0eb8..e7f574c 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -30,6 +30,7 @@ import frc.robot.utils.VisionEstimation; import frc.robot.utils.Vision; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.VisionConstants; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; @@ -186,7 +187,7 @@ public Command enableFacePose(Pose2d fixture) { double totalDistance = Math.hypot(xFixtureDist, yFixtureDist); // Floating point value correction - if (Math.abs(totalDistance) < Constants.NumericalConstants.kEpsilon) + if (Math.abs(totalDistance) < NumericalConstants.kEpsilon) return; m_targetAutoAngle = Radians.of(Math.atan2(yFixtureDist, xFixtureDist)); From 00abd66fbec84ab313e2535b54b5658bc151dd6a Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 3 Feb 2026 20:13:40 -0600 Subject: [PATCH 15/20] Small fixes to constants and vision --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/utils/Vision.java | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 403872c..f2d90b8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -193,7 +193,7 @@ public static final class NumericalConstants { public static final class TurretConstants { public static final int kMotorId = 20; public static final Angle kMinAngle = Radians.of(0.0); - public static final Angle kMaxAngle = Radians.of(2.0 * Math.PI); + public static final Angle kMaxAngle = Radians.of((3.0 / 2.0) * Math.PI); public static final int kPositionBufferLength = 4000; public static final Time kEncoderReadingDelay = Seconds.of(0.005); diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index b388119..7ae17a3 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -3,6 +3,7 @@ import java.util.List; import java.util.Optional; import java.util.function.Consumer; +import java.util.function.Function; import java.util.function.Supplier; import org.photonvision.EstimatedRobotPose; @@ -33,7 +34,7 @@ public class Vision { PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); - Optional> m_turretAngleSupplier; + Optional> m_turretAngleSupplier; PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(VisionConstants.kTagLayout, VisionConstants.kRobotToCamOne); @@ -43,7 +44,7 @@ public class Vision { Consumer m_visionConsumer; private Matrix curStdDevs; - public Vision(Optional> turretAngleSupplier, Consumer visionConsumer) { + public Vision(Optional> turretAngleSupplier, Consumer visionConsumer) { m_turretAngleSupplier = turretAngleSupplier; m_visionConsumer = visionConsumer; } @@ -67,9 +68,8 @@ public void periodic() { } Optional visionEstimationCameraTwo = Optional.empty(); - m_poseEstimatorTwo.setRobotToCameraTransform(getTurretCameraTransform()); - for (var result : m_camera2.getAllUnreadResults()) { + m_poseEstimatorTwo.setRobotToCameraTransform(getTurretCameraTransform(result.getTimestampSeconds())); visionEstimationCameraTwo = m_poseEstimatorTwo.estimateCoprocMultiTagPose(result); if (visionEstimationCameraTwo.isEmpty()) { @@ -131,11 +131,11 @@ private void updateEstimationStdDevs( } } - private Transform3d getTurretCameraTransform() { + private Transform3d getTurretCameraTransform(double estimationTime) { if (m_turretAngleSupplier.isEmpty()) return VisionConstants.kRobotToCamTwo; - Angle turretAngle = m_turretAngleSupplier.get().get(); + Angle turretAngle = m_turretAngleSupplier.get().apply(estimationTime); Distance cameraXOffset = Meters .of(VisionConstants.kTurretCameraDistanceToCenter.in(Meters) * Math.cos(turretAngle.in(Radians))); From 908366965037cd4674e746fa2127a7b68fcb16de Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 5 Feb 2026 18:35:58 -0600 Subject: [PATCH 16/20] Working turret after incident --- src/main/java/frc/robot/Constants.java | 2 ++ .../frc/robot/subsystems/TurretSubsystem.java | 30 ++++++++++++------- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f2d90b8..64043d7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -206,6 +206,8 @@ public static final class TurretConstants { public static final double kP = 1.5; public static final double kI = 0.0; public static final double kD = 0.0; + + public static final int kSmartCurrentLimit = 40; } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index d3a0043..214a64d 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; @@ -15,6 +16,7 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.geometry.Pose2d; @@ -43,23 +45,21 @@ public class TurretSubsystem extends SubsystemBase { private Supplier m_robotPoseSupplier; - private SparkMaxConfig m_pidConfig = new SparkMaxConfig(); + private SparkMaxConfig m_config = new SparkMaxConfig(); public TurretSubsystem(Supplier robotPoseSupplier) { m_robotPoseSupplier = robotPoseSupplier; - m_pidConfig.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); - m_pidConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_turretMotor.configure(m_pidConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_config.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); + m_config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + m_config.smartCurrentLimit(TurretConstants.kSmartCurrentLimit); + m_config.idleMode(IdleMode.kBrake); + m_turretMotor.configure(m_config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } public void moveToAngle(Angle angle) { - if (angle.gt(TurretConstants.kFullRotation)) { - angle = angle.minus(TurretConstants.kFullRotation); - } else if (angle.lt(TurretConstants.kNoRotation)) { - angle = angle.plus(TurretConstants.kFullRotation); - } + angle = wrapAngle(angle); if (angle.gt(TurretConstants.kMaxAngle)) { System.out @@ -73,6 +73,7 @@ public void moveToAngle(Angle angle) { return; } + // System.out.println("Target angle is " + angle); m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); } @@ -104,6 +105,15 @@ public void pushCurrentEncoderReading() { @Override public void simulationPeriodic() { m_simTurretMotor.setVelocity(1.0); - System.out.println(m_simAbsoluteEncoder.getPosition()); + // System.out.println(m_simAbsoluteEncoder.getPosition()); + } + + private Angle wrapAngle(Angle angle) { + Angle wrap = Radians.of((angle.in(Radians) % TurretConstants.kFullRotation.in(Radians))); + if (wrap.lt(TurretConstants.kNoRotation)) { + wrap = wrap.plus(TurretConstants.kFullRotation); + } + + return wrap; } } \ No newline at end of file From 20e3fb453d6fb79d6f44cfe3f3b21098a6188430 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 7 Feb 2026 12:22:41 -0600 Subject: [PATCH 17/20] Clean up unused imports and add region logic --- simgui-ds.json | 3 +- src/main/java/frc/robot/Constants.java | 24 ++++++---- src/main/java/frc/robot/RobotContainer.java | 48 +++++++++++++++++-- .../robot/subsystems/ShooterSubsystem.java | 11 ++--- .../frc/robot/subsystems/TurretSubsystem.java | 7 --- 5 files changed, 67 insertions(+), 26 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 2f801a3..f671bb9 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -101,7 +101,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 64043d7..86ef4eb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,14 +22,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; -import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -224,6 +216,8 @@ public static final class ShooterConstants { // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part public static final double kHoodGearRatio = (62 / 25) * (14 / 218); + + public static final Angle kFeedAngle = Degrees.of(90.0); } public static final class Fixtures { @@ -234,5 +228,17 @@ public static final class Fixtures { // side public static final Pose2d kTopFeedPose = new Pose2d(); public static final Pose2d kBottomFeedPose = new Pose2d(); + + public static final Distance kFieldYMidpoint = Inches.of(317.69 / 2.0); + + public static final Distance kRedSideNeutralBorder = Inches.of(182.11); + public static final Distance kBlueSideNeutralBorder = Inches.of(651.22 - 182.11); + + public static enum FieldLocations { + AllianceSide, + NeutralLeftSide, + NeutralRightSide, + OpponentSide + } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9fb8209..53a1288 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,22 +3,26 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Radian; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; +import java.util.Optional; + import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.Fixtures; import frc.robot.Constants.OIConstants; +import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -76,4 +80,42 @@ public Runnable pushTurretEncoderReading() { m_turret.pushCurrentEncoderReading(); }; } + + public Fixtures.FieldLocations getRobotLocation() { + Optional alliance = DriverStation.getAlliance(); + Pose2d robotPose = m_drive.getPose(); + + double x = robotPose.getX(); + double y = robotPose.getY(); + + if (alliance.isPresent()) { + if (alliance.get() == Alliance.Blue) { + if (x < Fixtures.kBlueSideNeutralBorder.in(Meters) && x > Fixtures.kRedSideNeutralBorder.in(Meters)) { + if (y < Fixtures.kFieldYMidpoint.in(Meters)) { + return Fixtures.FieldLocations.NeutralLeftSide; + } else { + return Fixtures.FieldLocations.NeutralRightSide; + } + } else if (x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { + return Fixtures.FieldLocations.AllianceSide; + } else { + return Fixtures.FieldLocations.OpponentSide; + } + } else if (alliance.get() == Alliance.Red) { + if (x > Fixtures.kRedSideNeutralBorder.in(Meters) && x < Fixtures.kBlueSideNeutralBorder.in(Meters)) { + if (y < Fixtures.kFieldYMidpoint.in(Meters)) { + return Fixtures.FieldLocations.NeutralRightSide; + } else { + return Fixtures.FieldLocations.NeutralLeftSide; + } + } else if (x < Fixtures.kRedSideNeutralBorder.in(Meters)) { + return Fixtures.FieldLocations.AllianceSide; + } else { + return Fixtures.FieldLocations.OpponentSide; + } + } + } + + return null; + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 1b260a3..5c29608 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -14,13 +14,8 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -import frc.robot.utils.ShuffleboardPid; public class ShooterSubsystem extends SubsystemBase { @@ -48,12 +43,16 @@ public ShooterSubsystem() { .d(ShooterConstants.kHoodD); m_hoodConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + // Inverse since kHoodGearRatio gives encoder -> hood motion, and we need hood + // motion -> encoder + m_hoodConfig.encoder.positionConversionFactor(1.0 / ShooterConstants.kHoodGearRatio); + m_shooterMotor.configure(m_shooterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); m_hoodMotor.configure(m_hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } public void Aim(Angle angle) { - m_hoodClosedLoopController.setSetpoint(angle.in(Rotations) / ShooterConstants.kHoodGearRatio, + m_hoodClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 214a64d..af87583 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,6 +1,5 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; @@ -21,12 +20,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.system.plant.DCMotor; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; @@ -37,8 +32,6 @@ public class TurretSubsystem extends SubsystemBase { private final SparkMaxSim m_simTurretMotor = new SparkMaxSim(m_turretMotor, DCMotor.getNEO(1)); private final AbsoluteEncoder m_absoluteEncoder = m_turretMotor.getAbsoluteEncoder(); - private final SparkAbsoluteEncoderSim m_simAbsoluteEncoder = m_simTurretMotor.getAbsoluteEncoderSim(); - private final SparkClosedLoopController m_turretClosedLoopController = m_turretMotor.getClosedLoopController(); private final PositionBuffer m_positionBuffer = new PositionBuffer(TurretConstants.kPositionBufferLength); From 8b263ea394ce39389fc8a5d61f6675ccb90e21c6 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 7 Feb 2026 13:00:05 -0600 Subject: [PATCH 18/20] Add function for drivetrain to move to heading for feeding when turret cannot fully turn towards target --- src/main/java/frc/robot/Constants.java | 8 +++ src/main/java/frc/robot/RobotContainer.java | 41 ++------------- .../frc/robot/subsystems/DriveSubsystem.java | 50 ++++++++++++++++++- .../frc/robot/subsystems/TurretSubsystem.java | 1 - 4 files changed, 62 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 86ef4eb..fa5afb8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -240,5 +240,13 @@ public static enum FieldLocations { NeutralRightSide, OpponentSide } + + // Placeholders + public static final Angle kRedLeftSideFeedHeading = Degrees.of(40); + public static final Angle kRedRightSideFeedHeading = Degrees.of(160); + + // Placeholders + public static final Angle kBlueLeftSideFeedHeading = Degrees.of(-40); + public static final Angle kBlueRightSideFeedHeading = Degrees.of(-160); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 53a1288..e7254e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc.robot.Constants.Fixtures; import frc.robot.Constants.OIConstants; import frc.robot.Constants.TurretConstants; +import frc.robot.Constants.Fixtures.FieldLocations; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -81,41 +82,9 @@ public Runnable pushTurretEncoderReading() { }; } - public Fixtures.FieldLocations getRobotLocation() { - Optional alliance = DriverStation.getAlliance(); - Pose2d robotPose = m_drive.getPose(); - - double x = robotPose.getX(); - double y = robotPose.getY(); - - if (alliance.isPresent()) { - if (alliance.get() == Alliance.Blue) { - if (x < Fixtures.kBlueSideNeutralBorder.in(Meters) && x > Fixtures.kRedSideNeutralBorder.in(Meters)) { - if (y < Fixtures.kFieldYMidpoint.in(Meters)) { - return Fixtures.FieldLocations.NeutralLeftSide; - } else { - return Fixtures.FieldLocations.NeutralRightSide; - } - } else if (x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { - return Fixtures.FieldLocations.AllianceSide; - } else { - return Fixtures.FieldLocations.OpponentSide; - } - } else if (alliance.get() == Alliance.Red) { - if (x > Fixtures.kRedSideNeutralBorder.in(Meters) && x < Fixtures.kBlueSideNeutralBorder.in(Meters)) { - if (y < Fixtures.kFieldYMidpoint.in(Meters)) { - return Fixtures.FieldLocations.NeutralRightSide; - } else { - return Fixtures.FieldLocations.NeutralLeftSide; - } - } else if (x < Fixtures.kRedSideNeutralBorder.in(Meters)) { - return Fixtures.FieldLocations.AllianceSide; - } else { - return Fixtures.FieldLocations.OpponentSide; - } - } - } - - return null; + public Command feedPosition(Alliance alliance) { + return new RunCommand(() -> { + + }, m_drive, m_turret); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e7f574c..d899b25 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -21,6 +21,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -30,6 +31,7 @@ import frc.robot.utils.VisionEstimation; import frc.robot.utils.Vision; import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.Fixtures; import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.VisionConstants; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -177,7 +179,15 @@ public Command moveToAngle(Angle angle) { }); } - public Command enableFacePose(Pose2d fixture) { + public Command faceCardinalHeading(Angle heading) { + return new RunCommand(() -> { + Angle robotHeading = getHeading(); + m_targetAutoAngle = robotHeading.minus(heading); + m_isManualRotate = false; + }, this); + } + + public Command facePose(Pose2d fixture) { return new RunCommand(() -> { Pose2d robotPose = getPose(); @@ -379,6 +389,44 @@ public void addVisionMeasurement(VisionEstimation estimation) { m_poseEstimator.addVisionMeasurement(estimation.m_pose, estimation.m_timestamp, estimation.m_stdDevs); } + public Fixtures.FieldLocations getRobotLocation() { + Optional alliance = DriverStation.getAlliance(); + Pose2d robotPose = getPose(); + + double x = robotPose.getX(); + double y = robotPose.getY(); + + if (alliance.isPresent()) { + if (alliance.get() == Alliance.Blue) { + if (x < Fixtures.kBlueSideNeutralBorder.in(Meters) && x > Fixtures.kRedSideNeutralBorder.in(Meters)) { + if (y < Fixtures.kFieldYMidpoint.in(Meters)) { + return Fixtures.FieldLocations.NeutralLeftSide; + } else { + return Fixtures.FieldLocations.NeutralRightSide; + } + } else if (x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { + return Fixtures.FieldLocations.AllianceSide; + } else { + return Fixtures.FieldLocations.OpponentSide; + } + } else if (alliance.get() == Alliance.Red) { + if (x > Fixtures.kRedSideNeutralBorder.in(Meters) && x < Fixtures.kBlueSideNeutralBorder.in(Meters)) { + if (y < Fixtures.kFieldYMidpoint.in(Meters)) { + return Fixtures.FieldLocations.NeutralRightSide; + } else { + return Fixtures.FieldLocations.NeutralLeftSide; + } + } else if (x < Fixtures.kRedSideNeutralBorder.in(Meters)) { + return Fixtures.FieldLocations.AllianceSide; + } else { + return Fixtures.FieldLocations.OpponentSide; + } + } + } + + return null; + } + private Angle getOptimalAngle(Angle target, Angle robotHeading) { // Full robot rotations in radians Angle robotRotations = Radians diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index af87583..99d5330 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -8,7 +8,6 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; -import com.revrobotics.sim.SparkAbsoluteEncoderSim; import com.revrobotics.sim.SparkMaxSim; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.FeedbackSensor; From d3229746a356a3870d1f6eb0b17ff2ac3d28231e Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 9 Feb 2026 18:05:24 -0600 Subject: [PATCH 19/20] Position buffer readability fix --- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/DriveSubsystem.java | 19 ++++- .../java/frc/robot/utils/PositionBuffer.java | 84 ++++++++++++++----- src/main/java/frc/robot/utils/RingBuffer.java | 39 +++++---- 4 files changed, 105 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fa5afb8..5be2d40 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -167,7 +167,7 @@ public static final class VisionConstants { // Placeholder numbers public static final Pose3d kTurretAxisOfRotation = new Pose3d(Meters.of(0.2), Meters.of(0.3), Meters.of(0.3), new Rotation3d(0.0, 0.0, 0.0)); - public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.6); + public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); public static final Angle kCameraTwoPitch = Radians.of(0.0); public static final Angle kCameraTwoRoll = Radians.of(0.0); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d899b25..55c2ef1 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -179,10 +179,21 @@ public Command moveToAngle(Angle angle) { }); } + public Command faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { + return new RunCommand(() -> { + Angle robotAngle = getHeading(); + + if (withinRange(minAngle, maxAngle, robotAngle)) { + m_isManualRotate = true; + } else { + m_isManualRotate = false; + } + }, this); + } + public Command faceCardinalHeading(Angle heading) { return new RunCommand(() -> { - Angle robotHeading = getHeading(); - m_targetAutoAngle = robotHeading.minus(heading); + m_targetAutoAngle = heading; m_isManualRotate = false; }, this); } @@ -446,4 +457,8 @@ private Angle getOptimalAngle(Angle target, Angle robotHeading) { return delta.plus(robotHeading); } + + private static boolean withinRange(Angle min, Angle max, Angle angle) { + return !(angle.lt(min) || angle.gt(max)); + } } diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java index 6e94e41..3315aaf 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -15,10 +15,14 @@ public PositionBuffer(int length) { } public void pushElement(Angle angle, double delay) { - m_positions.push(new TimePosition(angle, Timer.getFPGATimestamp() - delay)); + try { + m_positions.push(new TimePosition(angle, Timer.getFPGATimestamp() - delay)); + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + } } - public Angle getAngleAtTime(double time) { + public Angle getAngleAtTime(double requestedTime) { // A fun little binary search algo int high = m_positions.getLength(); int low = 0; @@ -27,42 +31,78 @@ public Angle getAngleAtTime(double time) { int midpoint = low + (high - low) / 2; while (low < high) { - double timeAtMidpoint = m_positions.get(midpoint).timestamp; + double timeAtMidpoint; + try { + timeAtMidpoint = m_positions.get(midpoint).timestamp; + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + return null; + } - if (time > timeAtMidpoint) { + if (requestedTime > timeAtMidpoint) { low = midpoint + 1; - } else if (time < timeAtMidpoint) { + } else if (requestedTime < timeAtMidpoint) { high = midpoint; } midpoint = low + (high - low) / 2; } - Angle angleAtTimeStamp; - // Linearly interpolate velocity if we aren't on the first/last timestamp if (midpoint != 0 && midpoint != m_positions.getLength() - 1) { - TimePosition closestTimestamp = m_positions.get(midpoint); + TimePosition closestTimestamp; + + try { + closestTimestamp = m_positions.get(midpoint); + } catch (Exception e) { + return null; + } - double dt = m_positions.get(midpoint).timestamp - time; - TimePosition nextTimeStamp = dt > 0 ? m_positions.get(midpoint + 1) : m_positions.get(midpoint - 1); + double dt; - Angle daStamped = nextTimeStamp.angle.minus(closestTimestamp.angle); + try { + dt = m_positions.get(midpoint).timestamp - requestedTime; + } catch (Exception e) { + return null; + } - // Absolute since in order to preserve the sign, only one part of the fraction - // can keep its original sign. In this case we always keep the denominator - // positive - double dtStamped = Math.abs(nextTimeStamp.timestamp - closestTimestamp.timestamp); + TimePosition nextTimeStamp; + + // Stampted Time 1 should always be less than stampted time 2 + TimePosition stamptedTime1; + TimePosition stamptedTime2; + + if (dt > 0) { + try { + stamptedTime2 = m_positions.get(midpoint + 1); + stamptedTime1 = closestTimestamp; + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + return null; + } + } else { + try { + stamptedTime1 = m_positions.get(midpoint - 1); + stamptedTime2 = closestTimestamp; + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + return null; + } + } - double percentageTimeTraveled = dt / dtStamped; - Angle da = daStamped.times(percentageTimeTraveled); + double deltaTimeStampted = stamptedTime2.timestamp - stamptedTime1.timestamp; + double timeSinceStamp1 = requestedTime - stamptedTime1.timestamp; - angleAtTimeStamp = closestTimestamp.angle.plus(da); - } else { - angleAtTimeStamp = m_positions.get(midpoint).angle; - } + Angle deltaTheta = stamptedTime2.angle.minus(stamptedTime2.angle); - return angleAtTimeStamp; + return deltaTheta.times(timeSinceStamp1 / deltaTimeStampted).plus(stamptedTime1.angle); + } + try { + return m_positions.get(midpoint).angle; + } catch (Exception e) { + System.out.println("Ring Buffer Exception: " + e.getMessage()); + return null; + } } } diff --git a/src/main/java/frc/robot/utils/RingBuffer.java b/src/main/java/frc/robot/utils/RingBuffer.java index d7738ee..f15f4ac 100644 --- a/src/main/java/frc/robot/utils/RingBuffer.java +++ b/src/main/java/frc/robot/utils/RingBuffer.java @@ -21,31 +21,42 @@ public RingBuffer(int maxLength) { m_length = maxLength; } - public void push(T element) { + public void push(T element) throws Exception { if (m_head - m_tail == m_length) { - pop(); + try { + pop(); + } catch (Exception e) { + throw e; + } } - m_elements[getTrueIndex(m_head++)] = element; + synchronized (m_elements) { + m_elements[getTrueIndex(m_head++)] = element; + } } - public T pop() { + public T pop() throws Exception { if (m_tail == m_head) { - System.out.println("Cannot pop from empty ring buffer."); - return null; + System.out.println(); + throw new Exception("Cannot pop from empty ring buffer."); } - return (T) m_elements[getTrueIndex(m_tail++)]; + synchronized (m_elements) { + return (T) m_elements[getTrueIndex(m_tail++)]; + } } - public T get(int index) { - int trueIndex = m_tail + index; - if (trueIndex > m_head) { - System.out.println("Index " + index + " is out of bounds of the ring buffer."); - return null; - } + public T get(int index) throws Exception { + synchronized (m_elements) { + int trueIndex = m_tail + index; + if (trueIndex > m_head) { + System.out.println("Index " + index + " is out of bounds of the ring buffer."); - return (T) m_elements[getTrueIndex(trueIndex)]; + throw new Exception("Index " + index + " out of bounds."); + } + + return (T) m_elements[getTrueIndex(trueIndex)]; + } } public int getLength() { From 7bd4cfa0f12610da2fe535be957e4701af66f858 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 10 Feb 2026 19:08:53 -0600 Subject: [PATCH 20/20] Functional robot range aim function --- src/main/java/frc/robot/RobotContainer.java | 15 ++--- .../frc/robot/subsystems/DriveSubsystem.java | 60 ++++++++++--------- .../frc/robot/subsystems/TurretSubsystem.java | 12 ++-- 3 files changed, 44 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e7254e4..48d4fd4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,16 +3,13 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; -import java.util.Optional; - import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,10 +17,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.Fixtures; import frc.robot.Constants.OIConstants; -import frc.robot.Constants.TurretConstants; -import frc.robot.Constants.Fixtures.FieldLocations; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -61,11 +55,12 @@ public RobotContainer() { m_drive)); // TODO: Get rid of this - m_turret.setDefaultCommand(new RunCommand(() -> m_turret.pointToHeading(Radians.of(Math.PI / 4)), m_turret)); + m_turret.setDefaultCommand(new RunCommand(() -> m_turret.moveToAngle(Radians.of(Math.PI / 4)), m_turret)); } private void configureBindings() { - + m_driverController.a().whileTrue(m_drive.faceCardinalHeadingRange(Degrees.of(342), Degrees.of(190))); + m_driverController.a().whileFalse(m_drive.disableFaceHeading()); } public Command getAutonomousCommand() { @@ -84,7 +79,7 @@ public Runnable pushTurretEncoderReading() { public Command feedPosition(Alliance alliance) { return new RunCommand(() -> { - + }, m_drive, m_turret); } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 55c2ef1..f7e3d25 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -6,8 +6,6 @@ import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,8 +15,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; 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.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -33,13 +29,11 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; import frc.robot.Constants.NumericalConstants; -import frc.robot.Constants.VisionConstants; +import frc.robot.Constants.TurretConstants; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; - import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.sim.Pigeon2SimState; import com.pathplanner.lib.auto.AutoBuilder; @@ -47,16 +41,10 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import edu.wpi.first.units.measure.*; - import static edu.wpi.first.units.Units.*; import java.util.Optional; -import org.photonvision.PhotonCamera; - -import frc.robot.Constants; - public class DriveSubsystem extends SubsystemBase { // Create MAXSwerveModules @@ -180,24 +168,20 @@ public Command moveToAngle(Angle angle) { } public Command faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { - return new RunCommand(() -> { + return new InstantCommand(() -> { Angle robotAngle = getHeading(); + // System.out.println(robotAngle); if (withinRange(minAngle, maxAngle, robotAngle)) { m_isManualRotate = true; } else { m_isManualRotate = false; + System.out.println(getClosestAngle(minAngle, maxAngle, robotAngle)); + m_targetAutoAngle = getClosestAngle(minAngle, maxAngle, robotAngle); } }, this); } - public Command faceCardinalHeading(Angle heading) { - return new RunCommand(() -> { - m_targetAutoAngle = heading; - m_isManualRotate = false; - }, this); - } - public Command facePose(Pose2d fixture) { return new RunCommand(() -> { Pose2d robotPose = getPose(); @@ -217,7 +201,7 @@ public Command facePose(Pose2d fixture) { }); } - public Command disableFacePose() { + public Command disableFaceHeading() { return new InstantCommand(() -> { m_isManualRotate = true; }); @@ -321,6 +305,8 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ : m_pidController.calculate(getHeading().in(Radians), getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians)); + System.out.println("Target " + m_targetAutoAngle + ", Current" + getHeading()); + var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, @@ -438,12 +424,21 @@ public Fixtures.FieldLocations getRobotLocation() { return null; } - private Angle getOptimalAngle(Angle target, Angle robotHeading) { - // Full robot rotations in radians + private static Angle wrapAngle(Angle heading) { Angle robotRotations = Radians - .of(Math.floor(robotHeading.in(Radians) / (2 * Math.PI)) * 2.0 * Math.PI); + .of(Math.floor(heading.in(Radians) / (2 * Math.PI)) * 2.0 * Math.PI); + + Angle wrap = heading.minus(robotRotations); + + if (wrap.lt(Radians.of(0))) { + wrap = wrap.plus(TurretConstants.kFullRotation); + } - Angle wrappedRobotAngle = robotHeading.minus(robotRotations); + return wrap; + } + + private static Angle getOptimalAngle(Angle target, Angle robotHeading) { + Angle wrappedRobotAngle = wrapAngle(robotHeading); Angle delta = target.minus(wrappedRobotAngle); @@ -459,6 +454,17 @@ private Angle getOptimalAngle(Angle target, Angle robotHeading) { } private static boolean withinRange(Angle min, Angle max, Angle angle) { - return !(angle.lt(min) || angle.gt(max)); + angle = wrapAngle(angle); + min = getOptimalAngle(angle, min); + max = getOptimalAngle(angle, max); + return angle.gt(max) && angle.lt(min); + } + + private static Angle getClosestAngle(Angle t1, Angle t2, Angle angle) { + t1 = wrapAngle(t1); + t2 = wrapAngle(t2); + angle = wrapAngle(angle); + + return t1.minus(angle).abs(Rotations) < t2.minus(angle).abs(Rotations) ? t1 : t2; } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 99d5330..5ee9929 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -54,14 +54,14 @@ public void moveToAngle(Angle angle) { angle = wrapAngle(angle); if (angle.gt(TurretConstants.kMaxAngle)) { - System.out - .println("Angle " + angle + "is bigger than maximum angle " + - TurretConstants.kMaxAngle + "."); + // System.out + // .println("Angle " + angle + "is bigger than maximum angle " + + // TurretConstants.kMaxAngle + "."); return; } else if (angle.lt(TurretConstants.kMinAngle)) { - System.out.println( - "Angle " + angle + "is to smaller than minimum angle " + - TurretConstants.kMinAngle + "."); + // System.out.println( + // "Angle " + angle + "is to smaller than minimum angle " + + // TurretConstants.kMinAngle + "."); return; }