From 17d02292b43baed99e2bc2b9df116444503e6fca Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 12 Mar 2026 19:07:05 -0400 Subject: [PATCH 01/13] changes --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/autochooser/AutoChooser.java | 8 +-- .../frc/robot/commands/auto/DoNothing.java | 21 +++++--- .../calculations/TurretCalculationsTest.java | 53 ------------------- 4 files changed, 19 insertions(+), 65 deletions(-) delete mode 100644 src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8463dcf2..256e7f6f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -307,7 +307,7 @@ private void configureBindings() { controller.y().onTrue(new ClimberUp(climberSubsystem)); controller.x().onTrue(new ClimberDown(climberSubsystem)); controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); - controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); + controller.povRight().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index e5261981..9ce8c70b 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; +import frc.robot.commands.auto.DoNothing; import frc.robot.commands.auto.shoot.BlueDepotShoot; import frc.robot.commands.auto.shoot.BlueMidShoot; import frc.robot.commands.auto.shoot.BlueOutpostShoot; @@ -30,7 +31,6 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import frc.robot.utils.logging.commands.DoNothingCommand; public class AutoChooser { @@ -106,11 +106,11 @@ private void populateChoosers() { private void populateCommandMap() { //if AutoEvent is not dependent on alliance color don't put a color commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.DEPOT_SIDE), - new DoNothingCommand()); + new DoNothing(turret, angler)); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MID), - new DoNothingCommand()); + new DoNothing(turret, angler)); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.OUTPOST_SIDE), - new DoNothingCommand()); + new DoNothing(turret, angler)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.DEPOT_SIDE, Alliance.Blue), new BlueDepotShoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller)); //shoot depot blue diff --git a/src/main/java/frc/robot/commands/auto/DoNothing.java b/src/main/java/frc/robot/commands/auto/DoNothing.java index 1bce9784..b9c31d05 100644 --- a/src/main/java/frc/robot/commands/auto/DoNothing.java +++ b/src/main/java/frc/robot/commands/auto/DoNothing.java @@ -1,13 +1,20 @@ package frc.robot.commands.auto; -import frc.robot.utils.logging.commands.LoggableCommand; +import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.turret.RunTurretToRevLimit; +import frc.robot.subsystems.AnglerSubsystem; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class DoNothing extends LoggableCommand{ - public DoNothing() { - } +public class DoNothing extends LoggableSequentialCommandGroup{ + public DoNothing(TurretSubsystem turret, AnglerSubsystem angler) { + super( + new LoggableParallelCommandGroup( + new RunTurretToRevLimit(turret), + new RunAnglerToReverseLimit(angler) + ) - @Override - public boolean isFinished() { - return true; + ); } } \ No newline at end of file diff --git a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java b/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java deleted file mode 100644 index 276eab71..00000000 --- a/src/test/java/frc/robot/utils/calculations/TurretCalculationsTest.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.robot.utils.calculations; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; -import frc.robot.utils.math.TurretCalculations; - -public class TurretCalculationsTest { - - static final double DELTA = .0348; // standard deviation of 2 degrees or .0348 radians - double[] xPos = {5.827, 13.850, 9.211, 1.211, 3.811}; - double[] yPos = {2.359, 7.921, 5.386, 0.857, 4.035}; - double[] robotRotation = {toRadians(30), toRadians(-53), toRadians(180), toRadians(45), toRadians(-45)}; - double[] panAngles = {toRadians(-18), toRadians(-102), toRadians(-217), toRadians(-2), toRadians(-1)}; - boolean[] isBlueAlliance = {false, true, false, true, true}; - - private int index; - - private double toRadians(double degree) { - return degree * Math.PI / 180; - } - - @Test - void index0Test() { - index = 0; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index1Test() { - index = 1; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index2Test() { - index = 2; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index3Test() { - index = 3; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } - - @Test - void index4Test() { - index = 4; - assertEquals(TurretCalculations.calculateTurretAngle(xPos[index], yPos[index], robotRotation[index], isBlueAlliance[index]), panAngles[index], DELTA); - } -} \ No newline at end of file From e7607de165d804e8dae26f16c5dca75861a37666 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 14 Mar 2026 13:18:05 -0400 Subject: [PATCH 02/13] added pose adjustment --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 +- .../frc/robot/constants/GameConstants.java | 2 + .../robot/subsystems/ControllerSubsystem.java | 108 +++++++++++------- 4 files changed, 76 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 60f86cb1..190f46bb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -133,7 +133,7 @@ public void robotPeriodic() { SmartDashboard.putNumber("driverXbox.getLeftY()", driverXbox.getLeftY()); SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if (!Constants.TESTBED) { - Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); + Logger.recordOutput("MyPose", robotContainer.getDriveBase().getOdom()); SmartDashboard.putNumber("Robot X", robotContainer.getDriveBase().getPose().getX()); SmartDashboard.putNumber("Robot Y", robotContainer.getDriveBase().getPose().getY()); // Puts data on the elastic dashboard diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 256e7f6f..9968017d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -167,7 +167,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this, driveJoystick) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case REPLAY -> { @@ -186,7 +186,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this, driveJoystick) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } @@ -207,7 +207,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this, driveJoystick) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); @@ -337,7 +337,7 @@ private void configureBindings() { if (controllerSubsystem != null) { anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); - turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); + //turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem)); feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem)); } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 13636392..cffc0ccd 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -170,4 +170,6 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; + + public static final double PREDICTION_TIME = 0.05; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 8574fe51..56159ff6 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,18 +1,25 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.utils.math.TurretCalculations; +import swervelib.math.SwerveMath; + +import java.util.ArrayList; + import org.dyn4j.UnitConversion; import org.littletonrobotics.junction.Logger; import frc.robot.RobotContainer; 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.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -20,10 +27,14 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; + private CommandJoystick driverJoystick; + private ArrayList inputLog; // Placeholder target poses until real field target values are finalized - private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero); - private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, Constants.RED_HUB_Y_POSITION, Rotation2d.kZero); + private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, + Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero); + private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, + Constants.RED_HUB_Y_POSITION, Rotation2d.kZero); private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero); private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX"; @@ -39,20 +50,18 @@ public class ControllerSubsystem extends SubsystemBase { private static final String TARGET_HOPPER_SPEED_KEY = "controller/TargetHopperSpeed"; // Placeholder fixed-state settings. - private static final ShotTargets STOPPED_TARGETS = - new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, false); - private static final ShotTargets FIXED_TARGETS = - new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true); - private static final ShotTargets FIXED_2_TARGETS = - new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); + private static final ShotTargets STOPPED_TARGETS = new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, + false); + private static final ShotTargets FIXED_TARGETS = new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true); + private static final ShotTargets FIXED_2_TARGETS = new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); // Placeholder pose-driven profiles. - private static final PoseControlProfile BLUE_HUB_PROFILE = - new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0, 14.0); - private static final PoseControlProfile RED_HUB_PROFILE = - new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0, 14.0); - private static final PoseControlProfile SHUTTLE_PROFILE = - new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, -14.0); + private static final PoseControlProfile BLUE_HUB_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0, + 14.0); + private static final PoseControlProfile RED_HUB_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0, + 14.0); + private static final PoseControlProfile SHUTTLE_PROFILE = new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, + -14.0); private final SwerveSubsystem drivebase; private final RobotContainer robotContainer; @@ -62,12 +71,13 @@ public class ControllerSubsystem extends SubsystemBase { private ShotTargets activeTargets; private boolean driverActivatedShooting = false; - - public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) { + public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer, + CommandJoystick driverJoystick) { this.drivebase = drivebase; this.robotContainer = robotContainer; this.previousState = getCurrentShootState(); this.activeTargets = STOPPED_TARGETS; + this.driverJoystick = driverJoystick; SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); @@ -81,7 +91,7 @@ public void periodic() { updateStopDelayState(currentState); updateTargets(currentState, robotPose); - if(Constants.DEBUG){ + if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, activeTargets.anglerAngleDegrees); @@ -115,7 +125,8 @@ private Pose2d getRobotPose() { } private boolean shouldUseManualPose() { - // This can be confusing in case you're on the robot and have disabled the drivetrain + // This can be confusing in case you're on the robot and have disabled the + // drivetrain // Uncomment to control manually return false; } @@ -124,7 +135,7 @@ private Pose2d getManualPose() { double x = SmartDashboard.getNumber(MANUAL_POSE_X_KEY, 0.0); double y = SmartDashboard.getNumber(MANUAL_POSE_Y_KEY, 0.0); double r = SmartDashboard.getNumber(MANUAL_POSE_R_KEY, 0.0); - Logger.recordOutput("Manual Pose", new Pose2d(new Translation2d(x,y), new Rotation2d(r))); + Logger.recordOutput("Manual Pose", new Pose2d(new Translation2d(x, y), new Rotation2d(r))); return new Pose2d(x, y, new Rotation2d(r)); } @@ -158,7 +169,7 @@ private void updateTargets(ShootState state, Pose2d robotPose) { } private void updateStoppedTargets() { - //This makes the shooter wait half a second before stopping + // This makes the shooter wait half a second before stopping double shooterVelocity = stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS) ? 0.0 : activeTargets.shooterVelocityRpm; @@ -188,40 +199,59 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); - return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, + true); } private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { - return robotPose.getTranslation().getDistance(targetPose.getTranslation()); + return robotPosePredicitionCalculation(robotPose).getTranslation().getDistance(targetPose.getTranslation()); } + private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { + Translation2d cubedTranslation = SwerveMath + .cubeTranslation(new Translation2d(driverJoystick.getY() * (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)? -1 : 1), driverJoystick.getX() * (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)? -1 : 1))); + Transform2d speedTransform = new Transform2d( + cubedTranslation.getX() * UnitConversion.metersToFeet(Constants.MAX_SPEED) * Constants.PREDICTION_TIME, + cubedTranslation.getY() * UnitConversion.metersToFeet(Constants.MAX_SPEED) * Constants.PREDICTION_TIME, + new Rotation2d()); + Pose2d robotTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); + Pose2d predictedTransform = robotTransform.transformBy(speedTransform); + Pose2d predictedPose = new Pose2d(predictedTransform.getTranslation(), robotPose.getRotation()); + Logger.recordOutput("Predicted pose", predictedPose); + return predictedPose; + } + private void inputLogger(){ + Double[] inputs = {driverJoystick.getX(), driverJoystick.getY()}; + inputLog.add(inputs); + } private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; - return - 0.169 * distance * distance - - 1.73 * distance - + 20.4; - } + double distance = (3.281 * computedDistanceMeters) - 2; + SmartDashboard.putNumber("Calculated distance feet", distance); + return 0.169 * distance * distance + - 1.73 * distance + + 20.4; + } return profile.defaultAnglerAngleDegrees; } private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; - return Math.floor( - 8.46 * distance * distance - - 237 * distance - - 1_380); - } + double distance = (3.281 * computedDistanceMeters) - 2; + return 8.46 * distance * distance + - 237 * distance + - 1_380; + } return profile.defaultShooterVelocityRpm; } private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { - return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), DriverStation.getAlliance().get()== DriverStation.Alliance.Blue))); + return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), + robotPose.getRotation().getRadians(), + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue))); } - //Getters for all the subsystems to set posistion. + // Getters for all the subsystems to set posistion. public double getTargetAnglerAngleDegrees() { return activeTargets.anglerAngleDegrees; } @@ -254,8 +284,7 @@ public boolean driverActivatedShootingEnabled() { return driverActivatedShooting; } - - //Class to save all the fixed targets + // Class to save all the fixed targets private static final class ShotTargets { private final double anglerAngleDegrees; private final double shooterVelocityRpm; @@ -280,7 +309,8 @@ private ShotTargets( } } - //Class to save all the target poses and each subsystem position at that point so we can calculate true values later on + // Class to save all the target poses and each subsystem position at that point + // so we can calculate true values later on private static final class PoseControlProfile { private final Pose2d targetPose; private final double defaultAnglerAngleDegrees; From bb1c1a10be42191cddd95aeb98e2b493e8d8f6da Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sat, 14 Mar 2026 14:11:12 -0400 Subject: [PATCH 03/13] Changed constant --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9968017d..996247b6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -346,7 +346,7 @@ private void configureBindings() { SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> driveJoystick.getY() * -1, () -> driveJoystick.getX() * -1) - .withControllerRotationAxis(steerJoystick::getX) + .withControllerRotationAxis(() -> {steerJoystick.getX() * -1;}) .deadband(Constants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index cffc0ccd..ca7691e0 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -171,5 +171,5 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; - public static final double PREDICTION_TIME = 0.05; + public static final double PREDICTION_TIME = 0.5; } From a22167604dcc3e594db63121c4ae79dad43c531a Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 15 Mar 2026 10:15:01 -0400 Subject: [PATCH 04/13] Commit --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/constants/Constants.java | 2 ++ .../frc/robot/constants/GameConstants.java | 2 +- .../robot/subsystems/ControllerSubsystem.java | 28 ++++++++++++------- 4 files changed, 22 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 996247b6..57bad0db 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -346,7 +346,7 @@ private void configureBindings() { SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> driveJoystick.getY() * -1, () -> driveJoystick.getX() * -1) - .withControllerRotationAxis(() -> {steerJoystick.getX() * -1;}) + .withControllerRotationAxis(() -> {return steerJoystick.getX() * -1;}) .deadband(Constants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index e93318d5..2bcab5e5 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -18,6 +18,8 @@ // To use the other constatns files, change which line is not commented out here. // Then discard changes to this file before committing. public class Constants extends ConstantsReal2026 { + + public static final int DISTANCE_CHANGE_THRESHOLD = 2; // public class Constants extends ConstantsPushbot2026 { // public class Constants extends ConstantsTestbed2026 { } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index ca7691e0..f3c75d24 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -171,5 +171,5 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; - public static final double PREDICTION_TIME = 0.5; + public static final double PREDICTION_TIME = 2.25; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 56159ff6..4fbf857d 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -28,7 +28,8 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; private CommandJoystick driverJoystick; - private ArrayList inputLog; + private Pose2d predictedPose; + private ArrayList lastPoses = new ArrayList<>(); // Placeholder target poses until real field target values are finalized private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, @@ -88,7 +89,6 @@ public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContai public void periodic() { Pose2d robotPose = getRobotPose(); ShootState currentState = getCurrentShootState(); - updateStopDelayState(currentState); updateTargets(currentState, robotPose); if (Constants.DEBUG) { @@ -204,10 +204,14 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d } private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { - return robotPosePredicitionCalculation(robotPose).getTranslation().getDistance(targetPose.getTranslation()); + double distance = robotPosePredicitionCalculation(robotPose).getTranslation().getDistance(targetPose.getTranslation()); + return 3.281 * distance > 16 ? 6 : 3.281 * distance < 4.66 ? 1.42 : distance; } private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { + if(lastPoses.size() > 2){ + lastPoses.remove(0); + } Translation2d cubedTranslation = SwerveMath .cubeTranslation(new Translation2d(driverJoystick.getY() * (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)? -1 : 1), driverJoystick.getX() * (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)? -1 : 1))); Transform2d speedTransform = new Transform2d( @@ -218,16 +222,19 @@ private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { Pose2d predictedTransform = robotTransform.transformBy(speedTransform); Pose2d predictedPose = new Pose2d(predictedTransform.getTranslation(), robotPose.getRotation()); Logger.recordOutput("Predicted pose", predictedPose); + lastPoses.add(predictedPose); return predictedPose; } - private void inputLogger(){ - Double[] inputs = {driverJoystick.getX(), driverJoystick.getY()}; - inputLog.add(inputs); + private double distanceBetweenPreviousPoses(ArrayList storePoses){ + + if(storePoses.size() > 2){ + return storePoses.get(1).getTranslation().getDistance(storePoses.get(0).getTranslation()); + } + return 0; } private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { double distance = (3.281 * computedDistanceMeters) - 2; - SmartDashboard.putNumber("Calculated distance feet", distance); return 0.169 * distance * distance - 1.73 * distance + 20.4; @@ -237,10 +244,11 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distance = (3.281 * computedDistanceMeters) - 2; - return 8.46 * distance * distance + double distanceBetweenLastPoses = distanceBetweenPreviousPoses(lastPoses) * 100; + double distance = (3.81 * computedDistanceMeters) - 2; + return (8.46 * distance * distance - 237 * distance - - 1_380; + - 1_380) - (500 * distanceBetweenLastPoses > 0 ? distanceBetweenLastPoses : 0); } return profile.defaultShooterVelocityRpm; } From a4e7e0ce683d36313dd602c85d74687756204048 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 15 Mar 2026 10:22:12 -0400 Subject: [PATCH 05/13] Cleaned up code --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/constants/Constants.java | 2 -- src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 2 -- 3 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 190f46bb..60f86cb1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -133,7 +133,7 @@ public void robotPeriodic() { SmartDashboard.putNumber("driverXbox.getLeftY()", driverXbox.getLeftY()); SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if (!Constants.TESTBED) { - Logger.recordOutput("MyPose", robotContainer.getDriveBase().getOdom()); + Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); SmartDashboard.putNumber("Robot X", robotContainer.getDriveBase().getPose().getX()); SmartDashboard.putNumber("Robot Y", robotContainer.getDriveBase().getPose().getY()); // Puts data on the elastic dashboard diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 2bcab5e5..e93318d5 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -18,8 +18,6 @@ // To use the other constatns files, change which line is not commented out here. // Then discard changes to this file before committing. public class Constants extends ConstantsReal2026 { - - public static final int DISTANCE_CHANGE_THRESHOLD = 2; // public class Constants extends ConstantsPushbot2026 { // public class Constants extends ConstantsTestbed2026 { } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 4fbf857d..0d70a7d3 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.utils.math.TurretCalculations; @@ -28,7 +27,6 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; private CommandJoystick driverJoystick; - private Pose2d predictedPose; private ArrayList lastPoses = new ArrayList<>(); // Placeholder target poses until real field target values are finalized From 7b173df4bbfee18c7e7a002dd1d54a8a57a98321 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 15 Mar 2026 11:26:26 -0400 Subject: [PATCH 06/13] add constants for clamping the max distance --- src/main/java/frc/robot/constants/GameConstants.java | 2 ++ .../java/frc/robot/subsystems/ControllerSubsystem.java | 8 +++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index f3c75d24..be0367d7 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -172,4 +172,6 @@ public enum Mode { public static final double MAX_VISION_DISTANCE_SIMULATION = 6; public static final double PREDICTION_TIME = 2.25; + public static final double MAX_HUB_DISTANCE = 5.18; + public static final double MIN_HUB_DISTANCE = 1.42; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 0d70a7d3..1cdf044d 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -203,7 +203,13 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { double distance = robotPosePredicitionCalculation(robotPose).getTranslation().getDistance(targetPose.getTranslation()); - return 3.281 * distance > 16 ? 6 : 3.281 * distance < 4.66 ? 1.42 : distance; + if(distance > Constants.MAX_HUB_DISTANCE){ + return Constants.MAX_HUB_DISTANCE; + }else if(distance < Constants.MIN_HUB_DISTANCE){ + return Constants.MIN_HUB_DISTANCE; + }else{ + return distance; + } } private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { From 7b3fb63df8a2a546f619bdf475f8106c7615f94e Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Sun, 15 Mar 2026 11:55:08 -0400 Subject: [PATCH 07/13] Added it to use velocity instead of joysticks --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../frc/robot/subsystems/ControllerSubsystem.java | 15 ++------------- 2 files changed, 5 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 57bad0db..8e06118a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -167,7 +167,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this, driveJoystick) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case REPLAY -> { @@ -186,7 +186,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this, driveJoystick) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } @@ -207,7 +207,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this, driveJoystick) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 1cdf044d..3000ba24 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.utils.math.TurretCalculations; -import swervelib.math.SwerveMath; import java.util.ArrayList; @@ -18,7 +17,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.constants.Constants; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -26,7 +24,6 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; - private CommandJoystick driverJoystick; private ArrayList lastPoses = new ArrayList<>(); // Placeholder target poses until real field target values are finalized @@ -70,13 +67,11 @@ public class ControllerSubsystem extends SubsystemBase { private ShotTargets activeTargets; private boolean driverActivatedShooting = false; - public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer, - CommandJoystick driverJoystick) { + public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) { this.drivebase = drivebase; this.robotContainer = robotContainer; this.previousState = getCurrentShootState(); this.activeTargets = STOPPED_TARGETS; - this.driverJoystick = driverJoystick; SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); @@ -216,14 +211,8 @@ private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { if(lastPoses.size() > 2){ lastPoses.remove(0); } - Translation2d cubedTranslation = SwerveMath - .cubeTranslation(new Translation2d(driverJoystick.getY() * (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)? -1 : 1), driverJoystick.getX() * (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)? -1 : 1))); - Transform2d speedTransform = new Transform2d( - cubedTranslation.getX() * UnitConversion.metersToFeet(Constants.MAX_SPEED) * Constants.PREDICTION_TIME, - cubedTranslation.getY() * UnitConversion.metersToFeet(Constants.MAX_SPEED) * Constants.PREDICTION_TIME, - new Rotation2d()); Pose2d robotTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); - Pose2d predictedTransform = robotTransform.transformBy(speedTransform); + Pose2d predictedTransform = robotTransform.transformBy(new Transform2d(new Translation2d(drivebase.getFieldVelocity().vxMetersPerSecond * Constants.PREDICTION_TIME ,drivebase.getFieldVelocity().vyMetersPerSecond * Constants.PREDICTION_TIME), new Rotation2d())); Pose2d predictedPose = new Pose2d(predictedTransform.getTranslation(), robotPose.getRotation()); Logger.recordOutput("Predicted pose", predictedPose); lastPoses.add(predictedPose); From a6f6771751122bc5ee41a667af3244e1b2d674ff Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 17 Mar 2026 19:10:50 -0400 Subject: [PATCH 08/13] commit --- .../robot/subsystems/ControllerSubsystem.java | 18 +++--------------- 1 file changed, 3 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 3000ba24..52934937 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.utils.math.TurretCalculations; @@ -208,23 +209,11 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { } private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { - if(lastPoses.size() > 2){ - lastPoses.remove(0); - } - Pose2d robotTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); - Pose2d predictedTransform = robotTransform.transformBy(new Transform2d(new Translation2d(drivebase.getFieldVelocity().vxMetersPerSecond * Constants.PREDICTION_TIME ,drivebase.getFieldVelocity().vyMetersPerSecond * Constants.PREDICTION_TIME), new Rotation2d())); - Pose2d predictedPose = new Pose2d(predictedTransform.getTranslation(), robotPose.getRotation()); + Pose2d predictedPose = robotPose.exp(drivebase.getFieldVelocity().toTwist2d(Constants.PREDICTION_TIME)); Logger.recordOutput("Predicted pose", predictedPose); lastPoses.add(predictedPose); return predictedPose; } - private double distanceBetweenPreviousPoses(ArrayList storePoses){ - - if(storePoses.size() > 2){ - return storePoses.get(1).getTranslation().getDistance(storePoses.get(0).getTranslation()); - } - return 0; - } private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { double distance = (3.281 * computedDistanceMeters) - 2; @@ -237,11 +226,10 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { - double distanceBetweenLastPoses = distanceBetweenPreviousPoses(lastPoses) * 100; double distance = (3.81 * computedDistanceMeters) - 2; return (8.46 * distance * distance - 237 * distance - - 1_380) - (500 * distanceBetweenLastPoses > 0 ? distanceBetweenLastPoses : 0); + - 1_380); } return profile.defaultShooterVelocityRpm; } From c75b5edad688572751dc98ba900a901615787cec Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Wed, 18 Mar 2026 18:36:24 -0400 Subject: [PATCH 09/13] commit --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/subsystems/ControllerSubsystem.java | 29 ++++++++++++------- .../robot/utils/math/TurretCalculations.java | 7 +++++ 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e06118a..5c824254 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -337,7 +337,7 @@ private void configureBindings() { if (controllerSubsystem != null) { anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); - //turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); + turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem)); feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem)); } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 52934937..57033c9d 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -25,7 +25,6 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; - private ArrayList lastPoses = new ArrayList<>(); // Placeholder target poses until real field target values are finalized private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, @@ -198,22 +197,28 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d } private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { - double distance = robotPosePredicitionCalculation(robotPose).getTranslation().getDistance(targetPose.getTranslation()); - if(distance > Constants.MAX_HUB_DISTANCE){ - return Constants.MAX_HUB_DISTANCE; - }else if(distance < Constants.MIN_HUB_DISTANCE){ + double distance = robotPosePredicitionCalculation(robotPose).getTranslation() + .getDistance(targetPose.getTranslation()); + if (distance > Constants.MAX_HUB_DISTANCE) { + return Constants.MAX_HUB_DISTANCE; + } else if (distance < Constants.MIN_HUB_DISTANCE) { return Constants.MIN_HUB_DISTANCE; - }else{ + } else { return distance; } } private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { - Pose2d predictedPose = robotPose.exp(drivebase.getFieldVelocity().toTwist2d(Constants.PREDICTION_TIME)); + Pose2d robotPoseTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); + Pose2d predictedPose = robotPoseTransform + .plus(new Transform2d( + drivebase.getFieldVelocity().vxMetersPerSecond * Constants.PREDICTION_TIME, + drivebase.getFieldVelocity().vyMetersPerSecond * Constants.PREDICTION_TIME, + new Rotation2d())); Logger.recordOutput("Predicted pose", predictedPose); - lastPoses.add(predictedPose); return predictedPose; } + private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { double distance = (3.281 * computedDistanceMeters) - 2; @@ -235,9 +240,11 @@ private double calculateShooterVelocity(double computedDistanceMeters, PoseContr } private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { - return Math.floor(Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), - robotPose.getRotation().getRadians(), - DriverStation.getAlliance().get() == DriverStation.Alliance.Blue))); + Pose2d predictedPose = robotPosePredicitionCalculation(robotPose); + return Math.floor( + Math.toDegrees(TurretCalculations.calculateTurretAngle(predictedPose.getX(), predictedPose.getY(), + robotPose.getRotation().getRadians(), + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue))); } // Getters for all the subsystems to set posistion. diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index 7e88c7b0..25ab9482 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -1,5 +1,11 @@ package frc.robot.utils.math; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import frc.robot.constants.GameConstants; public class TurretCalculations { @@ -72,6 +78,7 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do // normalize angle between -PI and PI double normalizedPanAngle = panAngle - 2 * Math.PI * Math.floor((panAngle + Math.PI) / (2 * Math.PI)); + Logger.recordOutput("TurretPose", new Pose2d(new Translation2d(turretPosX,turretPosY), new Rotation2d(normalizedPanAngle))); return normalizedPanAngle; } From 28ac743b728bbbec327d575cbbee276242da0d0d Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Wed, 18 Mar 2026 20:01:58 -0400 Subject: [PATCH 10/13] Fixed --- .../java/frc/robot/subsystems/ControllerSubsystem.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 57033c9d..7d806d6e 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -83,7 +83,7 @@ public void periodic() { Pose2d robotPose = getRobotPose(); ShootState currentState = getCurrentShootState(); updateStopDelayState(currentState); - updateTargets(currentState, robotPose); + updateTargets(currentState, robotPosePredicitionCalculation(robotPose)); if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); @@ -197,7 +197,7 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d } private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { - double distance = robotPosePredicitionCalculation(robotPose).getTranslation() + double distance = robotPose.getTranslation() .getDistance(targetPose.getTranslation()); if (distance > Constants.MAX_HUB_DISTANCE) { return Constants.MAX_HUB_DISTANCE; @@ -240,11 +240,10 @@ private double calculateShooterVelocity(double computedDistanceMeters, PoseContr } private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { - Pose2d predictedPose = robotPosePredicitionCalculation(robotPose); return Math.floor( - Math.toDegrees(TurretCalculations.calculateTurretAngle(predictedPose.getX(), predictedPose.getY(), + Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), - DriverStation.getAlliance().get() == DriverStation.Alliance.Blue))); + Robot.allianceColor().get() == DriverStation.Alliance.Blue))); } // Getters for all the subsystems to set posistion. From 384260b9d832789da420262e5ef837d80b2ee3b1 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Wed, 18 Mar 2026 20:02:58 -0400 Subject: [PATCH 11/13] Fixed typo --- src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 7d806d6e..a621ea6d 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -83,7 +83,7 @@ public void periodic() { Pose2d robotPose = getRobotPose(); ShootState currentState = getCurrentShootState(); updateStopDelayState(currentState); - updateTargets(currentState, robotPosePredicitionCalculation(robotPose)); + updateTargets(currentState, robotPosePredictionCalculation(robotPose)); if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); @@ -208,7 +208,7 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { } } - private Pose2d robotPosePredicitionCalculation(Pose2d robotPose) { + private Pose2d robotPosePredictionCalculation(Pose2d robotPose) { Pose2d robotPoseTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); Pose2d predictedPose = robotPoseTransform .plus(new Transform2d( From 2d583b52279b584e8987c2a1f3ac9197a7145c1e Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Wed, 18 Mar 2026 20:16:58 -0400 Subject: [PATCH 12/13] Added flight time --- .../frc/robot/constants/GameConstants.java | 1 - .../robot/subsystems/ControllerSubsystem.java | 19 ++++++++++++------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index be0367d7..952b4ade 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -171,7 +171,6 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; - public static final double PREDICTION_TIME = 2.25; public static final double MAX_HUB_DISTANCE = 5.18; public static final double MIN_HUB_DISTANCE = 1.42; } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index a621ea6d..b9c9f66a 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -83,7 +83,7 @@ public void periodic() { Pose2d robotPose = getRobotPose(); ShootState currentState = getCurrentShootState(); updateStopDelayState(currentState); - updateTargets(currentState, robotPosePredictionCalculation(robotPose)); + updateTargets(currentState, robotPose); if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); @@ -148,16 +148,16 @@ private void updateTargets(ShootState state, Pose2d robotPose) { // No color, do nothing... useShotTargets(FIXED_TARGETS); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { - useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose))); } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { - useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose)); + useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose))); } else { // Unknown color, do nothing... useShotTargets(FIXED_TARGETS); } } - case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose)); + case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPosePredictionCalculation(SHUTTLE_PROFILE.targetPose,robotPose))); } } @@ -208,16 +208,21 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { } } - private Pose2d robotPosePredictionCalculation(Pose2d robotPose) { + private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPose) { + double flightTime = calculateFlightTime(robotPose.getTranslation() + .getDistance(targetPose.getTranslation())); Pose2d robotPoseTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d()); Pose2d predictedPose = robotPoseTransform .plus(new Transform2d( - drivebase.getFieldVelocity().vxMetersPerSecond * Constants.PREDICTION_TIME, - drivebase.getFieldVelocity().vyMetersPerSecond * Constants.PREDICTION_TIME, + drivebase.getFieldVelocity().vxMetersPerSecond * flightTime, + drivebase.getFieldVelocity().vyMetersPerSecond * flightTime, new Rotation2d())); Logger.recordOutput("Predicted pose", predictedPose); return predictedPose; } + private double calculateFlightTime(double computedDistanceMeters) { + return 0.208*computedDistanceMeters + 0.647; + } private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { From 7f6bf3af7ea5b3c5cdfb42e84ce98ecde3401d6d Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Wed, 18 Mar 2026 20:39:39 -0400 Subject: [PATCH 13/13] do nothing parrallele command --- src/main/java/frc/robot/commands/auto/DoNothing.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/DoNothing.java b/src/main/java/frc/robot/commands/auto/DoNothing.java index b9c31d05..b92bf2d1 100644 --- a/src/main/java/frc/robot/commands/auto/DoNothing.java +++ b/src/main/java/frc/robot/commands/auto/DoNothing.java @@ -5,9 +5,8 @@ import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; -import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class DoNothing extends LoggableSequentialCommandGroup{ +public class DoNothing extends LoggableParallelCommandGroup{ public DoNothing(TurretSubsystem turret, AnglerSubsystem angler) { super( new LoggableParallelCommandGroup(