Feature/more align controllers

simgui-ds.json

@@ -101,7 +101,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "03000000373500000410000044010000"
     }
   ],
   "useEnableDisableHotkeys": true

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,8 +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.pointprovider.impl.SinglePointProvider;
 import frc.robot.util.autonomous.AutonBuilder;
 import frc.robot.util.autonomous.AutonChooser;
 import frc.robot.util.helpers.AllianceManager;
@@ -101,7 +98,6 @@ public class RobotContainer {
   private DriveToPoint driveToPointWeight;
   private ShotCorrectionWeight shotCorrectionWeight;
   private LockToPointWeight lockToPointWeight;
-  private AutoAlignWeight driveAlignSysWeight;
 
   // dashboards
   private SysIdChooser sysIdChooser;
@@ -156,12 +152,6 @@ public RobotContainer() {
             .poseSatisfies(RobotOdometry.instance.getPose("Main")),
         () -> (!RobotState.isTest() ? driveController : pitController).a().getAsBoolean(), 4);
     DriveWeightCommand.addPersistentWeight(joystickDriveWeight);
-
-    driveAlignSysWeight = new AutoAlignWeight(
-        new AutopilotAlignController(new SinglePointProvider(() -> new Pose2d(5, 5, Rotation2d.k180deg),
-            () -> RobotOdometry.instance.getPose("Main"))),
-        driveSubsystem);
-
     driveToPointWeight = new DriveToPoint(() -> RobotOdometry.instance.getPose("Main"), () -> new Pose2d(
         AllianceManager.chooseFromAlliance(FieldConstants.blueTowerBarCenter, FieldConstants.redTowerBarCenter),
         new Rotation2d()));

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) {

src/main/java/frc/robot/util/autoalign/system/controller/AlignControllerBase.java

@@ -3,17 +3,20 @@
 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 {
   @Getter
   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);

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 = null;
-
   @Getter
   boolean isAligning = false;
   APTarget target;
@@ -28,11 +25,12 @@ public AutopilotAlignController(IAlignPointProvider pointProvider) {
     super(pointProvider);
     this.controller = RobotPIDConstants.constructPID(RobotPIDConstants.autopilotTurnPID);
     this.controller.enableContinuousInput(0, 2 * Math.PI);
+    target = new APTarget(pointProvider.getTargetPose());
+    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,13 +44,7 @@ public ChassisSpeeds calculate(ChassisSpeeds robotChassisSpeeds) {
     return new ChassisSpeeds();
   }
 
-  void update() {
-    if (isComplete()) {
-      getAlignSystemEventBus().fireEvent(ECompleteReachPoint.class, new ECompleteReachPoint());
-    }
-  }
-
-  boolean isComplete() {
+  public boolean isComplete() {
     return AutopilotConstants.kAutopilot.atTarget(pointProvider.getRobotPose(), target);
   }
 }

src/main/java/frc/robot/util/autoalign/system/controller/impl/LinearAlignController.java

@@ -0,0 +1,74 @@
+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.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;
+
+  private final SlewRateLimiter ACCEL_LIMITER;
+  private final double maxAccelerationMetersPerSecondSquared = 3;
+
+  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);
+    getListenerManager().registerEventTrigger(() -> isFinished(), new ECompleteReachPoint());
+  }
+
+  @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 !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;
+  }
+}

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<Class<? extends IEvent>, EventHandler<? extends IEvent>> events;
 
   public EventBus() {
@@ -28,24 +30,28 @@ public EventBus clearEvents() {
     events.clear();
     return this;
   }
-
-  public <E extends IEvent> void fireEvent(Class<E> eventClass, E event) {
-    EventHandler<E> handler = getHandler(eventClass);
+  @SuppressWarnings("unchecked")
+  public <E extends IEvent> void fireEvent(E event) {
+    EventHandler<E> handler = (EventHandler<E>) getHandler(event.getClass());
 
     if (handler == null) {
-      throw new IllegalArgumentException("Event is not registered: " + eventClass.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<E>) getHandler(event.getClass());
     handler.fire(event);
   }
 
   public <E extends IEvent> void registerListener(Class<E> eventClass, IEventListener<E> listener) {
     EventHandler<E> 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);
   }
 

src/main/java/frc/robot/util/eventbus/listenerHandler/ListenerManager.java

@@ -0,0 +1,55 @@
+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 {
+  private final Map<BooleanSupplier, List<IEvent>> fireEventHashMap;
+  private final List<BooleanSupplier> 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<IEvent> ev = fireEventHashMap.getOrDefault(condition, new ArrayList<>());
+    ev.add(fireEvent);
+    fireEventHashMap.put(condition, ev);
+  }
+
+  @Override
+  public void periodic() {
+    // 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<BooleanSupplier> toRemove = new ArrayList<BooleanSupplier>();
+    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)) {
+          eventBus.fireEvent(event);
+        }
+      }
+      awaitingToggleBack.add(x);
+      return;
+    });
+  }
+}

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);
+  }
 }