From 31207752283b4e00a5ecc4bf2d7393fd2b535c0f Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sat, 3 Oct 2026 18:49:46 -0400 Subject: [PATCH 1/9] Linear align controller --- .../robot/util/autoalign/AutoAlignHelper.java | 18 +---- .../impl/LinearAlignController.java | 80 +++++++++++++++++++ .../robot/util/helpers/DistanceManager.java | 8 ++ 3 files changed, 92 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java diff --git a/src/main/java/frc/robot/util/autoalign/AutoAlignHelper.java b/src/main/java/frc/robot/util/autoalign/AutoAlignHelper.java index 24178d78..7d1e945e 100644 --- a/src/main/java/frc/robot/util/autoalign/AutoAlignHelper.java +++ b/src/main/java/frc/robot/util/autoalign/AutoAlignHelper.java @@ -14,6 +14,8 @@ import frc.robot.subsystems.drive.AutoAlignConfig; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.util.helpers.DistanceManager; + import org.littletonrobotics.junction.Logger; public class AutoAlignHelper { @@ -53,19 +55,7 @@ public ChassisSpeeds getPoseSpeedsLine(Pose2d robotPose, Pose2d targetPose, Gyro // convert to robot relative from field relative ChassisSpeeds fieldRelative = new ChassisSpeeds(xSpeed, ySpeed, rotationalPID); - return convertToFieldRelative(fieldRelative, gyro, robot); - } - - public static ChassisSpeeds convertToFieldRelative(ChassisSpeeds fieldRelative, Gyro gyro, Pose2d robot) { - Translation2d xy = new Translation2d(fieldRelative.vxMetersPerSecond, fieldRelative.vyMetersPerSecond); - Translation2d rotated = xy.rotateBy( - new Rotation2d(gyro.getOffset() - gyro.getRawAngleRadians() + robot.getRotation().getRadians()) - .unaryMinus()); - Logger.recordOutput("A_DEBUG/speedConversionRotation", - new Rotation2d(gyro.getOffset() - gyro.getRawAngleRadians() + robot.getRotation().getRadians()) - .unaryMinus()); - Logger.recordOutput("A_DEBUG/gyroOffset", gyro.getOffset()); - return new ChassisSpeeds(rotated.getX(), rotated.getY(), fieldRelative.omegaRadiansPerSecond); + return DistanceManager.convertToFieldRelative(fieldRelative, gyro, robot); } public ChassisSpeeds getLocalAlignSpeedsLine(Translation2d vector, Gyro gyro, Rotation2d robotRotation, @@ -106,7 +96,7 @@ public ChassisSpeeds getLocalAlignSpeedsLine(Translation2d vector, Gyro gyro, Ro Logger.recordOutput("actualvector", vector); // convert chassis speeds ChassisSpeeds robotRelative = new ChassisSpeeds(vx, vy, rotational); - return convertToFieldRelative(robotRelative, gyro, new Pose2d()); + return DistanceManager.convertToFieldRelative(robotRelative, gyro, new Pose2d()); } public void resetLocalMotionProfile(Translation2d vector, DriveSubsystem driveSubsystem) { diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java new file mode 100644 index 00000000..6abd762c --- /dev/null +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java @@ -0,0 +1,80 @@ +package frc.robot.util.autoalign.system.controller.impl; + +import com.pathplanner.lib.config.PIDConstants; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.RobotPIDConstants; +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.util.autoalign.system.controller.AlignControllerBase; +import frc.robot.util.autoalign.system.eventshandler.AlignSystemEventBus; +import frc.robot.util.autoalign.system.pointprovider.IAlignPointProvider; + +public class LinearAlignController extends AlignControllerBase { + private final PIDController TRANSLATION_PID; + private final PIDController ROTATION_PID; + + private final SlewRateLimiter ACCEL_LIMITER; + private final double maxAccelerationMetersPerSecondSquared = 3; + + private final AlignSystemEventBus EVENT_BUS; + + private final double OUTPUT_DEADBAND = 0.01; + private final double TRANSLATION_DEADBAND = 0.02; + private final double ROTATION_DEADBAND = Units.degreesToRadians(5); + + public LinearAlignController(IAlignPointProvider pointProvider) { + super(pointProvider); + + TRANSLATION_PID = RobotPIDConstants.constructPID(new PIDConstants(0.25, 0, 0)); + ROTATION_PID = RobotPIDConstants.constructPID(new PIDConstants(0.5, 0.001, 0.0001)); + + ACCEL_LIMITER = new SlewRateLimiter(maxAccelerationMetersPerSecondSquared); + + EVENT_BUS = new AlignSystemEventBus(); + } + + @Override + public ChassisSpeeds calculate(ChassisSpeeds measuredSpeeds) { + Pose2d robot = pointProvider.getRobotPose(); + Pose2d target = pointProvider.getTargetPose(); + + Rotation2d angleToTarget = robot.getTranslation().minus(target.getTranslation()).getAngle(); + double dist = robot.getTranslation().getDistance(target.getTranslation()); + + double linearOutput = TRANSLATION_PID.calculate(dist, 0); + linearOutput = MathUtil.clamp(linearOutput, -1, 1); + linearOutput = MathUtil.applyDeadband(linearOutput, OUTPUT_DEADBAND); + linearOutput *= DriveConstants.maxSpeed; + linearOutput = ACCEL_LIMITER.calculate(linearOutput); + + double rotationalOutput = ROTATION_PID.calculate(robot.getRotation().minus(target.getRotation()).getRadians(), + 0); + rotationalOutput = MathUtil.clamp(rotationalOutput, -1, 1); + rotationalOutput = MathUtil.applyDeadband(rotationalOutput, OUTPUT_DEADBAND); + rotationalOutput *= DriveConstants.maxOmega; + + double xSpeed = Math.cos(angleToTarget.getRadians()) * linearOutput; + double ySpeed = -Math.sin(angleToTarget.getRadians()) * linearOutput; + + return new ChassisSpeeds(xSpeed, ySpeed, rotationalOutput); + } + + @Override + public boolean isAligning() { + return pointProvider.getRobotPose().getTranslation() + .getDistance(pointProvider.getTargetPose().getTranslation()) < TRANSLATION_DEADBAND + && Math.abs(pointProvider.getRobotPose().getRotation() + .minus(pointProvider.getTargetPose().getRotation()).getRadians()) < ROTATION_DEADBAND; + } + + @Override + public AlignSystemEventBus getAlignSystemEventBus() { + return EVENT_BUS; + } +} diff --git a/src/main/java/frc/robot/util/helpers/DistanceManager.java b/src/main/java/frc/robot/util/helpers/DistanceManager.java index 1d68772a..d75c19f7 100644 --- a/src/main/java/frc/robot/util/helpers/DistanceManager.java +++ b/src/main/java/frc/robot/util/helpers/DistanceManager.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.sensors.gyro.Gyro; public class DistanceManager { @@ -196,4 +197,11 @@ public static double calculatePolygonProtrusion(Translation2d[] vertices, Transl return maxProtrusion; } + public static ChassisSpeeds convertToFieldRelative(ChassisSpeeds fieldRelative, Gyro gyro, Pose2d robot) { + Translation2d xy = new Translation2d(fieldRelative.vxMetersPerSecond, fieldRelative.vyMetersPerSecond); + Translation2d rotated = xy.rotateBy( + new Rotation2d(gyro.getOffset() - gyro.getRawAngleRadians() + robot.getRotation().getRadians()) + .unaryMinus()); + return new ChassisSpeeds(rotated.getX(), rotated.getY(), fieldRelative.omegaRadiansPerSecond); + } } From 666bdf399205a8898259ba7605de3b0eed449748 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Sat, 3 Oct 2026 19:03:46 -0400 Subject: [PATCH 2/9] Todos --- .../system/controller/impl/LinearAlignController.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java index 6abd762c..44efe4bc 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java @@ -22,21 +22,19 @@ public class LinearAlignController extends AlignControllerBase { private final SlewRateLimiter ACCEL_LIMITER; private final double maxAccelerationMetersPerSecondSquared = 3; - private final AlignSystemEventBus EVENT_BUS; + private final AlignSystemEventBus EVENT_BUS = new AlignSystemEventBus(); private final double OUTPUT_DEADBAND = 0.01; private final double TRANSLATION_DEADBAND = 0.02; private final double ROTATION_DEADBAND = Units.degreesToRadians(5); public LinearAlignController(IAlignPointProvider pointProvider) { - super(pointProvider); + super(pointProvider); // TODO fix align bus crash TRANSLATION_PID = RobotPIDConstants.constructPID(new PIDConstants(0.25, 0, 0)); ROTATION_PID = RobotPIDConstants.constructPID(new PIDConstants(0.5, 0.001, 0.0001)); ACCEL_LIMITER = new SlewRateLimiter(maxAccelerationMetersPerSecondSquared); - - EVENT_BUS = new AlignSystemEventBus(); } @Override @@ -62,11 +60,11 @@ public ChassisSpeeds calculate(ChassisSpeeds measuredSpeeds) { double xSpeed = Math.cos(angleToTarget.getRadians()) * linearOutput; double ySpeed = -Math.sin(angleToTarget.getRadians()) * linearOutput; - return new ChassisSpeeds(xSpeed, ySpeed, rotationalOutput); + return new ChassisSpeeds(xSpeed, ySpeed, rotationalOutput); // TODO coordinate system } @Override - public boolean isAligning() { + public boolean isAligning() { // TODO fire event when true (in periodic) return pointProvider.getRobotPose().getTranslation() .getDistance(pointProvider.getTargetPose().getTranslation()) < TRANSLATION_DEADBAND && Math.abs(pointProvider.getRobotPose().getRotation() From 2085ac2c02d01edc6d2ce39ade4de36da3f25547 Mon Sep 17 00:00:00 2001 From: Shaun Mathew <143555953+SpeedSlicer@users.noreply.github.com> Date: Wed, 7 Oct 2026 19:32:06 -0400 Subject: [PATCH 3/9] Listener Manager --- .vscode/settings.json | 2 +- .../controller/AlignControllerBase.java | 5 ++- .../impl/AutopilotAlignController.java | 12 ++--- .../impl/LinearAlignController.java | 23 +++++----- .../robot/util/eventbus/handler/EventBus.java | 7 +-- .../listenerHandler/ListenerManager.java | 44 +++++++++++++++++++ 6 files changed, 67 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e6ede86..1745ba0d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.dependency.enableDependencyCheckup": false + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/AlignControllerBase.java b/src/main/java/frc/robot/util/autoalign/system/controller/AlignControllerBase.java index c827d3cd..21d9d17f 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/AlignControllerBase.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/AlignControllerBase.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.robot.util.autoalign.system.eventshandler.AlignSystemEventBus; import frc.robot.util.autoalign.system.pointprovider.IAlignPointProvider; +import frc.robot.util.eventbus.listenerHandler.ListenerManager; import lombok.Getter; public abstract class AlignControllerBase { @@ -10,10 +11,12 @@ public abstract class AlignControllerBase { protected IAlignPointProvider pointProvider; @Getter protected AlignSystemEventBus alignSystemEventBus; - + @Getter + protected ListenerManager listenerManager; public AlignControllerBase(IAlignPointProvider pp) { this.pointProvider = pp; this.alignSystemEventBus = new AlignSystemEventBus(); + listenerManager = new ListenerManager(alignSystemEventBus); pp.attachAlignController(this); } public abstract ChassisSpeeds calculate(ChassisSpeeds robotChassisSpeeds); diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java index 024c3c28..e9c8b82c 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java @@ -16,7 +16,7 @@ import lombok.Getter; public class AutopilotAlignController extends AlignControllerBase { - Pose2d goalPose = null; + Pose2d goalPose; @Getter boolean isAligning = false; @@ -28,11 +28,13 @@ public AutopilotAlignController(IAlignPointProvider pointProvider) { super(pointProvider); this.controller = RobotPIDConstants.constructPID(RobotPIDConstants.autopilotTurnPID); this.controller.enableContinuousInput(0, 2 * Math.PI); + goalPose = new Pose2d(); + target = new APTarget(new Pose2d()); + getListenerManager().registerEventTrigger(() -> isComplete(), new ECompleteReachPoint()); } @Override public ChassisSpeeds calculate(ChassisSpeeds robotChassisSpeeds) { - this.update(); target = new APTarget(pointProvider.getTargetPose()); if (!isComplete() && pointProvider.hasPoint()) { isAligning = true; @@ -46,12 +48,6 @@ public ChassisSpeeds calculate(ChassisSpeeds robotChassisSpeeds) { return new ChassisSpeeds(); } - void update() { - if (isComplete()) { - getAlignSystemEventBus().fireEvent(ECompleteReachPoint.class, new ECompleteReachPoint()); - } - } - boolean isComplete() { return AutopilotConstants.kAutopilot.atTarget(pointProvider.getRobotPose(), target); } diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java index 44efe4bc..bb783e3a 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java @@ -12,9 +12,8 @@ import frc.robot.constants.RobotPIDConstants; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.autoalign.system.controller.AlignControllerBase; -import frc.robot.util.autoalign.system.eventshandler.AlignSystemEventBus; +import frc.robot.util.autoalign.system.eventshandler.events.ECompleteReachPoint; import frc.robot.util.autoalign.system.pointprovider.IAlignPointProvider; - public class LinearAlignController extends AlignControllerBase { private final PIDController TRANSLATION_PID; private final PIDController ROTATION_PID; @@ -22,19 +21,18 @@ public class LinearAlignController extends AlignControllerBase { private final SlewRateLimiter ACCEL_LIMITER; private final double maxAccelerationMetersPerSecondSquared = 3; - private final AlignSystemEventBus EVENT_BUS = new AlignSystemEventBus(); - private final double OUTPUT_DEADBAND = 0.01; private final double TRANSLATION_DEADBAND = 0.02; private final double ROTATION_DEADBAND = Units.degreesToRadians(5); public LinearAlignController(IAlignPointProvider pointProvider) { - super(pointProvider); // TODO fix align bus crash + super(pointProvider); TRANSLATION_PID = RobotPIDConstants.constructPID(new PIDConstants(0.25, 0, 0)); ROTATION_PID = RobotPIDConstants.constructPID(new PIDConstants(0.5, 0.001, 0.0001)); - + ACCEL_LIMITER = new SlewRateLimiter(maxAccelerationMetersPerSecondSquared); + getListenerManager().registerEventTrigger(() -> isFinished(), new ECompleteReachPoint()); } @Override @@ -60,19 +58,18 @@ public ChassisSpeeds calculate(ChassisSpeeds measuredSpeeds) { double xSpeed = Math.cos(angleToTarget.getRadians()) * linearOutput; double ySpeed = -Math.sin(angleToTarget.getRadians()) * linearOutput; - return new ChassisSpeeds(xSpeed, ySpeed, rotationalOutput); // TODO coordinate system + return new ChassisSpeeds(xSpeed, ySpeed, rotationalOutput); } @Override - public boolean isAligning() { // TODO fire event when true (in periodic) + public boolean isAligning() { + return !isFinished(); + } + + public boolean isFinished() { return pointProvider.getRobotPose().getTranslation() .getDistance(pointProvider.getTargetPose().getTranslation()) < TRANSLATION_DEADBAND && Math.abs(pointProvider.getRobotPose().getRotation() .minus(pointProvider.getTargetPose().getRotation()).getRadians()) < ROTATION_DEADBAND; } - - @Override - public AlignSystemEventBus getAlignSystemEventBus() { - return EVENT_BUS; - } } diff --git a/src/main/java/frc/robot/util/eventbus/handler/EventBus.java b/src/main/java/frc/robot/util/eventbus/handler/EventBus.java index 94b1b38f..92fb7166 100644 --- a/src/main/java/frc/robot/util/eventbus/handler/EventBus.java +++ b/src/main/java/frc/robot/util/eventbus/handler/EventBus.java @@ -29,11 +29,12 @@ public EventBus clearEvents() { return this; } - public void fireEvent(Class eventClass, E event) { - EventHandler handler = getHandler(eventClass); + public void fireEvent(E event) { + @SuppressWarnings("unchecked") + EventHandler handler = (EventHandler) getHandler(event.getClass()); if (handler == null) { - throw new IllegalArgumentException("Event is not registered: " + eventClass.getName()); + throw new IllegalArgumentException("Event is not registered: " + event.getClass().getName()); } handler.fire(event); diff --git a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java new file mode 100644 index 00000000..1e1ff4d0 --- /dev/null +++ b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java @@ -0,0 +1,44 @@ +package frc.robot.util.eventbus.listenerHandler; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.BooleanSupplier; + +import frc.robot.util.periodic.PeriodicBase; +import frc.robot.util.eventbus.events.IEvent; +import frc.robot.util.eventbus.handler.EventBus; + +public class ListenerManager extends PeriodicBase { + Map> fireEventHashMap; + List awaitingToggleBack; + EventBus eventBus; + public ListenerManager(EventBus eventBus) { + this.eventBus = eventBus; + this.fireEventHashMap = new HashMap<>(); + this.awaitingToggleBack = new ArrayList<>(); + } + public void registerEventTrigger(BooleanSupplier condition, IEvent fireEvent) { + List ev = fireEventHashMap.getOrDefault(condition, new ArrayList<>()); + ev.add(fireEvent); + fireEventHashMap.put(condition, ev); + } + @Override + public void periodic() { + awaitingToggleBack.forEach((x) -> { + if (!x.getAsBoolean()) { + awaitingToggleBack.remove(x); + } + }); + fireEventHashMap.keySet().forEach((x) -> { + if (x.getAsBoolean() && !awaitingToggleBack.contains(x)) { + for (IEvent event : fireEventHashMap.get(x)) { + eventBus.fireEvent(event); + } + } + awaitingToggleBack.add(x); + return; + }); + } +} From f5c8dd365f5afc48805058141c6c6568af36bced Mon Sep 17 00:00:00 2001 From: Shaun Mathew <143555953+SpeedSlicer@users.noreply.github.com> Date: Wed, 7 Oct 2026 19:55:58 -0400 Subject: [PATCH 4/9] bloopdefloopdebloopdefloop, fixed the linear align controller, and took out a crash too! (please envision this with a fun rhyme sequence its sooooo hard to convey it while in a commit message) OK THANK YOU!!! --- simgui-ds.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 11 ++++++++--- .../impl/AutopilotAlignController.java | 4 ++-- .../controller/impl/LinearAlignController.java | 3 +-- .../listenerHandler/ListenerManager.java | 16 ++++++++++++---- 5 files changed, 24 insertions(+), 12 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 28f09afe..825047d8 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -101,7 +101,7 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "03000000373500000410000044010000" } ], "useEnableDisableHotkeys": true diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e8a15ad5..ffab9b50 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,6 +57,7 @@ import frc.robot.subsystems.turret.TurretConstants; import frc.robot.subsystems.turret.TurretSubsystem; import frc.robot.util.autoalign.system.controller.impl.AutopilotAlignController; +import frc.robot.util.autoalign.system.controller.impl.LinearAlignController; import frc.robot.util.autoalign.system.pointprovider.impl.SinglePointProvider; import frc.robot.util.autonomous.AutonBuilder; import frc.robot.util.autonomous.AutonChooser; @@ -101,7 +102,7 @@ public class RobotContainer { private DriveToPoint driveToPointWeight; private ShotCorrectionWeight shotCorrectionWeight; private LockToPointWeight lockToPointWeight; - private AutoAlignWeight driveAlignSysWeight; + private AutoAlignWeight autopilotAlignSysWeight, linearAlignController; // dashboards private SysIdChooser sysIdChooser; @@ -157,11 +158,15 @@ public RobotContainer() { () -> (!RobotState.isTest() ? driveController : pitController).a().getAsBoolean(), 4); DriveWeightCommand.addPersistentWeight(joystickDriveWeight); - driveAlignSysWeight = new AutoAlignWeight( + autopilotAlignSysWeight = new AutoAlignWeight( new AutopilotAlignController(new SinglePointProvider(() -> new Pose2d(5, 5, Rotation2d.k180deg), () -> RobotOdometry.instance.getPose("Main"))), driveSubsystem); - + linearAlignController = new AutoAlignWeight( + new LinearAlignController(new SinglePointProvider(() -> new Pose2d(5, 5, Rotation2d.k180deg), + () -> RobotOdometry.instance.getPose("Main"))), + driveSubsystem); + DriveWeightCommand.addPersistentWeight(linearAlignController); driveToPointWeight = new DriveToPoint(() -> RobotOdometry.instance.getPose("Main"), () -> new Pose2d( AllianceManager.chooseFromAlliance(FieldConstants.blueTowerBarCenter, FieldConstants.redTowerBarCenter), new Rotation2d())); diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java index e9c8b82c..92b7bbf2 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java @@ -28,8 +28,8 @@ public AutopilotAlignController(IAlignPointProvider pointProvider) { super(pointProvider); this.controller = RobotPIDConstants.constructPID(RobotPIDConstants.autopilotTurnPID); this.controller.enableContinuousInput(0, 2 * Math.PI); - goalPose = new Pose2d(); - target = new APTarget(new Pose2d()); + goalPose = pointProvider.getTargetPose(); + target = new APTarget(goalPose); getListenerManager().registerEventTrigger(() -> isComplete(), new ECompleteReachPoint()); } diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java index bb783e3a..16d6b4b1 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java @@ -56,8 +56,7 @@ public ChassisSpeeds calculate(ChassisSpeeds measuredSpeeds) { rotationalOutput *= DriveConstants.maxOmega; double xSpeed = Math.cos(angleToTarget.getRadians()) * linearOutput; - double ySpeed = -Math.sin(angleToTarget.getRadians()) * linearOutput; - + double ySpeed = Math.sin(angleToTarget.getRadians()) * linearOutput; return new ChassisSpeeds(xSpeed, ySpeed, rotationalOutput); } diff --git a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java index 1e1ff4d0..108c9e2f 100644 --- a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java +++ b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java @@ -26,11 +26,19 @@ public void registerEventTrigger(BooleanSupplier condition, IEvent fireEvent) { } @Override public void periodic() { - awaitingToggleBack.forEach((x) -> { - if (!x.getAsBoolean()) { - awaitingToggleBack.remove(x); + // Source - https://stackoverflow.com/a/18448795 + // Posted by Kevin DiTraglia, modified by community. See post 'Timeline' for + // change history + // Retrieved 2026-10-07, License - CC BY-SA 4.0 + + List toRemove = new ArrayList(); + for (BooleanSupplier bs : awaitingToggleBack) { + if (bs.getAsBoolean()) { + toRemove.add(bs); } - }); + } + awaitingToggleBack.removeAll(toRemove); + fireEventHashMap.keySet().forEach((x) -> { if (x.getAsBoolean() && !awaitingToggleBack.contains(x)) { for (IEvent event : fireEventHashMap.get(x)) { From 4ff807622156643ea05ba663f0f8bcfe7eebd55a Mon Sep 17 00:00:00 2001 From: Shaun Mathew <143555953+SpeedSlicer@users.noreply.github.com> Date: Wed, 7 Oct 2026 19:57:37 -0400 Subject: [PATCH 5/9] extra extra removed --- src/main/java/frc/robot/RobotContainer.java | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ffab9b50..26040df7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,7 +42,6 @@ import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.drive.DriveWeightCommand; -import frc.robot.subsystems.drive.weights.AutoAlignWeight; import frc.robot.subsystems.drive.weights.DriveToPoint; import frc.robot.subsystems.drive.weights.JoystickDriveWeight; import frc.robot.subsystems.drive.weights.LockToPointWeight; @@ -56,9 +55,6 @@ import frc.robot.subsystems.spindexer.SpindexerSubsystem; import frc.robot.subsystems.turret.TurretConstants; import frc.robot.subsystems.turret.TurretSubsystem; -import frc.robot.util.autoalign.system.controller.impl.AutopilotAlignController; -import frc.robot.util.autoalign.system.controller.impl.LinearAlignController; -import frc.robot.util.autoalign.system.pointprovider.impl.SinglePointProvider; import frc.robot.util.autonomous.AutonBuilder; import frc.robot.util.autonomous.AutonChooser; import frc.robot.util.helpers.AllianceManager; @@ -102,7 +98,6 @@ public class RobotContainer { private DriveToPoint driveToPointWeight; private ShotCorrectionWeight shotCorrectionWeight; private LockToPointWeight lockToPointWeight; - private AutoAlignWeight autopilotAlignSysWeight, linearAlignController; // dashboards private SysIdChooser sysIdChooser; @@ -157,16 +152,6 @@ public RobotContainer() { .poseSatisfies(RobotOdometry.instance.getPose("Main")), () -> (!RobotState.isTest() ? driveController : pitController).a().getAsBoolean(), 4); DriveWeightCommand.addPersistentWeight(joystickDriveWeight); - - autopilotAlignSysWeight = new AutoAlignWeight( - new AutopilotAlignController(new SinglePointProvider(() -> new Pose2d(5, 5, Rotation2d.k180deg), - () -> RobotOdometry.instance.getPose("Main"))), - driveSubsystem); - linearAlignController = new AutoAlignWeight( - new LinearAlignController(new SinglePointProvider(() -> new Pose2d(5, 5, Rotation2d.k180deg), - () -> RobotOdometry.instance.getPose("Main"))), - driveSubsystem); - DriveWeightCommand.addPersistentWeight(linearAlignController); driveToPointWeight = new DriveToPoint(() -> RobotOdometry.instance.getPose("Main"), () -> new Pose2d( AllianceManager.chooseFromAlliance(FieldConstants.blueTowerBarCenter, FieldConstants.redTowerBarCenter), new Rotation2d())); From 86449bac922b392d26e7df54dfe13c4522396f89 Mon Sep 17 00:00:00 2001 From: Shaun Mathew <143555953+SpeedSlicer@users.noreply.github.com> Date: Wed, 14 Oct 2026 18:44:11 -0400 Subject: [PATCH 6/9] fixes according to jake's comments --- .vscode/settings.json | 2 +- .../impl/AutopilotAlignController.java | 6 +----- .../robot/util/eventbus/handler/EventBus.java | 19 ++++++++++++------- .../listenerHandler/ListenerManager.java | 1 + 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 1745ba0d..5e6ede86 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.dependency.enableDependencyCheckup": false } diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java index 92b7bbf2..93d31a26 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java @@ -6,7 +6,6 @@ import com.therekrab.autopilot.Autopilot.APResult; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.robot.constants.RobotConstants.AutopilotConstants; import frc.robot.constants.RobotPIDConstants; @@ -16,8 +15,6 @@ import lombok.Getter; public class AutopilotAlignController extends AlignControllerBase { - Pose2d goalPose; - @Getter boolean isAligning = false; APTarget target; @@ -28,8 +25,7 @@ public AutopilotAlignController(IAlignPointProvider pointProvider) { super(pointProvider); this.controller = RobotPIDConstants.constructPID(RobotPIDConstants.autopilotTurnPID); this.controller.enableContinuousInput(0, 2 * Math.PI); - goalPose = pointProvider.getTargetPose(); - target = new APTarget(goalPose); + target = new APTarget(pointProvider.getTargetPose()); getListenerManager().registerEventTrigger(() -> isComplete(), new ECompleteReachPoint()); } diff --git a/src/main/java/frc/robot/util/eventbus/handler/EventBus.java b/src/main/java/frc/robot/util/eventbus/handler/EventBus.java index 92fb7166..c50ebc23 100644 --- a/src/main/java/frc/robot/util/eventbus/handler/EventBus.java +++ b/src/main/java/frc/robot/util/eventbus/handler/EventBus.java @@ -2,12 +2,14 @@ import java.util.HashMap; import java.util.Map; +import java.util.logging.LogManager; +import java.util.logging.Logger; import frc.robot.util.eventbus.events.IEvent; import frc.robot.util.eventbus.events.IEventListener; public class EventBus { - + public static Logger EVENT_LOGGER = LogManager.getLogManager().getLogger("[EVENTBUS]"); protected final Map, EventHandler> events; public EventBus() { @@ -28,15 +30,16 @@ public EventBus clearEvents() { events.clear(); return this; } - + @SuppressWarnings("unchecked") public void fireEvent(E event) { - @SuppressWarnings("unchecked") EventHandler handler = (EventHandler) getHandler(event.getClass()); if (handler == null) { - throw new IllegalArgumentException("Event is not registered: " + event.getClass().getName()); + addEvent(new EventHandler<>(event.getClass())); + EVENT_LOGGER.warning("Event " + event.getClass() + + " added to bus because of an attempt to fire the event. Please register this event if you want to use it. "); } - + handler = (EventHandler) getHandler(event.getClass()); handler.fire(event); } @@ -44,9 +47,11 @@ public void registerListener(Class eventClass, IEventListe EventHandler handler = getHandler(eventClass); if (handler == null) { - throw new IllegalArgumentException("Event is not registered: " + eventClass.getName()); + addEvent(new EventHandler<>(eventClass)); + EVENT_LOGGER.warning("Event " + eventClass.getCanonicalName() + + " added to bus because of an attempt to register a listener to the event. Please register this event if you want to use it. "); } - + handler = getHandler(eventClass); handler.add(listener); } diff --git a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java index 108c9e2f..74b81c98 100644 --- a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java +++ b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java @@ -14,6 +14,7 @@ public class ListenerManager extends PeriodicBase { Map> fireEventHashMap; List awaitingToggleBack; EventBus eventBus; + public ListenerManager(EventBus eventBus) { this.eventBus = eventBus; this.fireEventHashMap = new HashMap<>(); From ba5d3b45c162c15de82d6c3b156fe8cab5adfd88 Mon Sep 17 00:00:00 2001 From: Shaun Mathew <143555953+SpeedSlicer@users.noreply.github.com> Date: Wed, 14 Oct 2026 18:44:54 -0400 Subject: [PATCH 7/9] Update ListenerManager.java --- .../util/eventbus/listenerHandler/ListenerManager.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java index 74b81c98..58cadfc8 100644 --- a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java +++ b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java @@ -11,9 +11,9 @@ import frc.robot.util.eventbus.handler.EventBus; public class ListenerManager extends PeriodicBase { - Map> fireEventHashMap; - List awaitingToggleBack; - EventBus eventBus; + private Map> fireEventHashMap; + private List awaitingToggleBack; + private EventBus eventBus; public ListenerManager(EventBus eventBus) { this.eventBus = eventBus; From 3a156b37003ac4854c01d112b6fd05e73ab2a419 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 14 Oct 2026 20:26:38 -0400 Subject: [PATCH 8/9] Quick fixes --- .../system/controller/impl/AutopilotAlignController.java | 2 +- .../util/eventbus/listenerHandler/ListenerManager.java | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java index 93d31a26..0d6c837e 100644 --- a/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java +++ b/src/main/java/frc/robot/util/autoalign/system/controller/impl/AutopilotAlignController.java @@ -44,7 +44,7 @@ public ChassisSpeeds calculate(ChassisSpeeds robotChassisSpeeds) { return new ChassisSpeeds(); } - boolean isComplete() { + public boolean isComplete() { return AutopilotConstants.kAutopilot.atTarget(pointProvider.getRobotPose(), target); } } diff --git a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java index 58cadfc8..40cb0cc9 100644 --- a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java +++ b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java @@ -11,20 +11,22 @@ import frc.robot.util.eventbus.handler.EventBus; public class ListenerManager extends PeriodicBase { - private Map> fireEventHashMap; - private List awaitingToggleBack; - private EventBus eventBus; + private final Map> fireEventHashMap; + private final List awaitingToggleBack; + private final EventBus eventBus; public ListenerManager(EventBus eventBus) { this.eventBus = eventBus; this.fireEventHashMap = new HashMap<>(); this.awaitingToggleBack = new ArrayList<>(); } + public void registerEventTrigger(BooleanSupplier condition, IEvent fireEvent) { List ev = fireEventHashMap.getOrDefault(condition, new ArrayList<>()); ev.add(fireEvent); fireEventHashMap.put(condition, ev); } + @Override public void periodic() { // Source - https://stackoverflow.com/a/18448795 From b3bcd8580ad4e930c6c1d53974deaad757764623 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 14 Oct 2026 20:27:07 -0400 Subject: [PATCH 9/9] Spotless --- .../robot/util/eventbus/listenerHandler/ListenerManager.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java index 40cb0cc9..c9b85473 100644 --- a/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java +++ b/src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java @@ -26,7 +26,7 @@ public void registerEventTrigger(BooleanSupplier condition, IEvent fireEvent) { ev.add(fireEvent); fireEventHashMap.put(condition, ev); } - + @Override public void periodic() { // Source - https://stackoverflow.com/a/18448795