Merge branch 'main' into yeet-autos

team581 · Oct 31, 2023 · 73d1bce · 73d1bce
2 parents 161bda3 + fb8d839
commit 73d1bce
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/autos/Autos.java b/src/main/java/frc/robot/autos/Autos.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -31,6 +32,8 @@
 import frc.robot.managers.vision.AutoScoreManager;
 import frc.robot.managers.vision.GroundConeManager;
 import frc.robot.swerve.SwerveSubsystem;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.vision.VisionMode;
 import frc.robot.wrist.WristSubsystem;
 import java.lang.ref.WeakReference;
@@ -42,7 +45,7 @@
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
-public class Autos {
+public class Autos extends LifecycleSubsystem {
   private static Command wrapAutoEvent(String commandName, Command command) {
     return Commands.sequence(
             Commands.print("[COMMANDS] Starting auto event " + commandName),
@@ -82,6 +85,8 @@ private static Map<String, Command> wrapAutoEventMap(Map<String, Command> eventM
   private final GroundConeManager groundManager;
   private final ImuSubsystem imu;
 
+  private AutoKindWithoutTeam rawAuto = AutoKindWithoutTeam.MID_1_BALANCE;
+
   public Autos(
       LocalizationSubsystem localization,
       SwerveSubsystem swerve,
@@ -93,6 +98,7 @@ public Autos(
       Autobalance autoBalance,
       GroundConeManager groundManager,
       AutoScoreManager visionManager) {
+    super(SubsystemPriority.AUTO);
     this.localization = localization;
     this.swerve = swerve;
     this.imu = imu;
@@ -262,17 +268,25 @@ private Command getPreloadConeCommand() {
   }
 
   public Command getAutoCommand() {
-    AutoKindWithoutTeam rawAuto = autoChooser.get();
+    AutoKindWithoutTeam rawAutoNullable = autoChooser.get();
 
-    if (rawAuto == null) {
+    if (rawAutoNullable == null) {
       rawAuto = AutoKindWithoutTeam.MID_1_BALANCE;
+    } else {
+      rawAuto = rawAutoNullable;
     }
 
     AutoKind auto = FmsSubsystem.isRedAlliance() ? rawAuto.redVersion : rawAuto.blueVersion;
 
     return buildAutoCommand(auto);
   }
 
+  @Override
+  public void robotPeriodic() {
+    Logger.getInstance().recordOutput("Autos/CurrentAuto", rawAuto.toString());
+    SmartDashboard.putString("Autos/SelectedAuto", rawAuto.toString());
+  }
+
   private Command buildAutoCommand(AutoKind auto) {
     WeakReference<Command> ref = autosCache.get(auto);
     if (ref != null && ref.get() != null) {

diff --git a/src/main/java/frc/robot/controller/DriveController.java b/src/main/java/frc/robot/controller/DriveController.java
@@ -46,7 +46,7 @@ public double getForwardPercentage() {
 
   /** The rotation about the robot's z-axis as a percentage (<code>-1 <= x <= 1</code>) */
   public double getThetaPercentage() {
-    return joystickScale(-1 * getRightX());
+    return joystickScale(-1 * 0.75 * getRightX());
   }
 
   public NodeKind getAutoScoreNodeKind() {

diff --git a/src/main/java/frc/robot/swerve/SwerveModule.java b/src/main/java/frc/robot/swerve/SwerveModule.java
@@ -67,7 +67,7 @@ public SwerveModule(
     driveMotorConfigs.Voltage.PeakForwardVoltage = 12;
     driveMotorConfigs.Voltage.PeakReverseVoltage = -12;
 
-    driveMotorConfigs.CurrentLimits.SupplyCurrentLimit = 35;
+    driveMotorConfigs.CurrentLimits.SupplyCurrentLimit = 15;
     driveMotorConfigs.CurrentLimits.SupplyCurrentLimitEnable = true;
 
     if (constants.driveInversion) {
@@ -122,6 +122,9 @@ public void log() {
         .recordOutput(
             "Swerve/" + constants.corner.toString() + "/DriveMotorStatorCurrent",
             driveMotorStatorCurrent.refresh().getValue());
+
+    Logger.getInstance().recordOutput("Swerve/"+ constants.corner.toString() +"/SteerMotorCancoderPosition", getRawCancoderPosition().getDegrees());
+    Logger.getInstance().recordOutput("Swerve/" + constants.corner.toString() + "/SteerMorotPosition", getSteerMotorPosition().getDegrees());
   }
 
   public void setDesiredState(