port autobalance and auto rotate from spike

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;

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;
 

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<Rotation2d> angle) {
+    return run(() -> setAngle(angle.get()));
+  }
+
+  public Command getDisableCommand() {
+    return runOnce(() -> disable());
+  }
+}

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

src/main/java/frc/robot/managers/superstructure/NodeHeight.java → 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,

src/main/java/frc/robot/managers/superstructure/Positions.java → 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;
 

src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java → 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;

src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java → 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;

src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java → 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;
 

src/main/java/frc/robot/managers/superstructure/SuperstructureState.java → 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;