From 19f79921667ad613e9c47169ccc9918d0df56563 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 14 Feb 2026 12:32:47 -0600 Subject: [PATCH 01/25] Refactor turret to also give velocity data --- src/main/java/frc/robot/Constants.java | 15 +++- src/main/java/frc/robot/RobotContainer.java | 20 ++--- .../frc/robot/commands/AimCommandFactory.java | 83 +++++++++++++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 60 +++++++++----- .../frc/robot/subsystems/TurretSubsystem.java | 32 +++---- .../java/frc/robot/utils/PositionBuffer.java | 48 +++++++---- .../java/frc/robot/utils/TurretPosition.java | 8 ++ src/main/java/frc/robot/utils/Vision.java | 33 ++++++-- 8 files changed, 224 insertions(+), 75 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AimCommandFactory.java create mode 100644 src/main/java/frc/robot/utils/TurretPosition.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5be2d40..2221b1f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -91,6 +91,14 @@ public static final class DriveConstants { public static final double kAutoRotationP = Robot.isReal() ? 0.3 : 3.0; public static final double kAutoRotationI = 0.0; public static final double kAutoRotationD = 0.0; + + public static enum RangeType { + Within, + CloseMin, + CloseMax + } + + public static final Angle kTurnToAngleTolerance = Degrees.of(2); } public static final class ModuleConstants { @@ -176,6 +184,8 @@ public static final class VisionConstants { public static final Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); public static final Matrix kVisionStdDevs = VecBuilder.fill(1, 1, 1); + + public static final AngularVelocity kMaxTurretVisionSpeed = RPM.of(30); } public static final class NumericalConstants { @@ -185,7 +195,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((3.0 / 2.0) * Math.PI); + public static final Angle kMaxAngle = Degrees.of(340); public static final int kPositionBufferLength = 4000; public static final Time kEncoderReadingDelay = Seconds.of(0.005); @@ -200,6 +210,9 @@ public static final class TurretConstants { public static final double kD = 0.0; public static final int kSmartCurrentLimit = 40; + + public static final Angle kFeedMinAngle = Degrees.of(150); + public static final Angle kFeedMaxAngle = Degrees.of(200); } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48d4fd4..258463b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,33 +9,34 @@ 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.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.OIConstants; +import frc.robot.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; public class RobotContainer { - private final DriveSubsystem m_drive = new DriveSubsystem(); - private final CommandXboxController m_driverController = new CommandXboxController( OIConstants.kDriverControllerPort); private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); - private final TurretSubsystem m_turret; + private final TurretSubsystem m_turret = new TurretSubsystem(); - public RobotContainer() { - m_turret = new TurretSubsystem(m_drive::getPose); + private final DriveSubsystem m_drive = new DriveSubsystem(m_turret::getRotationAtTime); + + AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret); + public RobotContainer() { m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); @@ -53,14 +54,11 @@ public RobotContainer() { -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), true), m_drive)); - - // TODO: Get rid of this - 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()); + m_driverController.a() + .whileTrue(m_aimFactory.HoldTurretHeading(Degrees.of(10))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java new file mode 100644 index 0000000..de4880f --- /dev/null +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -0,0 +1,83 @@ +package frc.robot.commands; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; + +import java.util.function.Supplier; + +import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.Fixtures; +import frc.robot.Constants.TurretConstants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class AimCommandFactory { + + private DriveSubsystem m_drive; + private TurretSubsystem m_turret; + + public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret) { + m_drive = drive; + m_turret = turret; + } + + public Command Aim(Supplier isFeedingLeftSide) { + return new RunCommand(() -> { + Fixtures.FieldLocations location = m_drive.getRobotLocation(); + Pose2d robotPose = m_drive.getPose(); + Angle robotAngle = Radians.of(robotPose.getRotation().getRadians()); + + switch (location) { + case AllianceSide: { + Pose2d fixturePose = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub + : Fixtures.kRedAllianceHub; + + double dy = fixturePose.getX() - robotPose.getX(); + double dx = fixturePose.getY() - robotPose.getY(); + + Angle absHeading = Radians.of(Math.atan2(dy, dx)); + Angle relHeading = absHeading.minus(robotAngle); + + m_turret.moveToAngle(relHeading); + break; + } + case NeutralLeftSide: { + // Heading changes 180 degrees depending on which alliance you are on + Angle offset = DriverStation.getAlliance().get() == Alliance.Blue ? Degrees.of(0) : Degrees.of(180); + Angle absHeading = isFeedingLeftSide.get() ? offset.minus(Degrees.of(50)) + : offset.plus(Degrees.of(50)); + Angle relHeading = absHeading.minus(robotAngle); + + robotPose = m_drive.getPose(); + break; + } + case NeutralRightSide: { + break; + } + case OpponentSide: { + break; + } + default: + break; + } + }, m_drive, m_turret).until(() -> { + return true; + }); + } + + public Command HoldTurretHeading(Angle heading) { + return new RunCommand(() -> { + System.out.println(m_drive.getHeading()); + m_turret.moveToAngle(heading.minus(m_drive.getHeading())); + }, m_turret); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index f7e3d25..043b21b 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; @@ -24,12 +25,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; import frc.robot.utils.ShuffleboardPid; +import frc.robot.utils.TurretPosition; 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.TurretConstants; +import frc.robot.Constants.DriveConstants.RangeType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -44,6 +47,8 @@ import static edu.wpi.first.units.Units.*; import java.util.Optional; +import java.util.function.Function; +import java.util.function.Supplier; public class DriveSubsystem extends SubsystemBase { @@ -82,7 +87,7 @@ public class DriveSubsystem extends SubsystemBase { private final Field2d m_field = new Field2d(); - private final Vision m_vision = new Vision(Optional.empty(), this::addVisionMeasurement); + private final Vision m_vision; // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, @@ -107,7 +112,10 @@ public class DriveSubsystem extends SubsystemBase { /** * Creates a new DriveSubsystem. */ - public DriveSubsystem() { + public DriveSubsystem(Function turretPositionSupplier) { + + m_vision = new Vision(Optional.of(turretPositionSupplier), this::addVisionMeasurement, + this::getAngularVelocity); // Usage reporting for MAXSwerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_MaxSwerve); @@ -167,19 +175,18 @@ public Command moveToAngle(Angle angle) { }); } - public Command faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { - 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 RangeType faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { + Angle robotAngle = getHeading(); + // System.out.println(robotAngle); + + if (withinRange(minAngle, maxAngle, robotAngle)) { + m_isManualRotate = true; + return RangeType.Within; + } else { + m_isManualRotate = false; + m_targetAutoAngle = getClosestAngle(minAngle, maxAngle, robotAngle); + return m_targetAutoAngle.isEquivalent(minAngle) ? RangeType.CloseMin : RangeType.CloseMax; + } } public Command facePose(Pose2d fixture) { @@ -233,6 +240,13 @@ public void periodic() { }); } + if (!m_isManualRotate && Math + .abs(m_targetAutoAngle.in(Degrees) - getHeading().in(Degrees)) < DriveConstants.kTurnToAngleTolerance + .in(Degrees)) { + + m_isManualRotate = true; + } + // System.out.println("Current rotation: " + // getPose().getRotation().getRadians()); @@ -299,13 +313,17 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ .println("Setpoint: " + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians) + ", Current: " + getHeading().in(Radians)); + double pidCalculation = m_pidController.calculate(getHeading().in(Radians), + getOptimalAngle(m_targetAutoAngle, 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)); + double rotDelivered = (m_isManualRotate) + ? rot * DriveConstants.kMaxAngularSpeed.magnitude() + : pidCalculation; - System.out.println("Target " + m_targetAutoAngle + ", Current" + getHeading()); + // System.out.println("Target " + m_targetAutoAngle + ", Current" + + // getHeading()); var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative @@ -424,6 +442,10 @@ public Fixtures.FieldLocations getRobotLocation() { return null; } + private AngularVelocity getAngularVelocity() { + return DegreesPerSecond.of(pidgey.getAngularVelocityZDevice().getValueAsDouble()); + } + private static Angle wrapAngle(Angle heading) { Angle robotRotations = Radians .of(Math.floor(heading.in(Radians) / (2 * Math.PI)) * 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 5ee9929..2f5aa04 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.RPM; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; @@ -24,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; +import frc.robot.utils.TurretPosition; public class TurretSubsystem extends SubsystemBase { @@ -35,13 +37,9 @@ public class TurretSubsystem extends SubsystemBase { private final PositionBuffer m_positionBuffer = new PositionBuffer(TurretConstants.kPositionBufferLength); - private Supplier m_robotPoseSupplier; - private SparkMaxConfig m_config = new SparkMaxConfig(); - public TurretSubsystem(Supplier robotPoseSupplier) { - m_robotPoseSupplier = robotPoseSupplier; - + public TurretSubsystem() { m_config.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); m_config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); m_config.smartCurrentLimit(TurretConstants.kSmartCurrentLimit); @@ -54,14 +52,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; } @@ -69,18 +67,11 @@ public void moveToAngle(Angle angle) { m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); } - public void pointToHeading(Angle heading) { - // 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() { - return Rotations.of(m_absoluteEncoder.getPosition()); + return wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())); } - public Angle getRotationAtTime(double timestamp) { + public TurretPosition getRotationAtTime(double timestamp) { return m_positionBuffer.getAngleAtTime(timestamp); } @@ -91,6 +82,7 @@ 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()), + RPM.of(m_absoluteEncoder.getVelocity()), TurretConstants.kEncoderReadingDelay.in(Seconds)); } diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java index 3315aaf..fbd4eca 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -1,28 +1,40 @@ package frc.robot.utils; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Timer; public class PositionBuffer { - private record TimePosition(Angle angle, double timestamp) { - } + TurretPosition m_position; - private final RingBuffer m_positions; + private final RingBuffer m_positions; public PositionBuffer(int length) { m_positions = new RingBuffer<>(length); } - public void pushElement(Angle angle, double delay) { + public void pushElement(Angle angle, AngularVelocity velocity, double delay) { try { - m_positions.push(new TimePosition(angle, Timer.getFPGATimestamp() - delay)); + m_positions.push(new TurretPosition(angle, velocity, Timer.getFPGATimestamp() - delay)); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); } } - public Angle getAngleAtTime(double requestedTime) { + private double interpolate(double x1, double x2, double y1, double y2, double x) { + double dsx = x2 - x1; + double dy = y2 - y1; + double dx = x - x1; + + return (dx / dsx) * dy + y1; + } + + public TurretPosition getAngleAtTime(double requestedTime) { // A fun little binary search algo int high = m_positions.getLength(); int low = 0; @@ -33,7 +45,7 @@ public Angle getAngleAtTime(double requestedTime) { while (low < high) { double timeAtMidpoint; try { - timeAtMidpoint = m_positions.get(midpoint).timestamp; + timeAtMidpoint = m_positions.get(midpoint).timestamp(); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; @@ -50,7 +62,7 @@ public Angle getAngleAtTime(double requestedTime) { // Linearly interpolate velocity if we aren't on the first/last timestamp if (midpoint != 0 && midpoint != m_positions.getLength() - 1) { - TimePosition closestTimestamp; + TurretPosition closestTimestamp; try { closestTimestamp = m_positions.get(midpoint); @@ -61,16 +73,16 @@ public Angle getAngleAtTime(double requestedTime) { double dt; try { - dt = m_positions.get(midpoint).timestamp - requestedTime; + dt = m_positions.get(midpoint).timestamp() - requestedTime; } catch (Exception e) { return null; } - TimePosition nextTimeStamp; + TurretPosition nextTimeStamp; // Stampted Time 1 should always be less than stampted time 2 - TimePosition stamptedTime1; - TimePosition stamptedTime2; + TurretPosition stamptedTime1; + TurretPosition stamptedTime2; if (dt > 0) { try { @@ -90,16 +102,18 @@ public Angle getAngleAtTime(double requestedTime) { } } - double deltaTimeStampted = stamptedTime2.timestamp - stamptedTime1.timestamp; - double timeSinceStamp1 = requestedTime - stamptedTime1.timestamp; + Angle angle = Radians.of(interpolate(stamptedTime1.timestamp(), stamptedTime2.timestamp(), + stamptedTime1.angle().in(Radians), stamptedTime2.angle().in(Radians), requestedTime)); - Angle deltaTheta = stamptedTime2.angle.minus(stamptedTime2.angle); + AngularVelocity velocity = RPM + .of(interpolate(stamptedTime1.timestamp(), stamptedTime2.timestamp(), + stamptedTime1.velocity().in(RPM), stamptedTime2.velocity().in(RPM), requestedTime)); - return deltaTheta.times(timeSinceStamp1 / deltaTimeStampted).plus(stamptedTime1.angle); + return new TurretPosition(angle, velocity, requestedTime); } try { - return m_positions.get(midpoint).angle; + return m_positions.get(midpoint); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; diff --git a/src/main/java/frc/robot/utils/TurretPosition.java b/src/main/java/frc/robot/utils/TurretPosition.java new file mode 100644 index 0000000..24a41dd --- /dev/null +++ b/src/main/java/frc/robot/utils/TurretPosition.java @@ -0,0 +1,8 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +public record TurretPosition(Angle angle, AngularVelocity velocity, + double timestamp) { +} diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 7ae17a3..ca11b12 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -34,7 +34,8 @@ public class Vision { PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); - Optional> m_turretAngleSupplier; + Optional> m_turretPositionSupplier; + Supplier m_robotAngularVelocitySupplier; PhotonPoseEstimator m_poseEstimatorOne = new PhotonPoseEstimator(VisionConstants.kTagLayout, VisionConstants.kRobotToCamOne); @@ -44,9 +45,11 @@ public class Vision { Consumer m_visionConsumer; private Matrix curStdDevs; - public Vision(Optional> turretAngleSupplier, Consumer visionConsumer) { - m_turretAngleSupplier = turretAngleSupplier; + public Vision(Optional> turretPositionSupplier, + Consumer visionConsumer, Supplier robotAngularVelocitySupplier) { + m_turretPositionSupplier = turretPositionSupplier; m_visionConsumer = visionConsumer; + m_robotAngularVelocitySupplier = robotAngularVelocitySupplier; } public void periodic() { @@ -69,7 +72,15 @@ public void periodic() { Optional visionEstimationCameraTwo = Optional.empty(); for (var result : m_camera2.getAllUnreadResults()) { - m_poseEstimatorTwo.setRobotToCameraTransform(getTurretCameraTransform(result.getTimestampSeconds())); + Transform3d cameraTransform; + + try { + cameraTransform = getTurretCameraTransform(result.getTimestampSeconds()); + } catch (Exception e) { + System.out.println(e.getMessage()); + break; + } + m_poseEstimatorTwo.setRobotToCameraTransform(cameraTransform); visionEstimationCameraTwo = m_poseEstimatorTwo.estimateCoprocMultiTagPose(result); if (visionEstimationCameraTwo.isEmpty()) { @@ -131,11 +142,19 @@ private void updateEstimationStdDevs( } } - private Transform3d getTurretCameraTransform(double estimationTime) { - if (m_turretAngleSupplier.isEmpty()) + private Transform3d getTurretCameraTransform(double estimationTime) throws Exception { + if (m_turretPositionSupplier.isEmpty()) return VisionConstants.kRobotToCamTwo; - Angle turretAngle = m_turretAngleSupplier.get().apply(estimationTime); + TurretPosition turretPosition = m_turretPositionSupplier.get().apply(estimationTime); + + // Getting the net velocity of the turret relative to the field + if (turretPosition.velocity().plus(m_robotAngularVelocitySupplier.get()) + .gt(VisionConstants.kMaxTurretVisionSpeed)) { + throw new Exception("Turret exceeds max speed for processing april tags."); + } + + Angle turretAngle = turretPosition.angle(); Distance cameraXOffset = Meters .of(VisionConstants.kTurretCameraDistanceToCenter.in(Meters) * Math.cos(turretAngle.in(Radians))); From b3701fa008a696f604398e4c8f33b3a0be3c931d Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 16 Feb 2026 16:58:57 -0600 Subject: [PATCH 02/25] Initial attempt to properly set position of camera dynamically --- src/main/java/frc/robot/Constants.java | 9 ++-- .../java/frc/robot/utils/PositionBuffer.java | 40 ++++++++---------- src/main/java/frc/robot/utils/Vision.java | 41 +++++++++++-------- 3 files changed, 46 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2221b1f..2f51c73 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -173,11 +173,14 @@ public static final class VisionConstants { .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 Translation3d kTurretAxisOfRotation = new Translation3d(Meters.of(0.2), Meters.of(0.3), + Meters.of(0.3)); public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); - public static final Angle kCameraTwoPitch = Radians.of(0.0); + public static final Distance kCameraTwoZ = Inches.of(18.0); + + public static final Angle kCameraTwoPitch = Radians.of(15.0); public static final Angle kCameraTwoRoll = Radians.of(0.0); + public static final Angle kCameraTwoYaw = 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); diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java index fbd4eca..695cfc5 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -42,10 +42,13 @@ public TurretPosition getAngleAtTime(double requestedTime) { // Avoiding overflow (though isn't really necessary here) int midpoint = low + (high - low) / 2; + TurretPosition midpointTurretPosition = null; + double timeAtMidpoint = 0.0; + while (low < high) { - double timeAtMidpoint; try { - timeAtMidpoint = m_positions.get(midpoint).timestamp(); + midpointTurretPosition = m_positions.get(midpoint); + timeAtMidpoint = midpointTurretPosition.timestamp(); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; @@ -62,52 +65,43 @@ public TurretPosition getAngleAtTime(double requestedTime) { // Linearly interpolate velocity if we aren't on the first/last timestamp if (midpoint != 0 && midpoint != m_positions.getLength() - 1) { - TurretPosition closestTimestamp; - - try { - closestTimestamp = m_positions.get(midpoint); - } catch (Exception e) { - return null; - } - double dt; try { - dt = m_positions.get(midpoint).timestamp() - requestedTime; + dt = timeAtMidpoint - requestedTime; } catch (Exception e) { return null; } - TurretPosition nextTimeStamp; - // Stampted Time 1 should always be less than stampted time 2 - TurretPosition stamptedTime1; - TurretPosition stamptedTime2; + TurretPosition firstTurretPosition; + TurretPosition secondTurretPosition; if (dt > 0) { try { - stamptedTime2 = m_positions.get(midpoint + 1); - stamptedTime1 = closestTimestamp; + secondTurretPosition = m_positions.get(midpoint + 1); + firstTurretPosition = midpointTurretPosition; } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; } } else { try { - stamptedTime1 = m_positions.get(midpoint - 1); - stamptedTime2 = closestTimestamp; + firstTurretPosition = m_positions.get(midpoint - 1); + secondTurretPosition = midpointTurretPosition; } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); return null; } } - Angle angle = Radians.of(interpolate(stamptedTime1.timestamp(), stamptedTime2.timestamp(), - stamptedTime1.angle().in(Radians), stamptedTime2.angle().in(Radians), requestedTime)); + Angle angle = Radians.of(interpolate(firstTurretPosition.timestamp(), secondTurretPosition.timestamp(), + firstTurretPosition.angle().in(Radians), secondTurretPosition.angle().in(Radians), requestedTime)); AngularVelocity velocity = RPM - .of(interpolate(stamptedTime1.timestamp(), stamptedTime2.timestamp(), - stamptedTime1.velocity().in(RPM), stamptedTime2.velocity().in(RPM), requestedTime)); + .of(interpolate(firstTurretPosition.timestamp(), secondTurretPosition.timestamp(), + firstTurretPosition.velocity().in(RPM), secondTurretPosition.velocity().in(RPM), + requestedTime)); return new TurretPosition(angle, velocity, requestedTime); } diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index ca11b12..2c006d8 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -20,6 +20,7 @@ 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.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.robot.Constants.VisionConstants; @@ -74,12 +75,13 @@ public void periodic() { for (var result : m_camera2.getAllUnreadResults()) { Transform3d cameraTransform; - try { - cameraTransform = getTurretCameraTransform(result.getTimestampSeconds()); - } catch (Exception e) { - System.out.println(e.getMessage()); + cameraTransform = getTurretCameraTransform(result.getTimestampSeconds()); + + if (cameraTransform == null) { + System.out.println("Turret exceeded max velocity valid for reading april tags."); break; } + m_poseEstimatorTwo.setRobotToCameraTransform(cameraTransform); visionEstimationCameraTwo = m_poseEstimatorTwo.estimateCoprocMultiTagPose(result); @@ -142,31 +144,34 @@ private void updateEstimationStdDevs( } } - private Transform3d getTurretCameraTransform(double estimationTime) throws Exception { + private Transform3d getTurretCameraTransform(double estimationTime) { if (m_turretPositionSupplier.isEmpty()) return VisionConstants.kRobotToCamTwo; TurretPosition turretPosition = m_turretPositionSupplier.get().apply(estimationTime); // Getting the net velocity of the turret relative to the field - if (turretPosition.velocity().plus(m_robotAngularVelocitySupplier.get()) - .gt(VisionConstants.kMaxTurretVisionSpeed)) { - throw new Exception("Turret exceeds max speed for processing april tags."); + if (turretPosition == null || turretPosition.velocity().plus( + m_robotAngularVelocitySupplier.get()).abs(DegreesPerSecond) > VisionConstants.kMaxTurretVisionSpeed + .in(DegreesPerSecond)) { + return null; } - Angle turretAngle = turretPosition.angle(); + Distance cameraX = VisionConstants.kTurretCameraDistanceToCenter + .times(Math.cos(turretPosition.angle().in(Radians))) + .plus(VisionConstants.kTurretAxisOfRotation.getMeasureX()); + + Distance cameraY = VisionConstants.kTurretCameraDistanceToCenter + .times(Math.sin(turretPosition.angle().in(Radians))) + .plus(VisionConstants.kTurretAxisOfRotation.getMeasureY()); - 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))); + Translation3d cameraPosition = new Translation3d(cameraX, cameraY, + VisionConstants.kCameraTwoZ); - Distance cameraX = VisionConstants.kTurretAxisOfRotation.getMeasureX().plus(cameraXOffset); - Distance cameraY = VisionConstants.kTurretAxisOfRotation.getMeasureY().plus(cameraYOffset); - Distance cameraZ = VisionConstants.kTurretAxisOfRotation.getMeasureZ(); + Rotation3d cameraRotation = new Rotation3d(VisionConstants.kCameraTwoRoll, VisionConstants.kCameraTwoPitch, + turretPosition.angle().plus(VisionConstants.kCameraTwoYaw)); - return new Transform3d(cameraX, cameraY, cameraZ, - new Rotation3d(VisionConstants.kCameraTwoPitch, VisionConstants.kCameraTwoRoll, turretAngle)); + return new Transform3d(cameraPosition, cameraRotation); } private Matrix getCurrentStdDevs() { From d8896aa8b825c585feddc3ec93b5f6c4fd43d11f Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 16 Feb 2026 20:15:59 -0600 Subject: [PATCH 03/25] 180 degrees of error on point to april tag --- src/main/java/frc/robot/Constants.java | 14 +++++++++----- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../frc/robot/commands/AimCommandFactory.java | 13 +++++++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 5 +++-- .../frc/robot/subsystems/TurretSubsystem.java | 18 +++++++++++++++--- .../java/frc/robot/utils/PositionBuffer.java | 4 +++- src/main/java/frc/robot/utils/Vision.java | 16 +++++++++++----- 7 files changed, 56 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2f51c73..774ae2f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -173,14 +173,15 @@ public static final class VisionConstants { .loadField(AprilTagFields.kDefaultField); // Placeholder numbers - public static final Translation3d kTurretAxisOfRotation = new Translation3d(Meters.of(0.2), Meters.of(0.3), - Meters.of(0.3)); public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); public static final Distance kCameraTwoZ = Inches.of(18.0); - public static final Angle kCameraTwoPitch = Radians.of(15.0); - public static final Angle kCameraTwoRoll = Radians.of(0.0); - public static final Angle kCameraTwoYaw = Radians.of(0.0); + public static final Translation3d kTurretCenterOfRotation = new Translation3d(Meters.of(0.0), Meters.of(0.0), + Inches.of(18)); + + public static final Angle kCameraTwoPitch = Degrees.of(15.0); + public static final Angle kCameraTwoRoll = Degrees.of(0.0); + public static final Angle kCameraTwoYaw = Degrees.of(-21.0); public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); @@ -264,5 +265,8 @@ public static enum FieldLocations { // Placeholders public static final Angle kBlueLeftSideFeedHeading = Degrees.of(-40); public static final Angle kBlueRightSideFeedHeading = Degrees.of(-160); + + public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark) + .getTagPose(3).get().toPose2d(); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 258463b..baf6518 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +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.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; @@ -58,7 +59,7 @@ public RobotContainer() { private void configureBindings() { m_driverController.a() - .whileTrue(m_aimFactory.HoldTurretHeading(Degrees.of(10))); + .whileTrue(m_aimFactory.HoldTurretHeading(Degrees.of(90.0))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index de4880f..94b013f 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -80,4 +80,17 @@ public Command HoldTurretHeading(Angle heading) { m_turret.moveToAngle(heading.minus(m_drive.getHeading())); }, m_turret); } + + public Command PointTurretToFixture(Pose2d fixture) { + return new RunCommand(() -> { + Pose2d robotPose = m_drive.getPose(); + + double dx = fixture.getX() - robotPose.getX(); + double dy = fixture.getY() - robotPose.getY(); + + Angle angle = Radians.of(robotPose.getRotation().getRadians()).minus(Radians.of(Math.atan2(dy, dx))); + + m_turret.moveToAngle(angle); + }, m_turret); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 043b21b..67b8e05 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -400,8 +400,9 @@ 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); + // System.out.println("Vision applied."); + m_poseEstimator.addVisionMeasurement(estimation.m_pose, + estimation.m_timestamp, estimation.m_stdDevs); } public Fixtures.FieldLocations getRobotLocation() { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 2f5aa04..ca7547c 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.function.Supplier; @@ -39,12 +40,18 @@ public class TurretSubsystem extends SubsystemBase { private SparkMaxConfig m_config = new SparkMaxConfig(); + private int i = 0; + public TurretSubsystem() { 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); + + m_positionBuffer.pushElement(wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + RPM.of(m_absoluteEncoder.getVelocity()), + TurretConstants.kEncoderReadingDelay.in(Seconds)); } public void moveToAngle(Angle angle) { @@ -73,17 +80,22 @@ public Angle getRotation() { public TurretPosition getRotationAtTime(double timestamp) { return m_positionBuffer.getAngleAtTime(timestamp); + // return new TurretPosition(getRotation(), RotationsPerSecond.of(0.0), + // timestamp); } @Override public void periodic() { + m_positionBuffer.pushElement(wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + RPM.of(m_absoluteEncoder.getVelocity()), + TurretConstants.kEncoderReadingDelay.in(Seconds)); } // Connected to another periodic loop that runs quicker than 0.02 seconds public void pushCurrentEncoderReading() { - m_positionBuffer.pushElement(Rotations.of(m_absoluteEncoder.getPosition()), - RPM.of(m_absoluteEncoder.getVelocity()), - TurretConstants.kEncoderReadingDelay.in(Seconds)); + // m_positionBuffer.pushElement(wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + // RPM.of(m_absoluteEncoder.getVelocity()), + // 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 index 695cfc5..4a75068 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -7,6 +7,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants.NumericalConstants; public class PositionBuffer { @@ -31,7 +32,7 @@ private double interpolate(double x1, double x2, double y1, double y2, double x) double dy = y2 - y1; double dx = x - x1; - return (dx / dsx) * dy + y1; + return (dx / dsx > 0.0 ? dsx : NumericalConstants.kEpsilon) * dy + y1; } public TurretPosition getAngleAtTime(double requestedTime) { @@ -107,6 +108,7 @@ public TurretPosition getAngleAtTime(double requestedTime) { } try { + // System.out.println(m_positions.get(midpoint) + " is midpoint"); return m_positions.get(midpoint); } catch (Exception e) { System.out.println("Ring Buffer Exception: " + e.getMessage()); diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 2c006d8..7400a7e 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -91,7 +91,10 @@ public void periodic() { updateEstimationStdDevs(visionEstimationCameraTwo, result.getTargets(), m_poseEstimatorTwo); + // final var tmp = visionEstimationCameraTwo; + visionEstimationCameraTwo.ifPresent(estimation -> { + // System.out.println(tmp + ", " + cameraTransform); m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), estimation.timestampSeconds, getCurrentStdDevs())); }); @@ -158,18 +161,21 @@ private Transform3d getTurretCameraTransform(double estimationTime) { } Distance cameraX = VisionConstants.kTurretCameraDistanceToCenter - .times(Math.cos(turretPosition.angle().in(Radians))) - .plus(VisionConstants.kTurretAxisOfRotation.getMeasureX()); + .times(Math.cos(turretPosition.angle().minus(VisionConstants.kCameraTwoYaw).in(Radians))) + .plus(VisionConstants.kTurretCenterOfRotation.getMeasureX()); Distance cameraY = VisionConstants.kTurretCameraDistanceToCenter - .times(Math.sin(turretPosition.angle().in(Radians))) - .plus(VisionConstants.kTurretAxisOfRotation.getMeasureY()); + .times(Math.sin(turretPosition.angle().minus(VisionConstants.kCameraTwoYaw).in(Radians))) + .plus(VisionConstants.kTurretCenterOfRotation.getMeasureY()); Translation3d cameraPosition = new Translation3d(cameraX, cameraY, VisionConstants.kCameraTwoZ); Rotation3d cameraRotation = new Rotation3d(VisionConstants.kCameraTwoRoll, VisionConstants.kCameraTwoPitch, - turretPosition.angle().plus(VisionConstants.kCameraTwoYaw)); + turretPosition.angle()); + + // System.out.println(cameraX + ", " + cameraY + ", " + + // turretPosition.angle().in(Degrees)); return new Transform3d(cameraPosition, cameraRotation); } From b910044981f55507cfc8c78a2a1874b115470619 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 17 Feb 2026 16:41:31 -0600 Subject: [PATCH 04/25] FIX THE TURRET (YAYYYYY) --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/AimCommandFactory.java | 2 +- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index baf6518..d5e1b1c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,7 +59,7 @@ public RobotContainer() { private void configureBindings() { m_driverController.a() - .whileTrue(m_aimFactory.HoldTurretHeading(Degrees.of(90.0))); + .whileTrue(m_aimFactory.PointTurretToFixture(Fixtures.kRedHubAprilTag)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 94b013f..9b4aa2a 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -88,7 +88,7 @@ public Command PointTurretToFixture(Pose2d fixture) { double dx = fixture.getX() - robotPose.getX(); double dy = fixture.getY() - robotPose.getY(); - Angle angle = Radians.of(robotPose.getRotation().getRadians()).minus(Radians.of(Math.atan2(dy, dx))); + Angle angle = Radians.of(Math.atan2(dy, dx)).minus(Radians.of(robotPose.getRotation().getRadians())); m_turret.moveToAngle(angle); }, m_turret); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index ca7547c..9484f28 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -47,6 +47,8 @@ public TurretSubsystem() { m_config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); m_config.smartCurrentLimit(TurretConstants.kSmartCurrentLimit); m_config.idleMode(IdleMode.kBrake); + m_config.absoluteEncoder.inverted(true); + m_config.inverted(true); m_turretMotor.configure(m_config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); m_positionBuffer.pushElement(wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), From a9a7ceaaffbee8394daf0c0d05eb730ffbeb7074 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 17 Feb 2026 19:57:52 -0600 Subject: [PATCH 05/25] Fix turret camera rotation bug and attempt at general function to move turret to angle --- src/main/java/frc/robot/Constants.java | 14 ++-- src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/commands/AimCommandFactory.java | 68 ++++++++++++++++- .../frc/robot/subsystems/DriveSubsystem.java | 73 ++++++++++--------- .../frc/robot/subsystems/TurretSubsystem.java | 44 ++++++----- .../frc/robot/utils/UtilityFunctions.java | 28 +++++++ src/main/java/frc/robot/utils/Vision.java | 8 -- 8 files changed, 173 insertions(+), 72 deletions(-) create mode 100644 src/main/java/frc/robot/utils/UtilityFunctions.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 774ae2f..d22fe78 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -194,6 +194,8 @@ public static final class VisionConstants { public static final class NumericalConstants { public static final double kEpsilon = 1e-6; + public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); + public static final Angle kNoRotation = Radians.of(0.0); } public static final class TurretConstants { @@ -206,17 +208,19 @@ public static final class TurretConstants { 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 int kSmartCurrentLimit = 40; - public static final Angle kFeedMinAngle = Degrees.of(150); - public static final Angle kFeedMaxAngle = Degrees.of(200); + public static final Angle kFeedMinAngle1 = Degrees.of(150); + public static final Angle kFeedMaxAngle1 = Degrees.of(200); + + public static final Angle kFeedMinAngle2 = Degrees.of(30); + public static final Angle kFeedMaxAngle2 = Degrees.of(60); + + public static final Angle kOvershootAmount = Degrees.of(10.0); } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6cd1383..246eafd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,7 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); addPeriodic(m_robotContainer.pushTurretEncoderReading(), - Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); + Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); } @Override @@ -63,6 +63,7 @@ public void teleopInit() { @Override public void teleopPeriodic() { + m_robotContainer.periodic(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d5e1b1c..199d63d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.UtilityFunctions; public class RobotContainer { @@ -59,7 +60,7 @@ public RobotContainer() { private void configureBindings() { m_driverController.a() - .whileTrue(m_aimFactory.PointTurretToFixture(Fixtures.kRedHubAprilTag)); + .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(90))); } public Command getAutonomousCommand() { @@ -81,4 +82,8 @@ public Command feedPosition(Alliance alliance) { }, m_drive, m_turret); } + + public void periodic() { + m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); + } } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 9b4aa2a..672b3d7 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -5,6 +5,8 @@ import java.util.function.Supplier; +import com.ctre.phoenix.Util; + import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; @@ -16,9 +18,11 @@ import frc.robot.Constants; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.UtilityFunctions; public class AimCommandFactory { @@ -74,13 +78,39 @@ public Command Aim(Supplier isFeedingLeftSide) { }); } - public Command HoldTurretHeading(Angle heading) { + public Command MoveTurretToHeadingCommand(Angle heading) { return new RunCommand(() -> { - System.out.println(m_drive.getHeading()); - m_turret.moveToAngle(heading.minus(m_drive.getHeading())); + MoveTurretToHeading(heading); }, m_turret); } + public void MoveTurretToHeading(Angle heading) { + Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); + Angle robotRelativeTurretAngle = heading.minus(robotHeading); + + if (withinRange(TurretConstants.kFeedMinAngle1, TurretConstants.kFeedMaxAngle1, robotHeading) + || withinRange(TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2, robotHeading)) { + m_turret.moveToAngle(robotRelativeTurretAngle); + } else { + // Gets which ray the robot is closest to + Angle closest = getClosestAngle(heading, TurretConstants.kFeedMinAngle1, TurretConstants.kFeedMaxAngle1, + TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2); + + // The overshoot is negative if the robot has to move in a negative direction; + // same for positive + Angle overshoot = robotRelativeTurretAngle.minus(closest).in(Degrees) > 0.0 + ? TurretConstants.kOvershootAmount + : TurretConstants.kOvershootAmount.times(-1.0); + + closest = closest.plus(overshoot); + + Angle driveTarget = heading.minus(closest); + + m_drive.moveToAngle(driveTarget); + m_turret.moveToAngle(closest); + } + } + public Command PointTurretToFixture(Pose2d fixture) { return new RunCommand(() -> { Pose2d robotPose = m_drive.getPose(); @@ -90,7 +120,37 @@ public Command PointTurretToFixture(Pose2d fixture) { Angle angle = Radians.of(Math.atan2(dy, dx)).minus(Radians.of(robotPose.getRotation().getRadians())); - m_turret.moveToAngle(angle); + MoveTurretToHeading(angle); }, m_turret); } + + private static boolean withinRange(Angle min, Angle max, Angle a) { + a = UtilityFunctions.WrapAngle(a); + return a.gt(min) && a.lt(max); + } + + private static Angle angleDiff(Angle a1, Angle a2) { + a1 = UtilityFunctions.WrapTo180(a1); + a2 = UtilityFunctions.WrapTo180(a2); + + return UtilityFunctions.WrapTo180(a1.minus(a2)); + } + + private static Angle getClosestAngle(Angle a, Angle... others) { + a = UtilityFunctions.WrapTo180(a); + + Angle closest = UtilityFunctions.WrapTo180(others[others.length - 1]); + double closestDistance = angleDiff(others[others.length - 1], a).abs(Degrees); + + for (int i = 0; i < others.length - 1; i++) { + final double curDist = angleDiff(UtilityFunctions.WrapTo180(others[i]), a).abs(Degrees); + + if (curDist < closestDistance) { + closestDistance = curDist; + closest = others[i]; + } + } + + return closest; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 67b8e05..8cb3b46 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -26,6 +26,7 @@ import frc.robot.Robot; import frc.robot.utils.ShuffleboardPid; import frc.robot.utils.TurretPosition; +import frc.robot.utils.UtilityFunctions; import frc.robot.utils.VisionEstimation; import frc.robot.utils.Vision; import frc.robot.Constants.DriveConstants; @@ -167,14 +168,23 @@ ChassisSpeeds getRobotRelativeSpeeds() { return DriveConstants.kDriveKinematics.toChassisSpeeds(fl, fr, rl, rr); } - public Command moveToAngle(Angle angle) { + public Command moveToAngleCommand(Angle angle) { return new InstantCommand( () -> { - m_isManualRotate = false; - m_targetAutoAngle = angle; + moveToAngle(angle); }); } + public void moveToAngle(Angle angle) { + m_isManualRotate = false; + m_targetAutoAngle = angle; + } + + public void moveByAngle(Angle angle) { + m_isManualRotate = false; + m_targetAutoAngle = getHeading().plus(angle); + } + public RangeType faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { Angle robotAngle = getHeading(); // System.out.println(robotAngle); @@ -240,10 +250,9 @@ public void periodic() { }); } - if (!m_isManualRotate && Math - .abs(m_targetAutoAngle.in(Degrees) - getHeading().in(Degrees)) < DriveConstants.kTurnToAngleTolerance - .in(Degrees)) { - + if (!m_isManualRotate + && UtilityFunctions.WrapAngle(UtilityFunctions.WrapAngle(getHeading()).minus(m_targetAutoAngle)) + .abs(Degrees) < DriveConstants.kTurnToAngleTolerance.in(Degrees)) { m_isManualRotate = true; } @@ -308,24 +317,29 @@ public void resetOdometry(Pose2d pose) { public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { // 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)); + // if (!m_isManualRotate) + // System.out + // .println("Setpoint: " + getOptimalAngle(m_targetAutoAngle, + // getHeading()).in(Radians) + ", Current: " + // + getHeading().in(Radians)); + + if (Math.abs(rot) > NumericalConstants.kEpsilon) { + m_isManualRotate = true; + } - double pidCalculation = m_pidController.calculate(getHeading().in(Radians), + final double pidCalculation = m_pidController.calculate(getHeading().in(Radians), getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians)); - double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeed.magnitude(); - double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeed.magnitude(); - double rotDelivered = (m_isManualRotate) + final double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeed.magnitude(); + final double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeed.magnitude(); + final double rotDelivered = (m_isManualRotate) ? rot * DriveConstants.kMaxAngularSpeed.magnitude() : pidCalculation; // System.out.println("Target " + m_targetAutoAngle + ", Current" + // getHeading()); - var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + final var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, new Rotation2d(getHeading())) @@ -338,8 +352,8 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ m_rearLeft.setDesiredState(swerveModuleStates[2]); m_rearRight.setDesiredState(swerveModuleStates[3]); - double latestTime = Timer.getFPGATimestamp(); - double timeElapsed = latestTime - m_latestTime < 0.20 ? latestTime - m_latestTime + final double latestTime = Timer.getFPGATimestamp(); + final double timeElapsed = latestTime - m_latestTime < 0.20 ? latestTime - m_latestTime : DriveConstants.kPeriodicInterval.in(Seconds); m_latestTime = latestTime; @@ -447,21 +461,8 @@ private AngularVelocity getAngularVelocity() { return DegreesPerSecond.of(pidgey.getAngularVelocityZDevice().getValueAsDouble()); } - private static Angle wrapAngle(Angle heading) { - Angle robotRotations = Radians - .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); - } - - return wrap; - } - private static Angle getOptimalAngle(Angle target, Angle robotHeading) { - Angle wrappedRobotAngle = wrapAngle(robotHeading); + Angle wrappedRobotAngle = UtilityFunctions.WrapAngle(robotHeading); Angle delta = target.minus(wrappedRobotAngle); @@ -477,16 +478,16 @@ private static Angle getOptimalAngle(Angle target, Angle robotHeading) { } private static boolean withinRange(Angle min, Angle max, Angle angle) { - angle = wrapAngle(angle); + angle = UtilityFunctions.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); + t1 = UtilityFunctions.WrapAngle(t1); + t2 = UtilityFunctions.WrapAngle(t2); + angle = UtilityFunctions.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 9484f28..ff75b91 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.RPM; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; @@ -23,10 +24,16 @@ import edu.wpi.first.math.system.plant.DCMotor; import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; import frc.robot.utils.TurretPosition; +import frc.robot.utils.UtilityFunctions; public class TurretSubsystem extends SubsystemBase { @@ -40,7 +47,12 @@ public class TurretSubsystem extends SubsystemBase { private SparkMaxConfig m_config = new SparkMaxConfig(); - private int i = 0; + Mechanism2d m_simMech = new Mechanism2d(4.0, 4.0); + MechanismRoot2d m_mechRoot = m_simMech.getRoot("Turret Root", 2.0, 2.0); + MechanismLigament2d m_simLigament = new MechanismLigament2d("Turret", 2, 0); + + Angle m_targetAngle = Degrees.of(0.0); + Angle m_simAngle = Degrees.of(0.0); public TurretSubsystem() { m_config.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); @@ -51,14 +63,16 @@ public TurretSubsystem() { m_config.inverted(true); m_turretMotor.configure(m_config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - m_positionBuffer.pushElement(wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), RPM.of(m_absoluteEncoder.getVelocity()), TurretConstants.kEncoderReadingDelay.in(Seconds)); + + m_simLigament = m_mechRoot.append(m_simLigament); } public void moveToAngle(Angle angle) { - angle = wrapAngle(angle); + angle = UtilityFunctions.WrapAngle(angle); if (angle.gt(TurretConstants.kMaxAngle)) { System.out @@ -73,11 +87,16 @@ public void moveToAngle(Angle angle) { } // System.out.println("Target angle is " + angle); + m_targetAngle = angle; m_turretClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); } public Angle getRotation() { - return wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())); + return UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())); + } + + public void addDriveHeading(Angle angle) { + m_simAngle = m_targetAngle.plus(angle); } public TurretPosition getRotationAtTime(double timestamp) { @@ -88,30 +107,21 @@ public TurretPosition getRotationAtTime(double timestamp) { @Override public void periodic() { - m_positionBuffer.pushElement(wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), RPM.of(m_absoluteEncoder.getVelocity()), TurretConstants.kEncoderReadingDelay.in(Seconds)); } // Connected to another periodic loop that runs quicker than 0.02 seconds public void pushCurrentEncoderReading() { - // m_positionBuffer.pushElement(wrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), + // m_positionBuffer.pushElement(UtilityFunctions.WrapAngle(Rotations.of(m_absoluteEncoder.getPosition())), // RPM.of(m_absoluteEncoder.getVelocity()), // TurretConstants.kEncoderReadingDelay.in(Seconds)); } @Override public void simulationPeriodic() { - m_simTurretMotor.setVelocity(1.0); - // 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; + m_simLigament.setAngle(m_simAngle.in(Degrees)); + SmartDashboard.putData("Turret Rotation", m_simMech); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java new file mode 100644 index 0000000..5ac158c --- /dev/null +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -0,0 +1,28 @@ +package frc.robot.utils; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.units.measure.Angle; +import frc.robot.Constants.NumericalConstants; + +public final class UtilityFunctions { + public static Angle WrapAngle(Angle angle) { + Angle wrap = Radians.of((angle.in(Radians) % NumericalConstants.kFullRotation.in(Radians))); + if (wrap.lt(NumericalConstants.kNoRotation)) { + wrap = wrap.plus(NumericalConstants.kFullRotation); + } + + return wrap; + } + + public static Angle WrapTo180(Angle angle) { + Angle wrapped = WrapAngle(angle); + + if (wrapped.gt(Degrees.of(180))) { + wrapped = wrapped.minus(NumericalConstants.kFullRotation); + } + + return wrapped; + } +} diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 7400a7e..24ec10b 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -9,15 +9,10 @@ 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.geometry.Translation3d; @@ -25,9 +20,6 @@ import edu.wpi.first.math.numbers.N3; import frc.robot.Constants.VisionConstants; 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 Vision { From 25aaed45632a6399936cf35d69758737a68a6940 Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Thu, 19 Feb 2026 08:42:28 -0600 Subject: [PATCH 06/25] Debug attempt #1 to figure out why robotHeading is always 90 degrees --- simgui-ds.json | 8 ++-- src/main/java/frc/robot/Constants.java | 19 +++++--- .../frc/robot/commands/AimCommandFactory.java | 44 +++++++++---------- .../frc/robot/subsystems/TurretSubsystem.java | 19 +++----- .../frc/robot/utils/UtilityFunctions.java | 9 +--- 5 files changed, 45 insertions(+), 54 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index f671bb9..c77974a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -16,15 +16,14 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 }, {}, { - "decKey": 79, - "incKey": 80 + "decKey": 81, + "incKey": 69 } ], "axisCount": 5, @@ -101,8 +100,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d22fe78..d2d8eba 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,16 +3,11 @@ // 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.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.geometry.Translation2d; @@ -22,6 +17,16 @@ 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.Degrees; +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.RPM; +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; @@ -214,8 +219,8 @@ public static final class TurretConstants { public static final int kSmartCurrentLimit = 40; - public static final Angle kFeedMinAngle1 = Degrees.of(150); - public static final Angle kFeedMaxAngle1 = Degrees.of(200); + public static final Angle kFeedMinAngle1 = Degrees.of(300); + public static final Angle kFeedMaxAngle1 = Degrees.of(330); public static final Angle kFeedMinAngle2 = Degrees.of(30); public static final Angle kFeedMaxAngle2 = Degrees.of(60); diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 672b3d7..6b52e40 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -1,24 +1,16 @@ package frc.robot.commands; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Radians; - import java.util.function.Supplier; -import com.ctre.phoenix.Util; - -import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; -import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -93,6 +85,7 @@ public void MoveTurretToHeading(Angle heading) { m_turret.moveToAngle(robotRelativeTurretAngle); } else { // Gets which ray the robot is closest to + System.out.println(m_drive.getHeading() .in(Degrees) + " is arg"); Angle closest = getClosestAngle(heading, TurretConstants.kFeedMinAngle1, TurretConstants.kFeedMaxAngle1, TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2); @@ -130,24 +123,31 @@ private static boolean withinRange(Angle min, Angle max, Angle a) { } private static Angle angleDiff(Angle a1, Angle a2) { - a1 = UtilityFunctions.WrapTo180(a1); - a2 = UtilityFunctions.WrapTo180(a2); - - return UtilityFunctions.WrapTo180(a1.minus(a2)); + Angle diff = a1.minus(a2); + return UtilityFunctions.WrapTo180(diff); } private static Angle getClosestAngle(Angle a, Angle... others) { - a = UtilityFunctions.WrapTo180(a); + a = UtilityFunctions.WrapAngle(a); + + for (Angle as : others) { + System.out.print(as.in(Degrees) + " "); + } + System.out.print(a.in(Degrees) + " is robot heading"); + System.out.println(); + + Angle closest = UtilityFunctions.WrapAngle(others[0]); + double closestDistance = angleDiff(a, closest).abs(Degrees); - Angle closest = UtilityFunctions.WrapTo180(others[others.length - 1]); - double closestDistance = angleDiff(others[others.length - 1], a).abs(Degrees); + for (int i = 1; i < others.length; i++) { + Angle candidate = UtilityFunctions.WrapAngle(others[i]); + double dif = angleDiff(a, candidate).abs(Degrees); - for (int i = 0; i < others.length - 1; i++) { - final double curDist = angleDiff(UtilityFunctions.WrapTo180(others[i]), a).abs(Degrees); + if (dif < closestDistance) { + closest = candidate; + closestDistance = dif; - if (curDist < closestDistance) { - closestDistance = curDist; - closest = others[i]; + System.out.println(closest + " " + others.length); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index ff75b91..3762860 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,27 +1,21 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import java.util.function.Supplier; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; import com.revrobotics.sim.SparkMaxSim; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; 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 com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; +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.wpilibj.smartdashboard.Mechanism2d; @@ -29,7 +23,6 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; import frc.robot.utils.TurretPosition; diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java index 5ac158c..98c77dc 100644 --- a/src/main/java/frc/robot/utils/UtilityFunctions.java +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -2,18 +2,13 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; - import edu.wpi.first.units.measure.Angle; import frc.robot.Constants.NumericalConstants; public final class UtilityFunctions { public static Angle WrapAngle(Angle angle) { - Angle wrap = Radians.of((angle.in(Radians) % NumericalConstants.kFullRotation.in(Radians))); - if (wrap.lt(NumericalConstants.kNoRotation)) { - wrap = wrap.plus(NumericalConstants.kFullRotation); - } - - return wrap; + return Radians.of(((angle.in(Radians) % NumericalConstants.kFullRotation.in(Radians)) + NumericalConstants.kFullRotation.in(Radians)) % NumericalConstants.kFullRotation + .in(Radians)); } public static Angle WrapTo180(Angle angle) { From 0b0a922d71c9010c5084944cd6255a719fbd4d1e Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 20 Feb 2026 17:16:07 -0600 Subject: [PATCH 07/25] IT WORKSSS!!1!!1! --- src/main/java/frc/robot/Constants.java | 8 ++-- src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/commands/AimCommandFactory.java | 29 +++++++----- .../frc/robot/subsystems/TurretSubsystem.java | 44 ++++++++++++++++--- 4 files changed, 60 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d2d8eba..40e03dd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -219,11 +219,11 @@ public static final class TurretConstants { public static final int kSmartCurrentLimit = 40; - public static final Angle kFeedMinAngle1 = Degrees.of(300); - public static final Angle kFeedMaxAngle1 = Degrees.of(330); + public static final Angle kFeedMinAngle1 = Degrees.of(240); + public static final Angle kFeedMaxAngle1 = Degrees.of(300); - public static final Angle kFeedMinAngle2 = Degrees.of(30); - public static final Angle kFeedMaxAngle2 = Degrees.of(60); + public static final Angle kFeedMinAngle2 = Degrees.of(60); + public static final Angle kFeedMaxAngle2 = Degrees.of(120); public static final Angle kOvershootAmount = Degrees.of(10.0); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 199d63d..2c8d562 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -60,7 +60,7 @@ public RobotContainer() { private void configureBindings() { m_driverController.a() - .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(90))); + .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); } public Command getAutonomousCommand() { @@ -85,5 +85,6 @@ public Command feedPosition(Alliance alliance) { public void periodic() { m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); + } } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 6b52e40..17cd99e 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -78,20 +78,22 @@ public Command MoveTurretToHeadingCommand(Angle heading) { public void MoveTurretToHeading(Angle heading) { Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); - Angle robotRelativeTurretAngle = heading.minus(robotHeading); - if (withinRange(TurretConstants.kFeedMinAngle1, TurretConstants.kFeedMaxAngle1, robotHeading) - || withinRange(TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2, robotHeading)) { + Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading)); + + if (withinRange(TurretConstants.kFeedMinAngle1, TurretConstants.kFeedMaxAngle1, robotRelativeTurretAngle) + || withinRange(TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2, + robotRelativeTurretAngle)) { m_turret.moveToAngle(robotRelativeTurretAngle); } else { // Gets which ray the robot is closest to - System.out.println(m_drive.getHeading() .in(Degrees) + " is arg"); - Angle closest = getClosestAngle(heading, TurretConstants.kFeedMinAngle1, TurretConstants.kFeedMaxAngle1, + Angle closest = getClosestAngle(robotRelativeTurretAngle, TurretConstants.kFeedMinAngle1, + TurretConstants.kFeedMaxAngle1, TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2); // The overshoot is negative if the robot has to move in a negative direction; // same for positive - Angle overshoot = robotRelativeTurretAngle.minus(closest).in(Degrees) > 0.0 + Angle overshoot = robotRelativeTurretAngle.minus(closest).in(Degrees) < 0.0 ? TurretConstants.kOvershootAmount : TurretConstants.kOvershootAmount.times(-1.0); @@ -119,6 +121,9 @@ public Command PointTurretToFixture(Pose2d fixture) { private static boolean withinRange(Angle min, Angle max, Angle a) { a = UtilityFunctions.WrapAngle(a); + min = UtilityFunctions.WrapAngle(min); + max = UtilityFunctions.WrapAngle(max); + return a.gt(min) && a.lt(max); } @@ -130,11 +135,11 @@ private static Angle angleDiff(Angle a1, Angle a2) { private static Angle getClosestAngle(Angle a, Angle... others) { a = UtilityFunctions.WrapAngle(a); - for (Angle as : others) { - System.out.print(as.in(Degrees) + " "); - } - System.out.print(a.in(Degrees) + " is robot heading"); - System.out.println(); + // for (Angle as : others) { + // System.out.print(as.in(Degrees) + " "); + // } + // System.out.print(a.in(Degrees) + " is robot heading"); + // System.out.println(); Angle closest = UtilityFunctions.WrapAngle(others[0]); double closestDistance = angleDiff(a, closest).abs(Degrees); @@ -147,7 +152,7 @@ private static Angle getClosestAngle(Angle a, Angle... others) { closest = candidate; closestDistance = dif; - System.out.println(closest + " " + others.length); + // System.out.println(closest + " " + others.length); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 3762860..47aeef0 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -17,11 +17,15 @@ import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; + +import java.awt.Color; + import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; @@ -40,12 +44,20 @@ public class TurretSubsystem extends SubsystemBase { private SparkMaxConfig m_config = new SparkMaxConfig(); - Mechanism2d m_simMech = new Mechanism2d(4.0, 4.0); - MechanismRoot2d m_mechRoot = m_simMech.getRoot("Turret Root", 2.0, 2.0); - MechanismLigament2d m_simLigament = new MechanismLigament2d("Turret", 2, 0); + private Mechanism2d m_simMech = new Mechanism2d(4.0, 4.0); + private MechanismRoot2d m_mechRoot = m_simMech.getRoot("Turret Root", 2.0, 2.0); + + private MechanismLigament2d m_simLigament = new MechanismLigament2d("Turret", 2, 0); + + private MechanismLigament2d m_min1 = new MechanismLigament2d("min1", 2, 0); + private MechanismLigament2d m_max1 = new MechanismLigament2d("max1", 2, 0); + private MechanismLigament2d m_min2 = new MechanismLigament2d("min2", 2, 0); + private MechanismLigament2d m_max2 = new MechanismLigament2d("max2", 2, 0); + + private Angle robotRotation; - Angle m_targetAngle = Degrees.of(0.0); - Angle m_simAngle = Degrees.of(0.0); + private Angle m_targetAngle = Degrees.of(0.0); + private Angle m_simAngle = Degrees.of(0.0); public TurretSubsystem() { m_config.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); @@ -61,6 +73,20 @@ public TurretSubsystem() { TurretConstants.kEncoderReadingDelay.in(Seconds)); m_simLigament = m_mechRoot.append(m_simLigament); + + m_min1 = m_mechRoot.append(m_min1); + m_min1.setColor(new Color8Bit("#FF00FF")); + + m_max1 = m_mechRoot.append(m_max1); + m_max1.setColor(new Color8Bit("#FF00FF")); + + m_min2 = m_mechRoot.append(m_min2); + m_min2.setColor(new Color8Bit("#FF0000")); + + m_max2 = m_mechRoot.append(m_max2); + m_max2.setColor(new Color8Bit("#FF0000")); + + robotRotation = Degrees.of(0); } public void moveToAngle(Angle angle) { @@ -89,7 +115,7 @@ public Angle getRotation() { } public void addDriveHeading(Angle angle) { - m_simAngle = m_targetAngle.plus(angle); + robotRotation = angle; } public TurretPosition getRotationAtTime(double timestamp) { @@ -114,7 +140,11 @@ public void pushCurrentEncoderReading() { @Override public void simulationPeriodic() { - m_simLigament.setAngle(m_simAngle.in(Degrees)); + m_simLigament.setAngle(m_targetAngle.plus(robotRotation).in(Degrees)); + m_min1.setAngle(TurretConstants.kFeedMinAngle1.plus(robotRotation).in(Degrees)); + m_max1.setAngle(TurretConstants.kFeedMaxAngle1.plus(robotRotation).in(Degrees)); + m_min2.setAngle(TurretConstants.kFeedMinAngle2.plus(robotRotation).in(Degrees)); + m_max2.setAngle(TurretConstants.kFeedMaxAngle2.plus(robotRotation).in(Degrees)); SmartDashboard.putData("Turret Rotation", m_simMech); } } \ No newline at end of file From 07f4bf3eeb0018d7877da1b6a047f7058b4ac703 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 21 Feb 2026 14:54:49 -0600 Subject: [PATCH 08/25] A beginning to automatic aiming --- simgui-ds.json | 3 +- src/main/java/frc/robot/Constants.java | 11 +++++ src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/commands/AimCommandFactory.java | 34 +++++++++++++++ .../frc/robot/subsystems/DriveSubsystem.java | 7 ++++ .../robot/subsystems/ShooterSubsystem.java | 41 +++++++++++++++++++ .../frc/robot/subsystems/TurretSubsystem.java | 6 +++ 7 files changed, 101 insertions(+), 2 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index c77974a..37ec5eb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -100,7 +100,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 40e03dd..bf2186b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,7 +8,9 @@ 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.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -34,6 +36,7 @@ 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.Velocity; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -201,6 +204,7 @@ public static final class NumericalConstants { public static final double kEpsilon = 1e-6; public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); public static final Angle kNoRotation = Radians.of(0.0); + public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.807); } public static final class TurretConstants { @@ -226,6 +230,9 @@ public static final class TurretConstants { public static final Angle kFeedMaxAngle2 = Degrees.of(120); public static final Angle kOvershootAmount = Degrees.of(10.0); + + public static final Transform2d kTurretOffset = new Transform2d(Meters.of(0.5), Meters.of(0.5), + new Rotation2d()); } public static final class ShooterConstants { @@ -244,6 +251,10 @@ public static final class ShooterConstants { public static final double kHoodGearRatio = (62 / 25) * (14 / 218); public static final Angle kFeedAngle = Degrees.of(90.0); + + public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); + + public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); } public static final class Fixtures { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c8d562..39c708d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,6 +85,5 @@ public Command feedPosition(Alliance alliance) { public void periodic() { m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); - } } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 17cd99e..f190a0a 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -3,14 +3,23 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; 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.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.Constants.Fixtures; +import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -70,6 +79,31 @@ public Command Aim(Supplier isFeedingLeftSide) { }); } + public void AimToHub() { + Pose2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub + : Fixtures.kRedAllianceHub; + + Pose2d turretPose = m_drive.getPose().plus(TurretConstants.kTurretOffset).rotateBy(new Rotation2d()); + ChassisSpeeds robotSpeeds = m_drive.getChassisSpeeds(); + + Pose2d distanceToHub = hubPosition.relativeTo(turretPose); + + LinearVelocity xTurretVelocity = MetersPerSecond.of(ShooterConstants.kMuzzleVelocity.in(MetersPerSecond) + * Math.cos(turretPose.getRotation().getRadians())) + .plus(MetersPerSecond.of(robotSpeeds.vxMetersPerSecond)); + + LinearVelocity yTurretVelocity = MetersPerSecond.of(ShooterConstants.kMuzzleVelocity.in(MetersPerSecond) + * Math.sin(turretPose.getRotation().getRadians())) + .plus(MetersPerSecond.of(robotSpeeds.vyMetersPerSecond)); + + Distance xOffTarget = Meters.of((xTurretVelocity.in(MetersPerSecond) / distanceToHub.getMeasureX().in(Meters)) + * xTurretVelocity.in(MetersPerSecond)); + Distance yOffTarget = Meters.of((yTurretVelocity.in(MetersPerSecond) / distanceToHub.getMeasureY().in(Meters)) + * yTurretVelocity.in(MetersPerSecond)); + + Pose2d newPoseToAimTo = hubPosition.plus(new Transform2d(xOffTarget, yOffTarget, new Rotation2d())); + } + public Command MoveTurretToHeadingCommand(Angle heading) { return new RunCommand(() -> { MoveTurretToHeading(heading); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 8cb3b46..3c1e4ab 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -419,6 +419,13 @@ public void addVisionMeasurement(VisionEstimation estimation) { estimation.m_timestamp, estimation.m_stdDevs); } + public ChassisSpeeds getChassisSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds( + DriveConstants.kDriveKinematics.toChassisSpeeds(m_frontLeft.getState(), m_frontRight.getState(), + m_rearLeft.getState(), m_rearRight.getState()), + new Rotation2d(getHeading())); + } + public Fixtures.FieldLocations getRobotLocation() { Optional alliance = DriverStation.getAlliance(); Pose2d robotPose = getPose(); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5c29608..c09db21 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -12,9 +12,13 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; public class ShooterSubsystem extends SubsystemBase { @@ -51,6 +55,43 @@ public ShooterSubsystem() { m_hoodMotor.configure(m_hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } + // Think of this as controlling the turret on a cross-section going from the + // turret at the angle it is at and the target. Forward is the distance away + // from the turret while maxZ is the maximum Z height you want the ball to + // have. endZ is the z position you want the ball to hit once forward is + // achieved + public LinearVelocity shootToPose(Distance forward, Distance maxZ, Distance endZ) { + double forwardMeters = forward.in(Meters); + double maxZMeters = maxZ.in(Meters); + double endZMeters = endZ.in(Meters); + double gravityMpsps = NumericalConstants.kGravity.in(MetersPerSecondPerSecond); + + LinearVelocity vy = MetersPerSecond.of(Math + .sqrt((2.0 * Math.pow(gravityMpsps, 2.0) * maxZMeters) + / (2 * gravityMpsps - 1.0))); + + double vyMps = vy.in(MetersPerSecond); + + LinearVelocity vx = MetersPerSecond + .of((-2.0 * vyMps * forwardMeters + Math.sqrt(Math.pow(2.0 * vyMps * forwardMeters, 2.0) + + 8.0 * endZMeters * gravityMpsps * Math.pow(forwardMeters, 2.0))) / (-4.0 * endZMeters)); + + double vxMps = vx.in(MetersPerSecond); + + LinearVelocity shooterMuzzleVelocity = MetersPerSecond.of(Math.pow(vxMps, 2.0) + Math.pow(vyMps, 2.0)); + Angle shooterAngle = Radians.of(Math.acos(vxMps / shooterMuzzleVelocity.in(MetersPerSecond))); + + if (shooterMuzzleVelocity.gt(ShooterConstants.kMaxMuzzleVelocity)) { + System.out.println("Necessary velocity to hit target " + shooterMuzzleVelocity + + " beyond max velocity of " + ShooterConstants.kMaxMuzzleVelocity + "."); + return null; + } + + Aim(shooterAngle); + + return shooterMuzzleVelocity; + } + public void Aim(Angle angle) { 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 47aeef0..1f17134 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; @@ -21,6 +22,7 @@ import java.awt.Color; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -138,6 +140,10 @@ public void pushCurrentEncoderReading() { // TurretConstants.kEncoderReadingDelay.in(Seconds)); } + public LinearVelocity getMuzzleVelocityAtHoodAngle() { + return MetersPerSecond.of(5); + } + @Override public void simulationPeriodic() { m_simLigament.setAngle(m_targetAngle.plus(robotRotation).in(Degrees)); From 831f5841e105de66eeb2cb7a4e0036c91917c6c1 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 21 Feb 2026 20:56:13 -0600 Subject: [PATCH 09/25] Working calculations for calculating hub target solution --- src/main/java/frc/robot/Constants.java | 34 ++++- src/main/java/frc/robot/RobotContainer.java | 23 ++++ .../frc/robot/commands/AimCommandFactory.java | 128 +++++++++++++++--- .../frc/robot/subsystems/DriveSubsystem.java | 8 +- .../robot/subsystems/ShooterSubsystem.java | 1 - .../frc/robot/subsystems/TurretSubsystem.java | 2 - .../java/frc/robot/utils/PositionBuffer.java | 19 +-- .../java/frc/robot/utils/ShootingEntry.java | 13 ++ .../java/frc/robot/utils/TargetSolution.java | 10 ++ .../frc/robot/utils/UtilityFunctions.java | 13 +- 10 files changed, 204 insertions(+), 47 deletions(-) create mode 100644 src/main/java/frc/robot/utils/ShootingEntry.java create mode 100644 src/main/java/frc/robot/utils/TargetSolution.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bf2186b..6ee8e19 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,6 +29,9 @@ 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 java.util.HashMap; + import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -37,6 +40,7 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Velocity; +import frc.robot.utils.ShootingEntry; /** * The Constants class provides a convenient place for teams to hold robot-wide @@ -231,8 +235,7 @@ public static final class TurretConstants { public static final Angle kOvershootAmount = Degrees.of(10.0); - public static final Transform2d kTurretOffset = new Transform2d(Meters.of(0.5), Meters.of(0.5), - new Rotation2d()); + public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); } public static final class ShooterConstants { @@ -252,19 +255,38 @@ public static final class ShooterConstants { public static final Angle kFeedAngle = Degrees.of(90.0); + public static final AngularVelocity kPlaceholderWheelVelocity = RPM.of(2000); public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); + + public static final ShootingEntry[] kShootingEntries = { + new ShootingEntry(Meters.of(0.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(1.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(2.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(3.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(4.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + new ShootingEntry(Meters.of(5.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, Seconds.of(1.0), + kFeedAngle), + }; + + public static final LinearVelocity kMaxStationaryVelocity = MetersPerSecond.of(1e-1); } public static final class Fixtures { - public static final Pose2d kRedAllianceHub = new Pose2d(); - public static final Pose2d kBlueAllianceHub = new Pose2d(); + public static final Translation2d kRedAllianceHub = new Translation2d(Inches.of(182.11), Inches.of(154.84)); + public static final Translation2d kBlueAllianceHub = new Translation2d(Inches.of(182.11), + Inches.of(651.22 - 182.11)); // 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(); + public static final Translation2d kTopFeedPose = new Translation2d(); + public static final Translation2d kBottomFeedPose = new Translation2d(); public static final Distance kFieldYMidpoint = Inches.of(317.69 / 2.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 39c708d..dca0230 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,18 @@ package frc.robot; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -22,6 +28,7 @@ import frc.robot.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.TargetSolution; import frc.robot.utils.UtilityFunctions; public class RobotContainer { @@ -37,6 +44,7 @@ public class RobotContainer { private final DriveSubsystem m_drive = new DriveSubsystem(m_turret::getRotationAtTime); AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret); + Field2d m_field; public RobotContainer() { m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); @@ -56,6 +64,8 @@ public RobotContainer() { -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), true), m_drive)); + + m_field = m_drive.getField(); } private void configureBindings() { @@ -85,5 +95,18 @@ public Command feedPosition(Alliance alliance) { public void periodic() { m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); + + TargetSolution solution = m_aimFactory.GetHubAimSolution(); + + Pose2d robotPose = m_drive.getPose(); + + Distance xDist = Meters.of(solution.distance().in(Meters) + * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureX()); + Distance yDist = Meters.of(solution.distance().in(Meters) + * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureY()); + + Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); + + m_field.getObject("targetPose").setPose(targetPose); } } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index f190a0a..1ddee3e 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -5,15 +5,21 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -23,6 +29,8 @@ import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.ShootingEntry; +import frc.robot.utils.TargetSolution; import frc.robot.utils.UtilityFunctions; public class AimCommandFactory { @@ -43,11 +51,12 @@ public Command Aim(Supplier isFeedingLeftSide) { switch (location) { case AllianceSide: { - Pose2d fixturePose = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub + Translation2d fixtureTranslation = DriverStation.getAlliance().get() == Alliance.Blue + ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; - double dy = fixturePose.getX() - robotPose.getX(); - double dx = fixturePose.getY() - robotPose.getY(); + double dy = fixtureTranslation.getX() - robotPose.getX(); + double dx = fixtureTranslation.getY() - robotPose.getY(); Angle absHeading = Radians.of(Math.atan2(dy, dx)); Angle relHeading = absHeading.minus(robotAngle); @@ -79,29 +88,24 @@ public Command Aim(Supplier isFeedingLeftSide) { }); } - public void AimToHub() { - Pose2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub + public TargetSolution GetHubAimSolution() { + Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; - Pose2d turretPose = m_drive.getPose().plus(TurretConstants.kTurretOffset).rotateBy(new Rotation2d()); - ChassisSpeeds robotSpeeds = m_drive.getChassisSpeeds(); - - Pose2d distanceToHub = hubPosition.relativeTo(turretPose); + Translation2d turretTranslation = m_drive.getPose().getTranslation().plus(TurretConstants.kTurretOffset); - LinearVelocity xTurretVelocity = MetersPerSecond.of(ShooterConstants.kMuzzleVelocity.in(MetersPerSecond) - * Math.cos(turretPose.getRotation().getRadians())) - .plus(MetersPerSecond.of(robotSpeeds.vxMetersPerSecond)); + Translation2d translationToHub = hubPosition.minus(turretTranslation); - LinearVelocity yTurretVelocity = MetersPerSecond.of(ShooterConstants.kMuzzleVelocity.in(MetersPerSecond) - * Math.sin(turretPose.getRotation().getRadians())) - .plus(MetersPerSecond.of(robotSpeeds.vyMetersPerSecond)); + Distance turretToHubDistance = Meters + .of(Math.hypot(translationToHub.getMeasureY().in(Meters), translationToHub.getMeasureX().in(Meters))); + Angle turretToHubAngle = Radians + .of(Math.atan2(translationToHub.getMeasureY().in(Meters), translationToHub.getMeasureX().in(Meters))); - Distance xOffTarget = Meters.of((xTurretVelocity.in(MetersPerSecond) / distanceToHub.getMeasureX().in(Meters)) - * xTurretVelocity.in(MetersPerSecond)); - Distance yOffTarget = Meters.of((yTurretVelocity.in(MetersPerSecond) / distanceToHub.getMeasureY().in(Meters)) - * yTurretVelocity.in(MetersPerSecond)); + ChassisSpeeds robotSpeeds = m_drive.getChassisSpeeds(); - Pose2d newPoseToAimTo = hubPosition.plus(new Transform2d(xOffTarget, yOffTarget, new Rotation2d())); + return getInterpolatedShootingParameters(turretToHubDistance, + MetersPerSecond.of(robotSpeeds.vxMetersPerSecond), MetersPerSecond.of(robotSpeeds.vyMetersPerSecond), + turretToHubAngle); } public Command MoveTurretToHeadingCommand(Angle heading) { @@ -192,4 +196,88 @@ private static Angle getClosestAngle(Angle a, Angle... others) { return closest; } + + private static int getFirstEntryIndex(Distance distance) { + int low = 0; + int high = ShooterConstants.kShootingEntries.length; + int mid = 0; + + while (low < high) { + mid = (low + high) / 2; + ShootingEntry midEntry = ShooterConstants.kShootingEntries[mid]; + + if (distance.gt(midEntry.distance())) { + low = mid + 1; + } else { + high = mid; + } + } + + ShootingEntry closestEntry = ShooterConstants.kShootingEntries[mid]; + + int previousEntryIndex; + + if (distance.lt(closestEntry.distance())) { + if (mid == 0) { + previousEntryIndex = mid; + } else { + previousEntryIndex = mid - 1; + } + } else { + if (mid == ShooterConstants.kShootingEntries.length - 1) { + previousEntryIndex = mid - 1; + } else { + previousEntryIndex = mid; + } + } + + return previousEntryIndex; + } + + private static TargetSolution getInterpolatedShootingParameters(Distance distance, LinearVelocity vx, + LinearVelocity vy, Angle turretAngle) { + + LinearVelocity robotVelocity = MetersPerSecond.of(Math.hypot(vx.in(MetersPerSecond), vy.in(MetersPerSecond))); + + int firstEntryIndex = getFirstEntryIndex(distance); + + ShootingEntry firstEntry = ShooterConstants.kShootingEntries[firstEntryIndex]; + ShootingEntry secondEntry = ShooterConstants.kShootingEntries[firstEntryIndex + 1]; + + Angle phi = Radians.of(0.0); + + if (robotVelocity.gt(ShooterConstants.kMaxStationaryVelocity)) { + Time timeOfFlight = Seconds.of(UtilityFunctions.interpolate(firstEntry.distance().in(Meters), + secondEntry.distance().in(Meters), firstEntry.timeOfFlight().in(Seconds), + secondEntry.timeOfFlight().in(Seconds), distance.in(Meters))); + + LinearVelocity radialVelocityTorwardsHub = MetersPerSecond + .of(vy.in(MetersPerSecond) * Math.sin(turretAngle.in(Radians)) + + vx.in(MetersPerSecond) * Math.cos(turretAngle.in(Radians))); + + LinearVelocity tangentialVelocityFromHub = MetersPerSecond + .of(vx.in(MetersPerSecond) * Math.sin(turretAngle.in(Radians)) + + vy.in(MetersPerSecond) * Math.cos(turretAngle.in(Radians))); + + Distance sideDistance = tangentialVelocityFromHub.times(timeOfFlight); + distance = distance.minus(radialVelocityTorwardsHub.times(timeOfFlight)); + + phi = Radians.of(Math.atan(sideDistance.in(Meters) / distance.in(Meters))); + + int transformedFirstEntryIndex = getFirstEntryIndex(distance); + + firstEntry = ShooterConstants.kShootingEntries[transformedFirstEntryIndex]; + secondEntry = ShooterConstants.kShootingEntries[transformedFirstEntryIndex + 1]; + } + + AngularVelocity wheelSpeed = RadiansPerSecond.of(UtilityFunctions.interpolate(firstEntry.distance().in(Meters), + secondEntry.distance().in(Meters), firstEntry.wheelVelocity().in(RadiansPerSecond), + secondEntry.wheelVelocity().in(RadiansPerSecond), distance.in(Meters))); + + Angle hoodAngle = Radians.of(UtilityFunctions.interpolate(firstEntry.distance().in(Meters), + secondEntry.distance().in(Meters), firstEntry.shooterAngle().in(Radians), + secondEntry.shooterAngle().in(Radians), distance.in(Meters))); + + return new TargetSolution(hoodAngle, wheelSpeed, phi, distance, turretAngle); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 3c1e4ab..9b7c31b 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -32,7 +32,6 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; import frc.robot.Constants.NumericalConstants; -import frc.robot.Constants.TurretConstants; import frc.robot.Constants.DriveConstants.RangeType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; @@ -49,7 +48,6 @@ import java.util.Optional; import java.util.function.Function; -import java.util.function.Supplier; public class DriveSubsystem extends SubsystemBase { @@ -420,10 +418,16 @@ public void addVisionMeasurement(VisionEstimation estimation) { } public ChassisSpeeds getChassisSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds( DriveConstants.kDriveKinematics.toChassisSpeeds(m_frontLeft.getState(), m_frontRight.getState(), m_rearLeft.getState(), m_rearRight.getState()), new Rotation2d(getHeading())); + + } + + public Field2d getField() { + return m_field; } public Fixtures.FieldLocations getRobotLocation() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index c09db21..863f3cb 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -12,7 +12,6 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 1f17134..9c976e4 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -19,8 +19,6 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; -import java.awt.Color; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java index 4a75068..15e291a 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -2,12 +2,9 @@ import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants.NumericalConstants; public class PositionBuffer { @@ -27,14 +24,6 @@ public void pushElement(Angle angle, AngularVelocity velocity, double delay) { } } - private double interpolate(double x1, double x2, double y1, double y2, double x) { - double dsx = x2 - x1; - double dy = y2 - y1; - double dx = x - x1; - - return (dx / dsx > 0.0 ? dsx : NumericalConstants.kEpsilon) * dy + y1; - } - public TurretPosition getAngleAtTime(double requestedTime) { // A fun little binary search algo int high = m_positions.getLength(); @@ -64,7 +53,8 @@ public TurretPosition getAngleAtTime(double requestedTime) { midpoint = low + (high - low) / 2; } - // Linearly interpolate velocity if we aren't on the first/last timestamp + // Linearly UtilityFunctions.interpolate velocity if we aren't on the first/last + // timestamp if (midpoint != 0 && midpoint != m_positions.getLength() - 1) { double dt; @@ -96,11 +86,12 @@ public TurretPosition getAngleAtTime(double requestedTime) { } } - Angle angle = Radians.of(interpolate(firstTurretPosition.timestamp(), secondTurretPosition.timestamp(), + Angle angle = Radians.of(UtilityFunctions.interpolate(firstTurretPosition.timestamp(), + secondTurretPosition.timestamp(), firstTurretPosition.angle().in(Radians), secondTurretPosition.angle().in(Radians), requestedTime)); AngularVelocity velocity = RPM - .of(interpolate(firstTurretPosition.timestamp(), secondTurretPosition.timestamp(), + .of(UtilityFunctions.interpolate(firstTurretPosition.timestamp(), secondTurretPosition.timestamp(), firstTurretPosition.velocity().in(RPM), secondTurretPosition.velocity().in(RPM), requestedTime)); diff --git a/src/main/java/frc/robot/utils/ShootingEntry.java b/src/main/java/frc/robot/utils/ShootingEntry.java new file mode 100644 index 0000000..0017da2 --- /dev/null +++ b/src/main/java/frc/robot/utils/ShootingEntry.java @@ -0,0 +1,13 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; + +public record ShootingEntry(Distance distance, AngularVelocity wheelVelocity, LinearVelocity muzzleVelocity, + Distance maxHeight, + Time timeOfFlight, Angle shooterAngle) { + +} diff --git a/src/main/java/frc/robot/utils/TargetSolution.java b/src/main/java/frc/robot/utils/TargetSolution.java new file mode 100644 index 0000000..64542a6 --- /dev/null +++ b/src/main/java/frc/robot/utils/TargetSolution.java @@ -0,0 +1,10 @@ +package frc.robot.utils; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; + +public record TargetSolution(Angle hoodAngle, AngularVelocity wheelSpeed, Angle phi, Distance distance, + Angle hubAngle) { + +} diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java index 98c77dc..ff91e45 100644 --- a/src/main/java/frc/robot/utils/UtilityFunctions.java +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -7,8 +7,9 @@ public final class UtilityFunctions { public static Angle WrapAngle(Angle angle) { - return Radians.of(((angle.in(Radians) % NumericalConstants.kFullRotation.in(Radians)) + NumericalConstants.kFullRotation.in(Radians)) % NumericalConstants.kFullRotation - .in(Radians)); + return Radians.of(((angle.in(Radians) % NumericalConstants.kFullRotation.in(Radians)) + + NumericalConstants.kFullRotation.in(Radians)) % NumericalConstants.kFullRotation + .in(Radians)); } public static Angle WrapTo180(Angle angle) { @@ -20,4 +21,12 @@ public static Angle WrapTo180(Angle angle) { return wrapped; } + + public static double interpolate(double x1, double x2, double y1, double y2, double x) { + double dsx = x2 - x1; + double dy = y2 - y1; + double dx = x - x1; + + return (dx / Math.abs(dsx) > 0.0 ? dsx : NumericalConstants.kEpsilon) * dy + y1; + } } From 77f8826e3e53f04873a18efcda63140547089763 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Sat, 21 Feb 2026 21:12:16 -0600 Subject: [PATCH 10/25] Get hood going to agle from 0 to 30 degrees. --- src/main/java/frc/robot/Constants.java | 14 ++++--- src/main/java/frc/robot/RobotContainer.java | 14 +++---- .../frc/robot/commands/AimCommandFactory.java | 12 +++++- .../robot/subsystems/ShooterSubsystem.java | 38 ++++++++++++++----- 4 files changed, 55 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bf2186b..1736b39 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -208,7 +208,7 @@ public static final class NumericalConstants { } public static final class TurretConstants { - public static final int kMotorId = 20; + public static final int kMotorId = 999; // Was 20 public static final Angle kMinAngle = Radians.of(0.0); public static final Angle kMaxAngle = Degrees.of(340); @@ -237,9 +237,9 @@ public static final class TurretConstants { public static final class ShooterConstants { public static final int kShooterMotorId = 30; - public static final int kHoodMotorId = 31; + public static final int kHoodMotorId = 20; // Was 31 - public static final double kHoodP = 0.1; + public static final double kHoodP = 5.0; // Only use this high P when converion factor is 1. public static final double kHoodI = 0.0; public static final double kHoodD = 0.0; @@ -248,8 +248,12 @@ public static final class ShooterConstants { 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); - + // NOTE: Need to use 14D so the result is a double, otherwise you end up with zero. + // 2.48 * 0.0642201834862385 = 0.1592660550458716 + // 16.5 motor rotations to one absolute encoder rotation (roughly) + // NOTE: gear ration commented out for now is it isn't used + //public static final double kHoodGearRatio = (62D / 25) * (14D / 218); + public static final int kHoodSmartCurrentLimit = 20; public static final Angle kFeedAngle = Degrees.of(90.0); public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 39c708d..c014b97 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,6 @@ package frc.robot; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Radians; - import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; @@ -13,14 +11,13 @@ 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.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.UtilityFunctions; @@ -33,10 +30,10 @@ public class RobotContainer { private final SendableChooser m_chooser = new SendableChooser<>(); private final TurretSubsystem m_turret = new TurretSubsystem(); - + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); private final DriveSubsystem m_drive = new DriveSubsystem(m_turret::getRotationAtTime); - AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret); + AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret, m_shooter); public RobotContainer() { m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); @@ -61,6 +58,9 @@ public RobotContainer() { private void configureBindings() { m_driverController.a() .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + + System.out.println("Bindings configured"); + m_driverController.x().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(30D))); } public Command getAutonomousCommand() { @@ -73,7 +73,7 @@ public Command getAutonomousCommand() { public Runnable pushTurretEncoderReading() { return () -> { - m_turret.pushCurrentEncoderReading(); + m_turret.pushCurrentEncoderReading(); }; } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index f190a0a..e8e8763 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -17,11 +17,13 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.Constants.Fixtures; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.UtilityFunctions; @@ -29,10 +31,12 @@ public class AimCommandFactory { private DriveSubsystem m_drive; private TurretSubsystem m_turret; + private ShooterSubsystem m_shooter; - public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret) { + public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { m_drive = drive; m_turret = turret; + m_shooter = shooter; } public Command Aim(Supplier isFeedingLeftSide) { @@ -109,6 +113,12 @@ public Command MoveTurretToHeadingCommand(Angle heading) { MoveTurretToHeading(heading); }, m_turret); } + public Command MoveHoodToAbsoluteCommand(Angle angle) { + return new InstantCommand(() -> { + System.out.println("Move Hood to position " + angle); + m_shooter.MoveHoodToPosition(angle); + }); + } public void MoveTurretToHeading(Angle heading) { Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index c09db21..c505333 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -12,7 +12,6 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -45,14 +44,18 @@ public ShooterSubsystem() { .p(ShooterConstants.kHoodP) .i(ShooterConstants.kHoodI) .d(ShooterConstants.kHoodD); + m_hoodConfig.smartCurrentLimit(ShooterConstants.kHoodSmartCurrentLimit); m_hoodConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + m_hoodConfig.absoluteEncoder.inverted(true); + m_hoodConfig.inverted(true); - // Inverse since kHoodGearRatio gives encoder -> hood motion, and we need hood - // motion -> encoder - m_hoodConfig.encoder.positionConversionFactor(1.0 / ShooterConstants.kHoodGearRatio); + // .6 rotations = 30 degrees + // 1 rotation = 50 degrees + m_hoodConfig.absoluteEncoder.positionConversionFactor(1); + m_hoodConfig.encoder.positionConversionFactor(1); - m_shooterMotor.configure(m_shooterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - m_hoodMotor.configure(m_hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_shooterMotor.configure(m_shooterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + m_hoodMotor.configure(m_hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } // Think of this as controlling the turret on a cross-section going from the @@ -87,14 +90,29 @@ public LinearVelocity shootToPose(Distance forward, Distance maxZ, Distance endZ return null; } - Aim(shooterAngle); + MoveHoodToPosition(shooterAngle); return shooterMuzzleVelocity; } - public void Aim(Angle angle) { - m_hoodClosedLoopController.setSetpoint(angle.in(Rotations), - ControlType.kPosition); + // Position between 0 and .55 + public void MoveHoodToPosition(Angle angle) { + System.out.println("Move hood to position: " + angle); + + // get target absolute encoder position. 0 starts in hood min, hood max is .55 (30 degrees of movement) + var targetPosition = angle.magnitude() * (0.55 / 30); + if(targetPosition < 0 || targetPosition > 0.55){ + System.out.println("Hood target position out of bounds. Target: " + targetPosition); + return; + } + var curentPosition = m_absoluteEncoder.getPosition(); + System.out.println("current hood: " + curentPosition); + if( curentPosition > 0.55){ + System.out.println("Hood position icorrect for safe movement. Pos: " + curentPosition); + return; + } + + m_hoodClosedLoopController.setSetpoint(targetPosition, ControlType.kPosition); } public void Spin(AngularVelocity shootSpeedVelocity) { From bb13de586d0dba35dc623677af9f287d7a983e47 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Mon, 23 Feb 2026 17:24:13 -0600 Subject: [PATCH 11/25] changed from magnitude to in degrees --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index c505333..2c76130 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -100,7 +100,7 @@ public void MoveHoodToPosition(Angle angle) { System.out.println("Move hood to position: " + angle); // get target absolute encoder position. 0 starts in hood min, hood max is .55 (30 degrees of movement) - var targetPosition = angle.magnitude() * (0.55 / 30); + var targetPosition = angle.in(Degrees) * (0.55 / 30); if(targetPosition < 0 || targetPosition > 0.55){ System.out.println("Hood target position out of bounds. Target: " + targetPosition); return; From 7accfd1138866071ad80ada845e642ec98f4491f Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Mon, 23 Feb 2026 17:35:59 -0600 Subject: [PATCH 12/25] added costants ffor max and min positions --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 6 +++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1736b39..d07775c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -259,6 +259,9 @@ public static final class ShooterConstants { public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); + + public static final double kHoodMinAbsolutePosition = 0.0; +public static final double kHoodMaxAbsolutePosition = 0.55; } public static final class Fixtures { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2c76130..6e78022 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -100,14 +100,14 @@ public void MoveHoodToPosition(Angle angle) { System.out.println("Move hood to position: " + angle); // get target absolute encoder position. 0 starts in hood min, hood max is .55 (30 degrees of movement) - var targetPosition = angle.in(Degrees) * (0.55 / 30); - if(targetPosition < 0 || targetPosition > 0.55){ + var targetPosition = angle.in(Degrees) * (ShooterConstants.kHoodMaxAbsolutePosition / 30); // TODO: Make seperate constant + if(targetPosition < 0 || targetPosition > ShooterConstants.kHoodMaxAbsolutePosition){ System.out.println("Hood target position out of bounds. Target: " + targetPosition); return; } var curentPosition = m_absoluteEncoder.getPosition(); System.out.println("current hood: " + curentPosition); - if( curentPosition > 0.55){ + if( curentPosition > ShooterConstants.kHoodMaxAbsolutePosition){ System.out.println("Hood position icorrect for safe movement. Pos: " + curentPosition); return; } From 966970ae1e73903d1f505e5e8e6f8a719e565123 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 23 Feb 2026 17:36:06 -0600 Subject: [PATCH 13/25] Start to shuffleboard turret parameters --- src/main/java/frc/robot/Constants.java | 4 ++ src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 44 ++++++++++++++++--- .../frc/robot/commands/AimCommandFactory.java | 9 +--- .../robot/subsystems/ShooterSubsystem.java | 13 ++++++ .../frc/robot/subsystems/TurretSubsystem.java | 5 +++ .../frc/robot/utils/UtilityFunctions.java | 5 +++ 7 files changed, 69 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6ee8e19..69dae03 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -236,6 +236,8 @@ public static final class TurretConstants { public static final Angle kOvershootAmount = Degrees.of(10.0); public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); + + public static final Angle kTurretAngleTolerance = Degrees.of(2.0); } public static final class ShooterConstants { @@ -275,6 +277,8 @@ public static final class ShooterConstants { kFeedAngle), }; + public static final Angle kHoodTolerence = Degrees.of(2.0); + public static final LinearVelocity kMaxStationaryVelocity = MetersPerSecond.of(1e-1); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 246eafd..b0deed2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,6 +23,7 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + m_robotContainer.periodic(); } @Override @@ -63,7 +64,7 @@ public void teleopInit() { @Override public void teleopPeriodic() { - m_robotContainer.periodic(); + m_robotContainer.teleopPeriodic(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dca0230..8686ca0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -12,18 +13,18 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; 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.InstantCommand; 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.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; @@ -46,10 +47,18 @@ public class RobotContainer { AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret); Field2d m_field; + // For getting data points for the lookup table + Angle commandedShooterAngle; + AngularVelocity commandedWheelVelocity; + public RobotContainer() { m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); + SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); + SmartDashboard.putNumber("Turret hood angle in degrees", 0.0); + SmartDashboard.putNumber("Turret angle in degrees", 0.0); + // Configure the button bindings configureBindings(); @@ -69,8 +78,12 @@ public RobotContainer() { } private void configureBindings() { - m_driverController.a() - .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + // m_driverController.a() + // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + + m_driverController.a().whileTrue(new InstantCommand(() -> { + m_turret.moveToAngle(commandedShooterAngle); + }).until(m_turret::isAtTarget).andThen()); } public Command getAutonomousCommand() { @@ -93,7 +106,7 @@ public Command feedPosition(Alliance alliance) { }, m_drive, m_turret); } - public void periodic() { + public void teleopPeriodic() { m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); TargetSolution solution = m_aimFactory.GetHubAimSolution(); @@ -109,4 +122,23 @@ public void periodic() { m_field.getObject("targetPose").setPose(targetPose); } + + public void periodic() { + commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); + commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Turret hood angle in degrees", 0.0)); + + System.out.println(commandedWheelVelocity + ", " + commandedShooterAngle); + } + + private Angle getSmartdashBoardRequestedShooterAngle() { + return Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); + } + + private AngularVelocity getSmartdashboardRequestedWheelSpeed() { + return RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); + } + + private Angle getSmartdashBoardRequestedTurretAngle() { + return Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)); + } } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 1ddee3e..35526ae 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -165,11 +165,6 @@ private static boolean withinRange(Angle min, Angle max, Angle a) { return a.gt(min) && a.lt(max); } - private static Angle angleDiff(Angle a1, Angle a2) { - Angle diff = a1.minus(a2); - return UtilityFunctions.WrapTo180(diff); - } - private static Angle getClosestAngle(Angle a, Angle... others) { a = UtilityFunctions.WrapAngle(a); @@ -180,11 +175,11 @@ private static Angle getClosestAngle(Angle a, Angle... others) { // System.out.println(); Angle closest = UtilityFunctions.WrapAngle(others[0]); - double closestDistance = angleDiff(a, closest).abs(Degrees); + double closestDistance = UtilityFunctions.angleDiff(a, closest).abs(Degrees); for (int i = 1; i < others.length; i++) { Angle candidate = UtilityFunctions.WrapAngle(others[i]); - double dif = angleDiff(a, candidate).abs(Degrees); + double dif = UtilityFunctions.angleDiff(a, candidate).abs(Degrees); if (dif < closestDistance) { closest = candidate; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 863f3cb..9037a86 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.utils.UtilityFunctions; public class ShooterSubsystem extends SubsystemBase { @@ -33,6 +34,8 @@ public class ShooterSubsystem extends SubsystemBase { private final SparkMaxConfig m_shooterConfig = new SparkMaxConfig(); private final SparkMaxConfig m_hoodConfig = new SparkMaxConfig(); + Angle m_targetAngle = Degrees.of(0.0); + public ShooterSubsystem() { m_shooterConfig.closedLoop @@ -92,6 +95,7 @@ public LinearVelocity shootToPose(Distance forward, Distance maxZ, Distance endZ } public void Aim(Angle angle) { + m_targetAngle = angle; m_hoodClosedLoopController.setSetpoint(angle.in(Rotations), ControlType.kPosition); } @@ -104,6 +108,15 @@ public void StopShooting() { Spin(RPM.of(0)); } + public Angle GetHoodAngle() { + return Rotations.of(m_absoluteEncoder.getPosition()); + } + + public boolean AtTarget() { + return UtilityFunctions.angleDiff(GetHoodAngle(), m_targetAngle).abs(Degrees) < ShooterConstants.kHoodTolerence + .in(Degrees); + } + @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 9c976e4..f62a4fe 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -151,4 +151,9 @@ public void simulationPeriodic() { m_max2.setAngle(TurretConstants.kFeedMaxAngle2.plus(robotRotation).in(Degrees)); SmartDashboard.putData("Turret Rotation", m_simMech); } + + public boolean isAtTarget() { + return UtilityFunctions.angleDiff(m_targetAngle, getRotation()) + .abs(Degrees) < TurretConstants.kTurretAngleTolerance.in(Degrees); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java index ff91e45..1e8c7eb 100644 --- a/src/main/java/frc/robot/utils/UtilityFunctions.java +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -29,4 +29,9 @@ public static double interpolate(double x1, double x2, double y1, double y2, dou return (dx / Math.abs(dsx) > 0.0 ? dsx : NumericalConstants.kEpsilon) * dy + y1; } + + public static Angle angleDiff(Angle a1, Angle a2) { + Angle diff = a1.minus(a2); + return WrapTo180(diff); + } } From 7d4acc3ad0a12595699b0afc05ccd7b75145d771 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Mon, 23 Feb 2026 17:45:51 -0600 Subject: [PATCH 14/25] build fix --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ccfcf5b..0055f24 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,8 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radians; + import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; From 2e16e51228d906bbb20827fb2728a67833ac4958 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 23 Feb 2026 18:14:08 -0600 Subject: [PATCH 15/25] Easy elastic adjustments to turret shooting parameters --- src/main/java/frc/robot/RobotContainer.java | 17 ++++++++++------- .../frc/robot/commands/AimCommandFactory.java | 14 ++++++++++++++ 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0055f24..38c44ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,7 +57,7 @@ public RobotContainer() { SmartDashboard.putData("Auto Choices", m_chooser); SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); - SmartDashboard.putNumber("Turret hood angle in degrees", 0.0); + SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); SmartDashboard.putNumber("Turret angle in degrees", 0.0); // Configure the button bindings @@ -82,11 +82,14 @@ private void configureBindings() { // m_driverController.a() // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - m_driverController.a().whileTrue(new InstantCommand(() -> { - m_turret.moveToAngle(commandedShooterAngle); - }).until(m_turret::isAtTarget).andThen()); + m_driverController.b() + .onTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)), + Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); - System.out.println("Bindings configured"); + m_driverController.a().onTrue( + m_aimFactory.Shoot(RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)))); + + System.out.println("Bindings configured"); m_driverController.x().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(30D))); } @@ -100,7 +103,7 @@ public Command getAutonomousCommand() { public Runnable pushTurretEncoderReading() { return () -> { - m_turret.pushCurrentEncoderReading(); + m_turret.pushCurrentEncoderReading(); }; } @@ -129,7 +132,7 @@ public void teleopPeriodic() { public void periodic() { commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); - commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Turret hood angle in degrees", 0.0)); + commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); System.out.println(commandedWheelVelocity + ", " + commandedShooterAngle); } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index cbde4dc..dcf27dc 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -117,6 +117,7 @@ public Command MoveTurretToHeadingCommand(Angle heading) { MoveTurretToHeading(heading); }, m_turret); } + public Command MoveHoodToAbsoluteCommand(Angle angle) { return new InstantCommand(() -> { System.out.println("Move Hood to position " + angle); @@ -285,4 +286,17 @@ private static TargetSolution getInterpolatedShootingParameters(Distance distanc return new TargetSolution(hoodAngle, wheelSpeed, phi, distance, turretAngle); } + + public Command Aim(Angle turretAngle, Angle hoodAngle) { + return new InstantCommand(() -> { + m_turret.moveToAngle(turretAngle); + m_shooter.MoveHoodToPosition(hoodAngle); + }); + } + + public Command Shoot(AngularVelocity shooterWheelVelocity) { + return new InstantCommand(() -> { + m_shooter.Spin(shooterWheelVelocity); + }); + } } \ No newline at end of file From 78b82602f122f15935600654d0bba8c477f14346 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 24 Feb 2026 16:09:08 -0600 Subject: [PATCH 16/25] Aim command updates --- src/main/java/frc/robot/Constants.java | 26 ++++- .../frc/robot/commands/AimCommandFactory.java | 109 +++++++++++------- .../frc/robot/subsystems/DriveSubsystem.java | 12 +- .../robot/subsystems/ShooterSubsystem.java | 13 ++- .../frc/robot/subsystems/TurretSubsystem.java | 2 +- .../java/frc/robot/utils/TargetSolution.java | 1 + 6 files changed, 99 insertions(+), 64 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d84d8ff..9b21d83 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -178,7 +178,10 @@ public static final class VisionConstants { 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), + + // These are not final numbers + public static final Transform3d kRobotToCamTwo = new Transform3d( + new Translation3d(Inches.of(8.375), Inches.of(-2.16), Inches.of(-20.668)), new Rotation3d(0, 0, 0)); public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout @@ -238,6 +241,9 @@ public static final class TurretConstants { public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); public static final Angle kTurretAngleTolerance = Degrees.of(2.0); + + // TODO: Change to real numbers + public static Angle kNonAimTurretAngle = Degrees.of(25.0); } public static final class ShooterConstants { @@ -253,11 +259,12 @@ public static final class ShooterConstants { public static final double kShooterD = 0.0; // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part - // NOTE: Need to use 14D so the result is a double, otherwise you end up with zero. + // NOTE: Need to use 14D so the result is a double, otherwise you end up with + // zero. // 2.48 * 0.0642201834862385 = 0.1592660550458716 // 16.5 motor rotations to one absolute encoder rotation (roughly) // NOTE: gear ration commented out for now is it isn't used - //public static final double kHoodGearRatio = (62D / 25) * (14D / 218); + // public static final double kHoodGearRatio = (62D / 25) * (14D / 218); public static final int kHoodSmartCurrentLimit = 20; public static final Angle kFeedAngle = Degrees.of(90.0); @@ -286,7 +293,15 @@ public static final class ShooterConstants { public static final LinearVelocity kMaxStationaryVelocity = MetersPerSecond.of(1e-1); public static final double kHoodMinAbsolutePosition = 0.0; -public static final double kHoodMaxAbsolutePosition = 0.55; + public static final double kHoodMaxAbsolutePosition = 0.55; + + public static final double kHoodDegreeConversionFactor = kHoodMaxAbsolutePosition / 30; + + // TODO: Change to real numbers + public static AngularVelocity kNonAimShooterVelocity = RPM.of(500); + public static Angle kNonAimHoodAngle = Degrees.of(15.0); + public static AngularVelocity kFeedingWheelVelocity = RPM.of(500); + public static Angle kHoodFeedingPosition = Degrees.of(25.0); } public static final class Fixtures { @@ -306,8 +321,7 @@ public static final class Fixtures { public static enum FieldLocations { AllianceSide, - NeutralLeftSide, - NeutralRightSide, + NeutralSide, OpponentSide } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index dcf27dc..db019f0 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -1,5 +1,6 @@ package frc.robot.commands; +import java.lang.annotation.Target; import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; @@ -23,6 +24,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.Constants.Fixtures; @@ -41,57 +43,82 @@ public class AimCommandFactory { private TurretSubsystem m_turret; private ShooterSubsystem m_shooter; + private boolean m_isAiming = false; + + private AngularVelocity m_wheelVelocity; + public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { m_drive = drive; m_turret = turret; m_shooter = shooter; } - public Command Aim(Supplier isFeedingLeftSide) { + public Command AimCommand(Supplier isFeedingLeftSide) { return new RunCommand(() -> { - Fixtures.FieldLocations location = m_drive.getRobotLocation(); - Pose2d robotPose = m_drive.getPose(); - Angle robotAngle = Radians.of(robotPose.getRotation().getRadians()); - - switch (location) { - case AllianceSide: { - Translation2d fixtureTranslation = DriverStation.getAlliance().get() == Alliance.Blue - ? Fixtures.kBlueAllianceHub - : Fixtures.kRedAllianceHub; - - double dy = fixtureTranslation.getX() - robotPose.getX(); - double dx = fixtureTranslation.getY() - robotPose.getY(); - - Angle absHeading = Radians.of(Math.atan2(dy, dx)); - Angle relHeading = absHeading.minus(robotAngle); - - m_turret.moveToAngle(relHeading); - break; - } - case NeutralLeftSide: { - // Heading changes 180 degrees depending on which alliance you are on - Angle offset = DriverStation.getAlliance().get() == Alliance.Blue ? Degrees.of(0) : Degrees.of(180); - Angle absHeading = isFeedingLeftSide.get() ? offset.minus(Degrees.of(50)) - : offset.plus(Degrees.of(50)); - Angle relHeading = absHeading.minus(robotAngle); - - robotPose = m_drive.getPose(); - break; - } - case NeutralRightSide: { - break; - } - case OpponentSide: { - break; - } - default: - break; - } - }, m_drive, m_turret).until(() -> { - return true; + Aim(isFeedingLeftSide); + m_isAiming = true; + }, m_drive, m_turret).finallyDo(() -> { + m_isAiming = false; + m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; }); } + private void Aim(Supplier isFeedingLeftSide) { + Fixtures.FieldLocations location = m_drive.getRobotLocation(); + + switch (location) { + case AllianceSide: { + TargetSolution solution = GetHubAimSolution(); + + MoveTurretToHeading(solution.hubAngle()); + m_shooter.MoveHoodToPosition(solution.hoodAngle()); + + m_wheelVelocity = solution.wheelSpeed(); + break; + } + case NeutralSide: { + // Heading changes 180 degrees depending on which alliance you are on + Angle offset = DriverStation.getAlliance().get() == Alliance.Blue ? Degrees.of(0) : Degrees.of(180); + Angle absHeading = isFeedingLeftSide.get() ? offset.minus(Degrees.of(50)) + : offset.plus(Degrees.of(50)); + + m_shooter.MoveHoodToPosition(ShooterConstants.kHoodFeedingPosition); + m_wheelVelocity = ShooterConstants.kFeedingWheelVelocity; + MoveTurretToHeading(absHeading); + break; + } + case OpponentSide: { + System.out.println("Why are you here???"); + } + default: + break; + } + } + + public Command AimHoodToPositionCommand(Angle angle) { + return new RunCommand(() -> { + m_shooter.MoveHoodToPosition(angle); + }).until(m_shooter::AtTarget); + } + + public Command AimTurretRelativeToRobot(Angle angle) { + return new RunCommand(() -> { + m_turret.moveToAngle(angle); + }, m_turret).until(m_turret::atTarget); + } + + public Command ShootCommand() { + return new ConditionalCommand(new RunCommand(this::Shoot, m_shooter), + AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) + .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) + .andThen(new RunCommand(this::Shoot, m_shooter)), + () -> m_isAiming); + } + + private void Shoot() { + m_shooter.Spin(m_wheelVelocity); + } + public TargetSolution GetHubAimSolution() { Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 9b7c31b..332a185 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -440,11 +440,7 @@ public Fixtures.FieldLocations getRobotLocation() { 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; - } + return Fixtures.FieldLocations.NeutralSide; } else if (x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { return Fixtures.FieldLocations.AllianceSide; } else { @@ -452,11 +448,7 @@ public Fixtures.FieldLocations getRobotLocation() { } } 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; - } + return Fixtures.FieldLocations.NeutralSide; } else if (x < Fixtures.kRedSideNeutralBorder.in(Meters)) { return Fixtures.FieldLocations.AllianceSide; } else { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index bc5075a..d39d64c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -101,16 +101,17 @@ public LinearVelocity shootToPose(Distance forward, Distance maxZ, Distance endZ // Position between 0 and .55 public void MoveHoodToPosition(Angle angle) { System.out.println("Move hood to position: " + angle); - - // get target absolute encoder position. 0 starts in hood min, hood max is .55 (30 degrees of movement) - var targetPosition = angle.in(Degrees) * (ShooterConstants.kHoodMaxAbsolutePosition / 30); // TODO: Make seperate constant - if(targetPosition < 0 || targetPosition > ShooterConstants.kHoodMaxAbsolutePosition){ + + // get target absolute encoder position. 0 starts in hood min, hood max is .55 + // (30 degrees of movement) + var targetPosition = angle.in(Degrees) * (ShooterConstants.kHoodDegreeConversionFactor); + if (targetPosition < 0 || targetPosition > ShooterConstants.kHoodMaxAbsolutePosition) { System.out.println("Hood target position out of bounds. Target: " + targetPosition); return; } var curentPosition = m_absoluteEncoder.getPosition(); System.out.println("current hood: " + curentPosition); - if( curentPosition > ShooterConstants.kHoodMaxAbsolutePosition){ + if (curentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { System.out.println("Hood position icorrect for safe movement. Pos: " + curentPosition); return; } @@ -127,7 +128,7 @@ public void StopShooting() { } public Angle GetHoodAngle() { - return Rotations.of(m_absoluteEncoder.getPosition()); + return Degrees.of(m_absoluteEncoder.getPosition() / ShooterConstants.kHoodDegreeConversionFactor); } public boolean AtTarget() { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index f62a4fe..920e687 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -152,7 +152,7 @@ public void simulationPeriodic() { SmartDashboard.putData("Turret Rotation", m_simMech); } - public boolean isAtTarget() { + public boolean atTarget() { return UtilityFunctions.angleDiff(m_targetAngle, getRotation()) .abs(Degrees) < TurretConstants.kTurretAngleTolerance.in(Degrees); } diff --git a/src/main/java/frc/robot/utils/TargetSolution.java b/src/main/java/frc/robot/utils/TargetSolution.java index 64542a6..2b6739f 100644 --- a/src/main/java/frc/robot/utils/TargetSolution.java +++ b/src/main/java/frc/robot/utils/TargetSolution.java @@ -4,6 +4,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; +// Phi is how much the aiming heading should be adjusted to properly correct for velocity public record TargetSolution(Angle hoodAngle, AngularVelocity wheelSpeed, Angle phi, Distance distance, Angle hubAngle) { From ded252867b0883ae1a4526857dc088b8af57f984 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 24 Feb 2026 17:01:47 -0600 Subject: [PATCH 17/25] Correct CAN IDs for driving and turret --- src/main/java/frc/robot/Constants.java | 20 ++++++++++---------- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9b21d83..5cfbdd8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -82,16 +82,16 @@ public static final class DriveConstants { public static final double kBackRightChassisAngularOffset = Math.PI / 2; // SPARK MAX CAN IDs Drive Motors - public static final int kFrontLeftDrivingCanId = 2; - public static final int kRearLeftDrivingCanId = 3; + public static final int kFrontLeftDrivingCanId = 10; + public static final int kRearLeftDrivingCanId = 14; public static final int kFrontRightDrivingCanId = 1; - public static final int kRearRightDrivingCanId = 4; + public static final int kRearRightDrivingCanId = 2; // SPARK MAX CAN IDs Turning Motors - public static final int kFrontLeftTurningCanId = 6; - public static final int kRearLeftTurningCanId = 7; - public static final int kFrontRightTurningCanId = 5; - public static final int kRearRightTurningCanId = 8; + public static final int kFrontLeftTurningCanId = 11; + public static final int kRearLeftTurningCanId = 15; + public static final int kFrontRightTurningCanId = 62; + public static final int kRearRightTurningCanId = 3; // Auxiliary Device Can IDs public static final int kPidgeyCanId = 13; @@ -215,7 +215,7 @@ public static final class NumericalConstants { } public static final class TurretConstants { - public static final int kMotorId = 999; // Was 20 + public static final int kMotorId = 18; // Was 20 public static final Angle kMinAngle = Radians.of(0.0); public static final Angle kMaxAngle = Degrees.of(340); @@ -247,8 +247,8 @@ public static final class TurretConstants { } public static final class ShooterConstants { - public static final int kShooterMotorId = 30; - public static final int kHoodMotorId = 20; // Was 31 + public static final int kShooterMotorId = 70; + public static final int kHoodMotorId = 71; // Was 31 public static final double kHoodP = 5.0; // Only use this high P when converion factor is 1. public static final double kHoodI = 0.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 38c44ae..8e0fc8e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,7 @@ public void periodic() { commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); - System.out.println(commandedWheelVelocity + ", " + commandedShooterAngle); + // System.out.println(commandedWheelVelocity + ", " + commandedShooterAngle); } private Angle getSmartdashBoardRequestedShooterAngle() { From fcdb6fb61bc7697c08fe7a5b6c653c750650424b Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 24 Feb 2026 17:25:03 -0600 Subject: [PATCH 18/25] Discretization working (I think, I tested on cart) --- .../frc/robot/subsystems/DriveSubsystem.java | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 332a185..660fc63 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -321,6 +321,12 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ // getHeading()).in(Radians) + ", Current: " // + getHeading().in(Radians)); + final double latestTime = Timer.getFPGATimestamp(); + final double timeElapsed = latestTime - m_latestTime < 0.20 ? latestTime - m_latestTime + : DriveConstants.kPeriodicInterval.in(Seconds); + + m_latestTime = latestTime; + if (Math.abs(rot) > NumericalConstants.kEpsilon) { m_isManualRotate = true; } @@ -337,11 +343,12 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ // System.out.println("Target " + m_targetAutoAngle + ", Current" + // getHeading()); - final var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + final var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(ChassisSpeeds.discretize( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, new Rotation2d(getHeading())) - : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); + : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered), + timeElapsed)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.kMaxSpeed.magnitude()); @@ -349,12 +356,6 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ m_frontRight.setDesiredState(swerveModuleStates[1]); m_rearLeft.setDesiredState(swerveModuleStates[2]); m_rearRight.setDesiredState(swerveModuleStates[3]); - - final double latestTime = Timer.getFPGATimestamp(); - final double timeElapsed = latestTime - m_latestTime < 0.20 ? latestTime - m_latestTime - : DriveConstants.kPeriodicInterval.in(Seconds); - - m_latestTime = latestTime; } public void drive(ChassisSpeeds speeds) { From 8513e30d6cfa24d6bb33cec62f3df060f14c8e02 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 24 Feb 2026 19:13:32 -0600 Subject: [PATCH 19/25] Make angles arrays and fix turret zero --- src/main/java/frc/robot/Constants.java | 23 +++++++++-- src/main/java/frc/robot/RobotContainer.java | 14 ++++--- .../frc/robot/commands/AimCommandFactory.java | 41 ++++++++++++++++--- .../frc/robot/subsystems/TurretSubsystem.java | 8 ++-- 4 files changed, 66 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5cfbdd8..68ce908 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,10 +28,13 @@ 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.Rotations; import static edu.wpi.first.units.Units.Seconds; import java.util.HashMap; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.Measure; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -230,11 +233,22 @@ public static final class TurretConstants { public static final int kSmartCurrentLimit = 40; - public static final Angle kFeedMinAngle1 = Degrees.of(240); - public static final Angle kFeedMaxAngle1 = Degrees.of(300); + public static final Angle kHubMinAngle1 = Rotations.of(0.375); + public static final Angle kHubMaxAngle1 = Rotations.of(0.582); - public static final Angle kFeedMinAngle2 = Degrees.of(60); - public static final Angle kFeedMaxAngle2 = Degrees.of(120); + public static final Angle kHubMinAngle2 = Rotations.of(0.833); + public static final Angle kHubMaxAngle2 = Rotations.of(0.000); + + public static final Angle kFeedMinAngle = Rotations.of(0.375); + public static final Angle kFeedMaxAngle = Rotations.of(0.582); + + public static final Angle[] kRestrictedAngles = new Angle[] { + kFeedMinAngle, kFeedMaxAngle + }; + + public static final Angle[] kUnrestrictedAngles = new Angle[] { + kHubMinAngle1, kHubMaxAngle1, kHubMinAngle2, kHubMaxAngle2 + }; public static final Angle kOvershootAmount = Degrees.of(10.0); @@ -302,6 +316,7 @@ public static final class ShooterConstants { public static Angle kNonAimHoodAngle = Degrees.of(15.0); public static AngularVelocity kFeedingWheelVelocity = RPM.of(500); public static Angle kHoodFeedingPosition = Degrees.of(25.0); + public static Measure kTurretAngleRestrictiveShooterAngle; } public static final class Fixtures { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e0fc8e..c4a1488 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -82,15 +82,17 @@ private void configureBindings() { // m_driverController.a() // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - m_driverController.b() - .onTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)), - Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); + // m_driverController.b() + // .onTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in + // degrees", 0.0)), + // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); - m_driverController.a().onTrue( - m_aimFactory.Shoot(RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)))); + // m_driverController.a().onTrue( + // m_aimFactory.Shoot(RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations + // per second", 0.0)))); System.out.println("Bindings configured"); - m_driverController.x().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(30D))); + m_driverController.x().onTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(30D))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index db019f0..672f7bc 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -157,15 +157,13 @@ public void MoveTurretToHeading(Angle heading) { Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading)); - if (withinRange(TurretConstants.kFeedMinAngle1, TurretConstants.kFeedMaxAngle1, robotRelativeTurretAngle) - || withinRange(TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2, - robotRelativeTurretAngle)) { + Angle[] currentRange = getCurrentTurretRange(); + + if (withinAngles(currentRange, robotRelativeTurretAngle)) { m_turret.moveToAngle(robotRelativeTurretAngle); } else { // Gets which ray the robot is closest to - Angle closest = getClosestAngle(robotRelativeTurretAngle, TurretConstants.kFeedMinAngle1, - TurretConstants.kFeedMaxAngle1, - TurretConstants.kFeedMinAngle2, TurretConstants.kFeedMaxAngle2); + Angle closest = getClosestAngle(robotRelativeTurretAngle, currentRange); // The overshoot is negative if the robot has to move in a negative direction; // same for positive @@ -326,4 +324,35 @@ public Command Shoot(AngularVelocity shooterWheelVelocity) { m_shooter.Spin(shooterWheelVelocity); }); } + + // TODO: Make this better + + // Our valid shooting ranges are going to change based on the shooter hood + // angle. If the hood angle is too low, then shooting the ball would lead to it + // hitting the side of the robot or other balls currently being held in the + // robot + private Angle[] getCurrentTurretRange() { + if (m_shooter.GetHoodAngle().lt(ShooterConstants.kTurretAngleRestrictiveShooterAngle)) { + return TurretConstants.kRestrictedAngles; + } + + return TurretConstants.kUnrestrictedAngles; + } + + // Must be organized where at every even index it contains the minimum and every + // odd index contains the max angle + private boolean withinAngles(Angle[] angles, Angle candidate) { + if (angles.length % 2 != 0) { + return false; + } + + for (int i = 0; i <= angles.length; i += 2) { + Angle min = angles[i]; + Angle max = angles[i + 1]; + if (withinRange(min, max, candidate)) + return true; + } + + return false; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 920e687..6448476 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -145,10 +145,10 @@ public LinearVelocity getMuzzleVelocityAtHoodAngle() { @Override public void simulationPeriodic() { m_simLigament.setAngle(m_targetAngle.plus(robotRotation).in(Degrees)); - m_min1.setAngle(TurretConstants.kFeedMinAngle1.plus(robotRotation).in(Degrees)); - m_max1.setAngle(TurretConstants.kFeedMaxAngle1.plus(robotRotation).in(Degrees)); - m_min2.setAngle(TurretConstants.kFeedMinAngle2.plus(robotRotation).in(Degrees)); - m_max2.setAngle(TurretConstants.kFeedMaxAngle2.plus(robotRotation).in(Degrees)); + m_min1.setAngle(TurretConstants.kHubMinAngle1.plus(robotRotation).in(Degrees)); + m_max1.setAngle(TurretConstants.kHubMaxAngle1.plus(robotRotation).in(Degrees)); + m_min2.setAngle(TurretConstants.kHubMinAngle2.plus(robotRotation).in(Degrees)); + m_max2.setAngle(TurretConstants.kHubMaxAngle2.plus(robotRotation).in(Degrees)); SmartDashboard.putData("Turret Rotation", m_simMech); } From 01d753fc965332b2b36feb1ca467688145ffe9ab Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Tue, 24 Feb 2026 19:30:18 -0600 Subject: [PATCH 20/25] nolan changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/AimCommandFactory.java | 2 +- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 68ce908..514e0d4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -316,7 +316,7 @@ public static final class ShooterConstants { public static Angle kNonAimHoodAngle = Degrees.of(15.0); public static AngularVelocity kFeedingWheelVelocity = RPM.of(500); public static Angle kHoodFeedingPosition = Degrees.of(25.0); - public static Measure kTurretAngleRestrictiveShooterAngle; + public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); } public static final class Fixtures { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 672f7bc..e4e7826 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -346,7 +346,7 @@ private boolean withinAngles(Angle[] angles, Angle candidate) { return false; } - for (int i = 0; i <= angles.length; i += 2) { + for (int i = 0; i < angles.length - 1; i += 2) { Angle min = angles[i]; Angle max = angles[i + 1]; if (withinRange(min, max, candidate)) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 660fc63..af622f3 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -32,6 +32,7 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; import frc.robot.Constants.NumericalConstants; +import frc.robot.Constants.OIConstants; import frc.robot.Constants.DriveConstants.RangeType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; @@ -327,7 +328,7 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ m_latestTime = latestTime; - if (Math.abs(rot) > NumericalConstants.kEpsilon) { + if (Math.abs(rot) > OIConstants.kDriveDeadband) { m_isManualRotate = true; } From 0034c96f85e57ee1aa7524a9fe25b8edf18c88d2 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 24 Feb 2026 20:16:48 -0600 Subject: [PATCH 21/25] Testing with cameras --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/AimCommandFactory.java | 17 ++++++++++++++++- .../frc/robot/subsystems/DriveSubsystem.java | 2 ++ 4 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 514e0d4..508ea95 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -103,7 +103,7 @@ public static final class DriveConstants { public static final Time kPeriodicInterval = Seconds.of(0.02); - public static final double kAutoRotationP = Robot.isReal() ? 0.3 : 3.0; + public static final double kAutoRotationP = Robot.isReal() ? 3.6 : 3.0; public static final double kAutoRotationI = 0.0; public static final double kAutoRotationD = 0.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c4a1488..b0f06dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -92,7 +92,7 @@ private void configureBindings() { // per second", 0.0)))); System.out.println("Bindings configured"); - m_driverController.x().onTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(30D))); + m_driverController.x().onTrue(m_aimFactory.PointAtHub(true)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index e4e7826..3c030cd 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -140,7 +140,7 @@ public TargetSolution GetHubAimSolution() { } public Command MoveTurretToHeadingCommand(Angle heading) { - return new RunCommand(() -> { + return new InstantCommand(() -> { MoveTurretToHeading(heading); }, m_turret); } @@ -152,6 +152,21 @@ public Command MoveHoodToAbsoluteCommand(Angle angle) { }); } + // TODO: DOES NOT WORK!!!1!1 + public Command PointAtHub(boolean isRed) { + return new InstantCommand(() -> { + Translation2d hubPosition = isRed ? Fixtures.kRedAllianceHub : Fixtures.kBlueAllianceHub; + Translation2d robotPose = m_drive.getPose().getTranslation(); + + double dx = hubPosition.getMeasureX().minus(robotPose.getMeasureX()).in(Meters); + double dy = hubPosition.getMeasureY().minus(robotPose.getMeasureY()).in(Meters); + + Angle angle = Radians.of(Math.atan2(dy, dx)); + + MoveTurretToHeading(angle); + }); + } + public void MoveTurretToHeading(Angle heading) { Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index af622f3..190e483 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -177,6 +177,8 @@ public Command moveToAngleCommand(Angle angle) { public void moveToAngle(Angle angle) { m_isManualRotate = false; m_targetAutoAngle = angle; + + System.out.println("Hello"); } public void moveByAngle(Angle angle) { From 29ef55adb4ed93f6f8ca7bec7b1f6bbd95ca69d9 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 26 Feb 2026 18:06:58 -0600 Subject: [PATCH 22/25] Adjustments to field positions --- src/main/java/frc/robot/Constants.java | 29 +++++++++++------- src/main/java/frc/robot/RobotContainer.java | 5 ++-- .../frc/robot/commands/AimCommandFactory.java | 7 +++-- .../frc/robot/subsystems/DriveSubsystem.java | 30 ++++++++++++------- 4 files changed, 46 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 508ea95..cb269b2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,6 +31,7 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; +import java.lang.reflect.Field; import java.util.HashMap; import edu.wpi.first.units.AngleUnit; @@ -320,19 +321,19 @@ public static final class ShooterConstants { } public static final class Fixtures { - public static final Translation2d kRedAllianceHub = new Translation2d(Inches.of(182.11), Inches.of(154.84)); - public static final Translation2d kBlueAllianceHub = new Translation2d(Inches.of(182.11), - Inches.of(651.22 - 182.11)); + public static final Translation2d kBlueAllianceHub = new Translation2d(Inches.of(182.11), Inches.of(154.84)); + public static final Translation2d kRedAllianceHub = new Translation2d(Inches.of(651.22 - 182.11), + Inches.of(158.84)); // From a top down perspective of the field with the red alliance on the left // side public static final Translation2d kTopFeedPose = new Translation2d(); public static final Translation2d kBottomFeedPose = new Translation2d(); - public static final Distance kFieldYMidpoint = Inches.of(317.69 / 2.0); + public static final Distance kFieldYMidpoint = Inches.of(158.84); - public static final Distance kRedSideNeutralBorder = Inches.of(182.11); - public static final Distance kBlueSideNeutralBorder = Inches.of(651.22 - 182.11); + public static final Distance kBlueSideNeutralBorder = Inches.of(182.11); + public static final Distance kRedSideNeutralBorder = Inches.of(651.22 - 182.11); public static enum FieldLocations { AllianceSide, @@ -340,13 +341,21 @@ public static enum FieldLocations { OpponentSide } + public static final HashMap kFieldLocationStringMap = new HashMap<>(); + + static { + kFieldLocationStringMap.put(FieldLocations.AllianceSide, "Alliance Side"); + kFieldLocationStringMap.put(FieldLocations.NeutralSide, "Neutral Side"); + kFieldLocationStringMap.put(FieldLocations.OpponentSide, "Opponent Side"); + } + // Placeholders - public static final Angle kRedLeftSideFeedHeading = Degrees.of(40); - public static final Angle kRedRightSideFeedHeading = Degrees.of(160); + public static final Angle kBlueLeftSideFeedHeading = Degrees.of(40); + public static final Angle kBlueRightSideFeedHeading = Degrees.of(160); // Placeholders - public static final Angle kBlueLeftSideFeedHeading = Degrees.of(-40); - public static final Angle kBlueRightSideFeedHeading = Degrees.of(-160); + public static final Angle kRedLeftSideFeedHeading = Degrees.of(-40); + public static final Angle kRedRightSideFeedHeading = Degrees.of(-160); public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark) .getTagPose(3).get().toPose2d(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b0f06dc..48095b2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,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.commands.AimCommandFactory; import frc.robot.subsystems.DriveSubsystem; @@ -92,7 +93,7 @@ private void configureBindings() { // per second", 0.0)))); System.out.println("Bindings configured"); - m_driverController.x().onTrue(m_aimFactory.PointAtHub(true)); + m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); } public Command getAutonomousCommand() { @@ -136,7 +137,7 @@ public void periodic() { commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); - // System.out.println(commandedWheelVelocity + ", " + commandedShooterAngle); + System.out.println(m_drive.getRobotLocation()); } private Angle getSmartdashBoardRequestedShooterAngle() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 3c030cd..457184d 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -154,7 +154,8 @@ public Command MoveHoodToAbsoluteCommand(Angle angle) { // TODO: DOES NOT WORK!!!1!1 public Command PointAtHub(boolean isRed) { - return new InstantCommand(() -> { + return new RunCommand(() -> { + // TODO: Uncomment this at some point Translation2d hubPosition = isRed ? Fixtures.kRedAllianceHub : Fixtures.kBlueAllianceHub; Translation2d robotPose = m_drive.getPose().getTranslation(); @@ -164,7 +165,8 @@ public Command PointAtHub(boolean isRed) { Angle angle = Radians.of(Math.atan2(dy, dx)); MoveTurretToHeading(angle); - }); + System.out.println(angle); + }).finallyDo(m_drive::disableFaceHeading); } public void MoveTurretToHeading(Angle heading) { @@ -190,6 +192,7 @@ public void MoveTurretToHeading(Angle heading) { Angle driveTarget = heading.minus(closest); + System.out.println(); m_drive.moveToAngle(driveTarget); m_turret.moveToAngle(closest); } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 190e483..f24663e 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -178,11 +178,12 @@ public void moveToAngle(Angle angle) { m_isManualRotate = false; m_targetAutoAngle = angle; - System.out.println("Hello"); + System.out.println("Is Manual Rotate is False in moveToAngle()"); } public void moveByAngle(Angle angle) { m_isManualRotate = false; + System.out.println("Is Manual Rotate is False in moveByAngle()"); m_targetAutoAngle = getHeading().plus(angle); } @@ -195,6 +196,7 @@ public RangeType faceCardinalHeadingRange(Angle minAngle, Angle maxAngle) { return RangeType.Within; } else { m_isManualRotate = false; + System.out.println("Is Manual Rotate is False in faceCardinalHeadingRange"); m_targetAutoAngle = getClosestAngle(minAngle, maxAngle, robotAngle); return m_targetAutoAngle.isEquivalent(minAngle) ? RangeType.CloseMin : RangeType.CloseMax; } @@ -216,13 +218,12 @@ public Command facePose(Pose2d fixture) { m_targetAutoAngle = Radians.of(Math.atan2(yFixtureDist, xFixtureDist)); m_isManualRotate = false; + System.out.println("Is Manual Rotate is False in facePose()"); }); } - public Command disableFaceHeading() { - return new InstantCommand(() -> { - m_isManualRotate = true; - }); + public void disableFaceHeading() { + m_isManualRotate = true; } @Override @@ -260,7 +261,7 @@ public void periodic() { // System.out.println("Current rotation: " + // getPose().getRotation().getRadians()); - m_poseEstimator.update(new Rotation2d(getHeading()), getModulePositions()); + m_poseEstimator.update(new Rotation2d(getGyroHeading()), getModulePositions()); m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); // System.out.println(m_poseEstimator.getEstimatedPosition()); @@ -270,6 +271,8 @@ public void periodic() { SmartDashboard.putData(m_field); m_pidController.periodic(); + + SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); } /** @@ -412,6 +415,12 @@ public void zeroHeading() { * @return the robot's heading in degrees, from -180 to 180 */ public Angle getHeading() { + // return pidgey.getYaw().getValue(); + // TODO: Don't use this code + return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); + } + + public Angle getGyroHeading() { return pidgey.getYaw().getValue(); } @@ -439,21 +448,20 @@ public Fixtures.FieldLocations getRobotLocation() { 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 (x > Fixtures.kBlueSideNeutralBorder.in(Meters) && x < Fixtures.kRedSideNeutralBorder.in(Meters)) { return Fixtures.FieldLocations.NeutralSide; - } else if (x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { + } 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 (x < Fixtures.kRedSideNeutralBorder.in(Meters) && x > Fixtures.kBlueSideNeutralBorder.in(Meters)) { return Fixtures.FieldLocations.NeutralSide; - } else if (x < Fixtures.kRedSideNeutralBorder.in(Meters)) { + } else if (x > Fixtures.kRedSideNeutralBorder.in(Meters)) { return Fixtures.FieldLocations.AllianceSide; } else { return Fixtures.FieldLocations.OpponentSide; From 6aeb022bfa9236fac16d9f04bc5ce6874e1eab80 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 26 Feb 2026 19:03:28 -0600 Subject: [PATCH 23/25] Create staging subsystem --- src/main/java/frc/robot/Constants.java | 11 +++++ src/main/java/frc/robot/RobotContainer.java | 13 +++--- .../frc/robot/commands/AimCommandFactory.java | 13 ++++-- .../robot/subsystems/ShooterSubsystem.java | 42 +------------------ .../robot/subsystems/StagingSubsystem.java | 28 +++++++++++++ .../frc/robot/subsystems/TurretSubsystem.java | 1 - 6 files changed, 55 insertions(+), 53 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/StagingSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cb269b2..daf558e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -320,6 +320,17 @@ public static final class ShooterConstants { public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); } + public static final class StagingConstants { + public static int kFeedIntoHoodMotor = 30; + public static double kFeedIntoHoodSpeed = 0.4; + + public static final int kAgitationMotorId = 31; + public static final double kAgitationSpeed = 0.2; + + public static final int kRollerMotorId = 32; + public static final double kRollerSpeed = 0.3; + } + public static final class Fixtures { public static final Translation2d kBlueAllianceHub = new Translation2d(Inches.of(182.11), Inches.of(154.84)); public static final Translation2d kRedAllianceHub = new Translation2d(Inches.of(651.22 - 182.11), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48095b2..edd68e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,14 +83,11 @@ private void configureBindings() { // m_driverController.a() // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - // m_driverController.b() - // .onTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in - // degrees", 0.0)), - // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); + m_driverController.b() + .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)), + Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); - // m_driverController.a().onTrue( - // m_aimFactory.Shoot(RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations - // per second", 0.0)))); + m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); System.out.println("Bindings configured"); m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); @@ -137,7 +134,7 @@ public void periodic() { commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); - System.out.println(m_drive.getRobotLocation()); + // System.out.println(m_drive.getRobotLocation()); } private Angle getSmartdashBoardRequestedShooterAngle() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 457184d..cc33c5d 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -32,6 +32,7 @@ import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.StagingSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.ShootingEntry; import frc.robot.utils.TargetSolution; @@ -47,6 +48,8 @@ public class AimCommandFactory { private AngularVelocity m_wheelVelocity; + private StagingSubsystem m_stager = new StagingSubsystem(); + public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { m_drive = drive; m_turret = turret; @@ -73,6 +76,8 @@ private void Aim(Supplier isFeedingLeftSide) { MoveTurretToHeading(solution.hubAngle()); m_shooter.MoveHoodToPosition(solution.hoodAngle()); + m_drive.moveByAngle(solution.phi()); + m_wheelVelocity = solution.wheelSpeed(); break; } @@ -112,7 +117,11 @@ public Command ShootCommand() { AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) .andThen(new RunCommand(this::Shoot, m_shooter)), - () -> m_isAiming); + () -> m_isAiming).alongWith(new RunCommand(() -> { + m_stager.Agitate(); + m_stager.Feed(); + m_stager.Roll(); + }, m_stager)).finallyDo(m_shooter::Stop); } private void Shoot() { @@ -152,10 +161,8 @@ public Command MoveHoodToAbsoluteCommand(Angle angle) { }); } - // TODO: DOES NOT WORK!!!1!1 public Command PointAtHub(boolean isRed) { return new RunCommand(() -> { - // TODO: Uncomment this at some point Translation2d hubPosition = isRed ? Fixtures.kRedAllianceHub : Fixtures.kBlueAllianceHub; Translation2d robotPose = m_drive.getPose().getTranslation(); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index d39d64c..6a5ee2d 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -14,10 +14,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.utils.UtilityFunctions; @@ -61,43 +58,6 @@ public ShooterSubsystem() { m_hoodMotor.configure(m_hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - // Think of this as controlling the turret on a cross-section going from the - // turret at the angle it is at and the target. Forward is the distance away - // from the turret while maxZ is the maximum Z height you want the ball to - // have. endZ is the z position you want the ball to hit once forward is - // achieved - public LinearVelocity shootToPose(Distance forward, Distance maxZ, Distance endZ) { - double forwardMeters = forward.in(Meters); - double maxZMeters = maxZ.in(Meters); - double endZMeters = endZ.in(Meters); - double gravityMpsps = NumericalConstants.kGravity.in(MetersPerSecondPerSecond); - - LinearVelocity vy = MetersPerSecond.of(Math - .sqrt((2.0 * Math.pow(gravityMpsps, 2.0) * maxZMeters) - / (2 * gravityMpsps - 1.0))); - - double vyMps = vy.in(MetersPerSecond); - - LinearVelocity vx = MetersPerSecond - .of((-2.0 * vyMps * forwardMeters + Math.sqrt(Math.pow(2.0 * vyMps * forwardMeters, 2.0) + - 8.0 * endZMeters * gravityMpsps * Math.pow(forwardMeters, 2.0))) / (-4.0 * endZMeters)); - - double vxMps = vx.in(MetersPerSecond); - - LinearVelocity shooterMuzzleVelocity = MetersPerSecond.of(Math.pow(vxMps, 2.0) + Math.pow(vyMps, 2.0)); - Angle shooterAngle = Radians.of(Math.acos(vxMps / shooterMuzzleVelocity.in(MetersPerSecond))); - - if (shooterMuzzleVelocity.gt(ShooterConstants.kMaxMuzzleVelocity)) { - System.out.println("Necessary velocity to hit target " + shooterMuzzleVelocity - + " beyond max velocity of " + ShooterConstants.kMaxMuzzleVelocity + "."); - return null; - } - - MoveHoodToPosition(shooterAngle); - - return shooterMuzzleVelocity; - } - // Position between 0 and .55 public void MoveHoodToPosition(Angle angle) { System.out.println("Move hood to position: " + angle); @@ -123,7 +83,7 @@ public void Spin(AngularVelocity shootSpeedVelocity) { m_shooterClosedLoopController.setSetpoint(shootSpeedVelocity.in(RPM), ControlType.kVelocity); } - public void StopShooting() { + public void Stop() { Spin(RPM.of(0)); } diff --git a/src/main/java/frc/robot/subsystems/StagingSubsystem.java b/src/main/java/frc/robot/subsystems/StagingSubsystem.java new file mode 100644 index 0000000..8535536 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/StagingSubsystem.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.StagingConstants; + +public class StagingSubsystem extends SubsystemBase { + SparkMax m_feedIntoHoodMotor = new SparkMax(StagingConstants.kFeedIntoHoodMotor, MotorType.kBrushless); + SparkMax m_agitationMotor = new SparkMax(StagingConstants.kAgitationMotorId, MotorType.kBrushless); + SparkMax m_rollerMotor = new SparkMax(StagingConstants.kRollerMotorId, null); + + public void Agitate() { + m_agitationMotor.set(StagingConstants.kAgitationSpeed); + } + + // NOT ACTUALLY FEEDING FUEL TO OTHER SIDE OF FIELD, this feed refers to feedin + // fuel into the hood + public void Feed() { + m_feedIntoHoodMotor.set(StagingConstants.kFeedIntoHoodSpeed); + } + + // Refers to the roller that rolls balls into the feeder + public void Roll() { + m_rollerMotor.set(StagingConstants.kRollerSpeed); + } +} diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6448476..8f610f0 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -35,7 +35,6 @@ 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 SparkClosedLoopController m_turretClosedLoopController = m_turretMotor.getClosedLoopController(); From 39f17e744e05200a1b6c5985997c8df62f7da865 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 26 Feb 2026 20:15:12 -0600 Subject: [PATCH 24/25] Add idle camera aim command --- src/main/java/frc/robot/Constants.java | 3 + .../frc/robot/commands/AimCommandFactory.java | 76 +++++++++++++++++++ 2 files changed, 79 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index daf558e..06fcb02 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -243,6 +243,9 @@ public static final class TurretConstants { public static final Angle kFeedMinAngle = Rotations.of(0.375); public static final Angle kFeedMaxAngle = Rotations.of(0.582); + public static final Angle kTurretCameraIdleViewMinAngle = Rotations.of(0.375); + public static final Angle kTurretCameraIdleViewMaxAngle = Rotations.of(0.582); + public static final Angle[] kRestrictedAngles = new Angle[] { kFeedMinAngle, kFeedMaxAngle }; diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index cc33c5d..3a8c004 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -1,9 +1,14 @@ package frc.robot.commands; import java.lang.annotation.Target; +import java.lang.reflect.Array; +import java.util.ArrayList; import java.util.function.Supplier; +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.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -26,8 +31,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.DriveSubsystem; @@ -48,6 +56,8 @@ public class AimCommandFactory { private AngularVelocity m_wheelVelocity; + private Translation2d m_lockedTag; + private StagingSubsystem m_stager = new StagingSubsystem(); public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { @@ -218,6 +228,72 @@ public Command PointTurretToFixture(Pose2d fixture) { }, m_turret); } + // Aims the camera at april tags within range + public Command IdleCameraAim() { + + // TODO: Finish + return new ConditionalCommand(new RunCommand(() -> { + Angle absoluteMinAngle = m_drive.getHeading().plus(TurretConstants.kTurretCameraIdleViewMinAngle); + Angle absoluteMaxAngle = m_drive.getHeading().plus(TurretConstants.kTurretCameraIdleViewMaxAngle); + Pose2d robotPose = m_drive.getPose(); + Translation2d robotTranslation = robotPose.getTranslation(); + + Angle toTagAngle = angleFromTranslation(robotTranslation, m_lockedTag); + + if (!withinRange(absoluteMinAngle, absoluteMaxAngle, toTagAngle)) { + double omega = ChassisSpeeds.fromFieldRelativeSpeeds(m_drive.getChassisSpeeds(), + robotPose.getRotation()).omegaRadiansPerSecond; + + ArrayList aprilTagsInView = aprilTagsWithinRange(absoluteMinAngle, absoluteMaxAngle, + robotTranslation); + + if (aprilTagsInView.isEmpty()) + return; + + Angle targetAngle = omega > 0 ? TurretConstants.kTurretCameraIdleViewMinAngle + : TurretConstants.kTurretCameraIdleViewMaxAngle; + + Translation2d tag; + Angle lowestAngle = NumericalConstants.kFullRotation.div(2); + + Angle closestAngle = getClosestAngle(targetAngle, + (Angle[]) aprilTagsInView.stream().map((Translation2d t) -> { + Angle a = angleFromTranslation(robotTranslation, t); + + return a; + }).toArray()); + + m_turret.moveToAngle(closestAngle.minus(robotPose.getRotation().getMeasure())); + // m_lockedTag = tag; + } + }, m_turret), null, () -> m_turret.getCurrentCommand() == null); + } + + private ArrayList aprilTagsWithinRange(Angle min, Angle max, Translation2d referenceTranslation) { + ArrayList anglesInRange = new ArrayList<>(); + + for (int i = 1; i <= 32; i++) { + Translation2d tag = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark).getTagPose(i).get() + .toPose2d().getTranslation(); + + Angle angleToTag = angleFromTranslation(referenceTranslation, + tag); + + if (angleToTag.gt(min) && angleToTag.lt(max)) { + anglesInRange.add(tag); + } + } + + return anglesInRange; + } + + private Angle angleFromTranslation(Translation2d reference, Translation2d target) { + double dx = target.minus(reference).getX(); + double dy = target.minus(reference).getY(); + + return Radians.of(Math.atan2(dy, dx)); + } + private static boolean withinRange(Angle min, Angle max, Angle a) { a = UtilityFunctions.WrapAngle(a); min = UtilityFunctions.WrapAngle(min); From 73c05ef1b42d18a2990cc4a8a2d8c11841e46b0e Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 27 Feb 2026 16:28:47 -0600 Subject: [PATCH 25/25] New idle aim command for moving turret to april tags while unoccupied --- src/main/java/frc/robot/Constants.java | 2 + .../frc/robot/commands/AimCommandFactory.java | 54 ++++++++++++------- 2 files changed, 37 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 06fcb02..db1ddb1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -245,6 +245,8 @@ public static final class TurretConstants { public static final Angle kTurretCameraIdleViewMinAngle = Rotations.of(0.375); public static final Angle kTurretCameraIdleViewMaxAngle = Rotations.of(0.582); + public static final Angle kTurretCameraMidPoint = kTurretCameraIdleViewMinAngle + .plus(kTurretCameraIdleViewMaxAngle).div(2.0); public static final Angle[] kRestrictedAngles = new Angle[] { kFeedMinAngle, kFeedMaxAngle diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 3a8c004..66ef6d3 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -231,40 +231,33 @@ public Command PointTurretToFixture(Pose2d fixture) { // Aims the camera at april tags within range public Command IdleCameraAim() { - // TODO: Finish + // TODO: Finish return new ConditionalCommand(new RunCommand(() -> { Angle absoluteMinAngle = m_drive.getHeading().plus(TurretConstants.kTurretCameraIdleViewMinAngle); Angle absoluteMaxAngle = m_drive.getHeading().plus(TurretConstants.kTurretCameraIdleViewMaxAngle); Pose2d robotPose = m_drive.getPose(); + Angle robotRotation = robotPose.getRotation().getMeasure(); Translation2d robotTranslation = robotPose.getTranslation(); Angle toTagAngle = angleFromTranslation(robotTranslation, m_lockedTag); - if (!withinRange(absoluteMinAngle, absoluteMaxAngle, toTagAngle)) { - double omega = ChassisSpeeds.fromFieldRelativeSpeeds(m_drive.getChassisSpeeds(), - robotPose.getRotation()).omegaRadiansPerSecond; - + if (!withinRange(robotRotation.plus(absoluteMinAngle), robotRotation.plus(absoluteMaxAngle), toTagAngle)) { ArrayList aprilTagsInView = aprilTagsWithinRange(absoluteMinAngle, absoluteMaxAngle, robotTranslation); if (aprilTagsInView.isEmpty()) return; - Angle targetAngle = omega > 0 ? TurretConstants.kTurretCameraIdleViewMinAngle - : TurretConstants.kTurretCameraIdleViewMaxAngle; - - Translation2d tag; - Angle lowestAngle = NumericalConstants.kFullRotation.div(2); + var tags = aprilTagsWithinRange(absoluteMinAngle, absoluteMaxAngle, robotTranslation); + Translation2d closestTag = getClosestAngleApriltag(TurretConstants.kTurretCameraMidPoint, + robotTranslation, + (Translation2d[]) tags.toArray()); - Angle closestAngle = getClosestAngle(targetAngle, - (Angle[]) aprilTagsInView.stream().map((Translation2d t) -> { - Angle a = angleFromTranslation(robotTranslation, t); + Angle angleToTag = angleFromTranslation(robotTranslation, closestTag); + Angle turretRelativeAngleToTag = UtilityFunctions.WrapAngle(angleToTag.minus(robotRotation)); - return a; - }).toArray()); - - m_turret.moveToAngle(closestAngle.minus(robotPose.getRotation().getMeasure())); - // m_lockedTag = tag; + m_lockedTag = closestTag; + m_turret.moveToAngle(turretRelativeAngleToTag); } }, m_turret), null, () -> m_turret.getCurrentCommand() == null); } @@ -287,7 +280,7 @@ private ArrayList aprilTagsWithinRange(Angle min, Angle max, Tran return anglesInRange; } - private Angle angleFromTranslation(Translation2d reference, Translation2d target) { + private static Angle angleFromTranslation(Translation2d reference, Translation2d target) { double dx = target.minus(reference).getX(); double dy = target.minus(reference).getY(); @@ -329,6 +322,29 @@ private static Angle getClosestAngle(Angle a, Angle... others) { return closest; } + private static Translation2d getClosestAngleApriltag(Angle referenceAngle, Translation2d robot, + Translation2d... tags) { + if (tags.length == 0) + return null; + + referenceAngle = UtilityFunctions.WrapAngle(referenceAngle); + + double closestDistance = 2 * Math.PI; + Translation2d closestPosition = new Translation2d(); + + for (Translation2d tag : tags) { + Angle candidate = UtilityFunctions.WrapAngle(angleFromTranslation(robot, tag)); + double dif = UtilityFunctions.angleDiff(referenceAngle, candidate).abs(Degrees); + + if (dif < closestDistance) { + closestDistance = dif; + closestPosition = tag; + } + } + + return closestPosition; + } + private static int getFirstEntryIndex(Distance distance) { int low = 0; int high = ShooterConstants.kShootingEntries.length;