diff --git a/src/main/java/frc/robot/controller/DriveController.java b/src/main/java/frc/robot/controller/DriveController.java index 2303576..b762aba 100644 --- a/src/main/java/frc/robot/controller/DriveController.java +++ b/src/main/java/frc/robot/controller/DriveController.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.autoscore.NodeKind; -import frc.robot.managers.superstructure.NodeHeight; +import frc.robot.managers.managers.NodeHeight; public class DriveController extends CommandXboxController { private boolean slowModeEnabled; diff --git a/src/main/java/frc/robot/lights/LightsSubsystem.java b/src/main/java/frc/robot/lights/LightsSubsystem.java index 9c27199..8492902 100644 --- a/src/main/java/frc/robot/lights/LightsSubsystem.java +++ b/src/main/java/frc/robot/lights/LightsSubsystem.java @@ -13,7 +13,7 @@ import frc.robot.intake.HeldGamePiece; import frc.robot.intake.IntakeState; import frc.robot.intake.IntakeSubsystem; -import frc.robot.managers.superstructure.SuperstructureManager; +import frc.robot.managers.managers.SuperstructureManager; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; diff --git a/src/main/java/frc/robot/managers/managers/AutoRotate.java b/src/main/java/frc/robot/managers/managers/AutoRotate.java new file mode 100644 index 0000000..52b43bb --- /dev/null +++ b/src/main/java/frc/robot/managers/managers/AutoRotate.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.managers.managers; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.fms.FmsSubsystem; +import frc.robot.swerve.SwerveSubsystem; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class AutoRotate extends LifecycleSubsystem { + public static Rotation2d getLeftAngle() { + return FmsSubsystem.isRedAlliance() ? Rotation2d.fromDegrees(270) : Rotation2d.fromDegrees(90); + } + + public static Rotation2d getRightAngle() { + return FmsSubsystem.isRedAlliance() ? Rotation2d.fromDegrees(90) : Rotation2d.fromDegrees(270); + } + + public static Rotation2d getForwardAngle() { + return FmsSubsystem.isRedAlliance() ? Rotation2d.fromDegrees(180) : Rotation2d.fromDegrees(0); + } + + public static Rotation2d getBackwardsAngle() { + return FmsSubsystem.isRedAlliance() ? Rotation2d.fromDegrees(0) : Rotation2d.fromDegrees(180); + } + + private final SwerveSubsystem swerve; + private Rotation2d angle = new Rotation2d(); + private boolean enabled; + + public AutoRotate(SwerveSubsystem swerve) { + super(SubsystemPriority.AUTOROTATE); + this.swerve = swerve; + } + + public void setAngle(Rotation2d angle) { + this.angle = angle; + this.enabled = true; + } + + public void disable() { + this.enabled = false; + swerve.disableSnapToAngle(); + } + + @Override + public void robotPeriodic() { + Logger.getInstance().recordOutput("AutoRotate/GoalAngle", angle.getDegrees()); + } + + @Override + public void enabledPeriodic() { + if (enabled) { + swerve.setSnapToAngle(angle); + } + } + + public Command getCommand(Supplier angle) { + return run(() -> setAngle(angle.get())); + } + + public Command getDisableCommand() { + return runOnce(() -> disable()); + } +} diff --git a/src/main/java/frc/robot/managers/managers/Autobalance.java b/src/main/java/frc/robot/managers/managers/Autobalance.java new file mode 100644 index 0000000..f14d40e --- /dev/null +++ b/src/main/java/frc/robot/managers/managers/Autobalance.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.managers.managers; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.fms.FmsSubsystem; +import frc.robot.imu.ImuSubsystem; +import frc.robot.swerve.SwerveSubsystem; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; + +public class Autobalance extends LifecycleSubsystem { + private final SwerveSubsystem swerve; + private final ImuSubsystem imu; + private boolean enabled = false; + private static final double DRIVE_VELOCITY = -0.475; + private static final double ANGLE_THRESHOLD = 9; + private final LinearFilter autoBalanceFilter = LinearFilter.movingAverage(13); + private Rotation2d averageRoll = new Rotation2d(); + private final Debouncer driveVelocityDebouncer = new Debouncer(9 * 0.02); + + public Autobalance(SwerveSubsystem swerve, ImuSubsystem imu) { + super(SubsystemPriority.AUTOBALANCE); + this.swerve = swerve; + this.imu = imu; + } + + public void setEnabled(boolean mode) { + enabled = mode; + if (!mode) { + swerve.disableSnapToAngle(); + } + } + + @Override + public void robotPeriodic() { + averageRoll = Rotation2d.fromDegrees(autoBalanceFilter.calculate(ANGLE_THRESHOLD)); + } + + @Override + public void enabledPeriodic() { + if (enabled) { + Rotation2d goalAngle = Rotation2d.fromDegrees(FmsSubsystem.isRedAlliance() ? 0 : 180); + + if (driveVelocityDebouncer.calculate(getDriveVelocity() == 0)) { + swerve.setXSwerve(true); + } else { + swerve.setXSwerve(false); + } + + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(getDriveVelocity(), 0, 0); + swerve.setSnapToAngle(goalAngle); + swerve.setChassisSpeeds(chassisSpeeds, false); + } + } + + private double getDriveVelocity() { + if (imu.getRoll().getDegrees() > ANGLE_THRESHOLD) { + return DRIVE_VELOCITY * -1; + } else if (imu.getRoll().getDegrees() < -ANGLE_THRESHOLD) { + return DRIVE_VELOCITY; + } else { + return 0; + } + } + + private boolean atGoal() { + return averageRoll.getDegrees() < ANGLE_THRESHOLD + && averageRoll.getDegrees() > -ANGLE_THRESHOLD; + } + + public Command getCommand() { + return Commands.run(() -> setEnabled(true), swerve) + .until(() -> atGoal()) + .withTimeout(15.0) + .andThen(runOnce(() -> setEnabled(false))) + .handleInterrupt(() -> setEnabled(false)); + } +} diff --git a/src/main/java/frc/robot/managers/superstructure/NodeHeight.java b/src/main/java/frc/robot/managers/managers/NodeHeight.java similarity index 85% rename from src/main/java/frc/robot/managers/superstructure/NodeHeight.java rename to src/main/java/frc/robot/managers/managers/NodeHeight.java index 92556a8..ec9a448 100644 --- a/src/main/java/frc/robot/managers/superstructure/NodeHeight.java +++ b/src/main/java/frc/robot/managers/managers/NodeHeight.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.managers.superstructure; +package frc.robot.managers.managers; public enum NodeHeight { LOW, diff --git a/src/main/java/frc/robot/managers/superstructure/Positions.java b/src/main/java/frc/robot/managers/managers/Positions.java similarity index 98% rename from src/main/java/frc/robot/managers/superstructure/Positions.java rename to src/main/java/frc/robot/managers/managers/Positions.java index 5517b75..cc1f990 100644 --- a/src/main/java/frc/robot/managers/superstructure/Positions.java +++ b/src/main/java/frc/robot/managers/managers/Positions.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.managers.superstructure; +package frc.robot.managers.managers; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/managers/SuperstructureManager.java similarity index 97% rename from src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java rename to src/main/java/frc/robot/managers/managers/SuperstructureManager.java index b8364a3..5da5439 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java +++ b/src/main/java/frc/robot/managers/managers/SuperstructureManager.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.managers.superstructure; +package frc.robot.managers.managers; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java b/src/main/java/frc/robot/managers/managers/SuperstructureMotionManager.java similarity index 97% rename from src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java rename to src/main/java/frc/robot/managers/managers/SuperstructureMotionManager.java index 1abb5d5..bd5d229 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java +++ b/src/main/java/frc/robot/managers/managers/SuperstructureMotionManager.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.managers.superstructure; +package frc.robot.managers.managers; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java b/src/main/java/frc/robot/managers/managers/SuperstructurePosition.java similarity index 95% rename from src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java rename to src/main/java/frc/robot/managers/managers/SuperstructurePosition.java index 049479d..e012333 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java +++ b/src/main/java/frc/robot/managers/managers/SuperstructurePosition.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.managers.superstructure; +package frc.robot.managers.managers; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureState.java b/src/main/java/frc/robot/managers/managers/SuperstructureState.java similarity index 94% rename from src/main/java/frc/robot/managers/superstructure/SuperstructureState.java rename to src/main/java/frc/robot/managers/managers/SuperstructureState.java index 5cf7d1e..19f9186 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/managers/managers/SuperstructureState.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.managers.superstructure; +package frc.robot.managers.managers; import frc.robot.intake.IntakeState;