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 7c457fe8..7150b7c6 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,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())); 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/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..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 @@ -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); } } 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..16d6b4b1 --- /dev/null +++ b/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; + } +} 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..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,14 +30,16 @@ public EventBus clearEvents() { events.clear(); return this; } - - public void fireEvent(Class eventClass, E event) { - EventHandler handler = getHandler(eventClass); + @SuppressWarnings("unchecked") + public void fireEvent(E event) { + EventHandler handler = (EventHandler) 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) getHandler(event.getClass()); handler.fire(event); } @@ -43,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 new file mode 100644 index 00000000..c9b85473 --- /dev/null +++ b/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> 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 + // 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)) { + eventBus.fireEvent(event); + } + } + awaitingToggleBack.add(x); + return; + }); + } +} 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); + } }