add set output methods to scoring to remove activation of motors

team401 · Sep 10, 2024 · 35cb5c0 · 35cb5c0
1 parent e755e95
commit 35cb5c0
Show file tree

Hide file tree

Showing 10 changed files with 65 additions and 24 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -33,5 +33,6 @@
     "editor.rulers": [
         100
     ],
-    "java.debug.settings.onBuildFailureProceed": true
+    "java.debug.settings.onBuildFailureProceed": true,
+    "java.format.settings.url": "eclipse-formatter.xml"
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -57,6 +57,7 @@ public static final class FeatureFlags {
         public static final boolean runEndgame = false;
         public static final boolean runDrive = true;
         public static final boolean demoMode = false;
+        public static final boolean outputScore = false;
 
         public static final boolean enableLEDS = true;
     }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -149,6 +149,9 @@ public void configureSubsystems() {
                     if (FeatureFlags.demoMode) {
                         scoringSubsystem.setDemo(true);
                     }
+                    if (!FeatureFlags.outputScore) {
+                        scoringSubsystem.setScoringOutput(false);
+                    }
                 }
 
                 if (FeatureFlags.runEndgame) {

diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIO.java b/src/main/java/frc/robot/subsystems/scoring/AimerIO.java
@@ -40,5 +40,5 @@ public default void setBrakeMode(boolean brake) {}
 
     public default void setStatorCurrentLimit(double limit) {}
 
-    public default void setMotorDisabled(boolean disabled) {}
+    public default void setOutput(boolean output) {}
 }
diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java b/src/main/java/frc/robot/subsystems/scoring/AimerIORoboRio.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants.FeatureFlags;
 import frc.robot.Constants.ScoringConstants;
 import org.littletonrobotics.junction.Logger;
 
@@ -59,7 +60,7 @@ public class AimerIORoboRio implements AimerIO {
     double lastPosition = 0.0;
     double lastTime = Utils.getCurrentTimeSeconds();
 
-    boolean motorDisabled = false;
+    boolean enableOutput = true;
 
     public AimerIORoboRio() {
         aimerLeft.setControl(new Follower(ScoringConstants.aimRightMotorId, true));
@@ -81,7 +82,9 @@ public AimerIORoboRio() {
                         .withStatorCurrentLimit(60)
                         .withStatorCurrentLimitEnable(true));
 
-        aimerRight.setPosition(0.0);
+        if (FeatureFlags.outputScore) {
+            aimerRight.setPosition(0.0);
+        }
 
         controller.setTolerance(0.015);
     }
@@ -183,8 +186,8 @@ public void setStatorCurrentLimit(double limit) {
     }
 
     @Override
-    public void setMotorDisabled(boolean disabled) {
-        motorDisabled = disabled;
+    public void setOutput(boolean output) {
+        enableOutput = output;
     }
 
     @Override
@@ -202,7 +205,7 @@ public void updateInputs(AimerIOInputs inputs) {
         double velocitySetpoint = trapezoidSetpoint.velocity;
 
         if (getEncoderPosition() == -1.75) {
-            motorDisabled = true;
+            enableOutput = false;
         }
 
         if (override) {
@@ -214,13 +217,13 @@ public void updateInputs(AimerIOInputs inputs) {
         }
 
         appliedVolts = MathUtil.clamp(appliedVolts, -12.0, 12.0);
-        if (!motorDisabled || override) {
+        if (enableOutput || override) {
             aimerRight.setVoltage(appliedVolts);
         } else {
             aimerRight.setVoltage(0.0);
         }
 
-        Logger.recordOutput("Scoring/motorDisabled", motorDisabled);
+        Logger.recordOutput("Scoring/Aimer/OutputEnabled", enableOutput);
 
         inputs.aimGoalAngleRad = goalAngleRad;
         inputs.aimProfileGoalAngleRad = controlSetpoint;

diff --git a/src/main/java/frc/robot/subsystems/scoring/HoodIO.java b/src/main/java/frc/robot/subsystems/scoring/HoodIO.java
@@ -24,6 +24,8 @@ public default void setOverrideVolts(double volts) {}
 
     public default void home() {}
 
+    public default void setOutput(boolean output) {}
+
     public default void setPID(double p, double i, double d) {}
 
     public default void setBrakeMode(boolean brake) {}

diff --git a/src/main/java/frc/robot/subsystems/scoring/HoodIOSparkFlex.java b/src/main/java/frc/robot/subsystems/scoring/HoodIOSparkFlex.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants.ScoringConstants;
+import org.littletonrobotics.junction.Logger;
 
 public class HoodIOSparkFlex implements HoodIO {
     private final CANSparkFlex hoodMotor =
@@ -22,6 +23,7 @@ public class HoodIOSparkFlex implements HoodIO {
                             ScoringConstants.hoodMaxAcceleration));
 
     private boolean override = false;
+    private boolean enableOutput = true;
     private boolean homing = false;
 
     double overrideVolts = 0.0;
@@ -99,6 +101,11 @@ public void setBrakeMode(boolean brake) {
         hoodMotor.setIdleMode(brake ? CANSparkFlex.IdleMode.kBrake : CANSparkFlex.IdleMode.kCoast);
     }
 
+    @Override
+    public void setOutput(boolean output) {
+        enableOutput = output;
+    }
+
     @Override
     public void updateInputs(HoodIOInputs inputs) {
         State trapezoidSetpoint =
@@ -107,6 +114,8 @@ public void updateInputs(HoodIOInputs inputs) {
                         new State(initialAngle, initialVelocity),
                         new State(goalAngleRad, 0.0));
 
+        Logger.recordOutput("Scoring/Hood/OutputEnabled", enableOutput);
+
         // if (homing) {
         //     if (hoodMotor.getOutputCurrent() > ScoringConstants.hoodHomeAmps
         //             && homeTimer.get() > 0.4) {
@@ -130,7 +139,9 @@ public void updateInputs(HoodIOInputs inputs) {
         // if (hoodMotor.getOutputCurrent() > 70) {
         //     hoodMotor.setVoltage(0.0);
         // } else {
-        hoodMotor.setVoltage(overrideVolts);
+        if (enableOutput) {
+            hoodMotor.setVoltage(overrideVolts);
+        }
         // }
 
         inputs.hoodAngleRad = hoodMotor.getEncoder().getPosition();

diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java
@@ -651,7 +651,13 @@ public void setDemo(boolean demo) {
     }
 
     public void setArmDisabled(boolean disabled) {
-        aimerIo.setMotorDisabled(disabled);
+        aimerIo.setOutput(!disabled);
+    }
+
+    public void setScoringOutput(boolean output) {
+        aimerIo.setOutput(output);
+        hoodIo.setOutput(output);
+        shooterIo.setOutput(output);
     }
 
     public void forceHood(boolean hoodForced) {

diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIO.java
@@ -31,6 +31,8 @@ public default void setKickerVolts(double volts) {}
 
     public default void setOverrideMode(boolean override) {}
 
+    public default void setOutput(boolean output) {}
+
     public default void setOverrideVolts(double volts) {}
 
     public default void setPID(double p, double i, double d) {}

diff --git a/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalon.java b/src/main/java/frc/robot/subsystems/scoring/ShooterIOTalon.java
@@ -10,6 +10,7 @@
 import frc.robot.Constants.ConversionConstants;
 import frc.robot.Constants.ScoringConstants;
 import frc.robot.Constants.SensorConstants;
+import org.littletonrobotics.junction.Logger;
 
 public class ShooterIOTalon implements ShooterIO {
     private final TalonFX kicker = new TalonFX(ScoringConstants.kickerMotorId);
@@ -22,6 +23,7 @@ public class ShooterIOTalon implements ShooterIO {
     DigitalInput bannerSensor = new DigitalInput(SensorConstants.indexerSensorPort);
 
     private boolean override = false;
+    private boolean enableOutput = true;
     private double overrideVolts = 0.0;
 
     double goalLeftVelocityRPM = 0.0;
@@ -70,30 +72,38 @@ public ShooterIOTalon() {
     public void setShooterVelocityRPM(double velocity) {
         goalLeftVelocityRPM = velocity;
         goalRightVelocityRPM = velocity * ScoringConstants.shooterOffsetAdjustment;
-
-        if (velocity == 0.0) {
-            shooterLeft.setVoltage(0.0);
-            shooterRight.setVoltage(0.0);
-        } else {
-            shooterLeft.setControl(
-                    new VelocityDutyCycle(
-                            goalLeftVelocityRPM / ConversionConstants.kMinutesToSeconds));
-            shooterRight.setControl(
-                    new VelocityDutyCycle(
-                            goalRightVelocityRPM / ConversionConstants.kMinutesToSeconds));
+        if (enableOutput) {
+            if (velocity == 0.0) {
+                shooterLeft.setVoltage(0.0);
+                shooterRight.setVoltage(0.0);
+            } else {
+                shooterLeft.setControl(
+                        new VelocityDutyCycle(
+                                goalLeftVelocityRPM / ConversionConstants.kMinutesToSeconds));
+                shooterRight.setControl(
+                        new VelocityDutyCycle(
+                                goalRightVelocityRPM / ConversionConstants.kMinutesToSeconds));
+            }
         }
     }
 
     @Override
     public void setKickerVolts(double volts) {
-        kicker.setVoltage(volts);
+        if (enableOutput) {
+            kicker.setVoltage(volts);
+        }
     }
 
     @Override
     public void setOverrideMode(boolean override) {
         this.override = override;
     }
 
+    @Override
+    public void setOutput(boolean output) {
+        enableOutput = output;
+    }
+
     @Override
     public void setOverrideVolts(double volts) {
         overrideVolts = volts;
@@ -121,11 +131,13 @@ public void setFF(double kS, double kV, double kA) {
 
     @Override
     public void updateInputs(ShooterIOInputs inputs) {
-        if (override) {
+        if (override && enableOutput) {
             shooterLeft.setVoltage(overrideVolts);
             shooterRight.setVoltage(overrideVolts);
         }
 
+        Logger.recordOutput("Scoring/Shooter/OutputEnabled", enableOutput);
+
         inputs.shooterLeftVelocityRPM =
                 shooterLeft.getVelocity().getValueAsDouble()
                         / ConversionConstants.kSecondsToMinutes;