add demo shooting

Author: linglejack06

src/main/java/frc/robot/Constants.java

@@ -575,6 +575,9 @@ public static final class ScoringConstants {
         public static final double hoodMaxVelocity = 0.5;
         public static final double hoodMaxAcceleration = 0.5;
 
+        public static final double demoShooterRPM = 1000;
+        public static final double demoAimAngle = 0.1;
+
         // NOTE - This should be monotonically increasing
         // Key - Distance in meters
         // Value - Aimer angle in radians

src/main/java/frc/robot/RobotContainer.java

@@ -135,6 +135,9 @@ public void configureSubsystems() {
                 if (FeatureFlags.runDrive) {
                     driveTelemetry =
                             new Telemetry(DriveConstants.MaxSpeedMetPerSec, new TelemetryIOLive());
+                    if (FeatureFlags.demoMode) {
+                        drivetrain.setDemo(true);
+                    }
                 }
 
                 if (FeatureFlags.runScoring) {
@@ -143,6 +146,9 @@ public void configureSubsystems() {
                                     new ShooterIOTalon(),
                                     new AimerIORoboRio(),
                                     new HoodIOSparkFlex());
+                    if (FeatureFlags.demoMode) {
+                        scoringSubsystem.setDemo(true);
+                    }
                 }
 
                 if (FeatureFlags.runEndgame) {

src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java

@@ -64,6 +64,7 @@ public class ScoringSubsystem extends SubsystemBase implements Tunable {
     private boolean overrideShoot = false;
     private boolean overrideStageAvoidance = false;
     private boolean overrideBeamBreak = false;
+    private boolean demo = false;
 
     private boolean hoodForced = false;
 
@@ -240,31 +241,53 @@ private void spit() {
     }
 
     private void prime() {
+        double shooterRPM;
+        double aimAngle;
         double distanceToGoal = findDistanceToGoal();
-        Logger.recordOutput("scoring/aimGoal", getAimerAngle(distanceToGoal));
-        shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distanceToGoal));
-        aimerIo.setAimAngleRad(getAimerAngle(distanceToGoal), false);
+        if (demo) {
+            shooterRPM = ScoringConstants.demoShooterRPM;
+            aimAngle = ScoringConstants.demoAimAngle;
+        } else {
+            shooterRPM = shooterInterpolated.getValue(distanceToGoal);
+            aimAngle = getAimerAngle(distanceToGoal);
+        }
+        Logger.recordOutput("scoring/aimGoal", aimAngle);
+        shooterIo.setShooterVelocityRPM(shooterRPM);
+        aimerIo.setAimAngleRad(aimAngle, false);
         if (!overrideBeamBreak) {
             shooterIo.setKickerVolts(hasNote() ? 0.0 : ScoringConstants.kickerIntakeVolts);
         }
-
-        boolean shooterReady =
-                shooterInputs.shooterLeftVelocityRPM
-                                < (shooterInputs.shooterLeftGoalVelocityRPM
-                                        + ScoringConstants.shooterUpperVelocityMarginRPM)
-                        && shooterInputs.shooterLeftVelocityRPM
-                                > (shooterInputs.shooterLeftGoalVelocityRPM
-                                        - ScoringConstants.shooterLowerVelocityMarginRPM);
-        boolean aimReady =
-                Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad)
-                                < aimerAngleTolerance.getValue(distanceToGoal)
-                        && Math.abs(aimerInputs.aimVelocityErrorRadPerSec)
-                                < ScoringConstants.aimAngleVelocityMargin;
-        boolean driveReady = driveAllignedSupplier.get();
+        boolean shooterReady;
+        boolean aimReady;
+        if (demo) {
+            shooterReady =
+                    shooterInputs.shooterLeftVelocityRPM
+                                    < ScoringConstants.shooterUpperVelocityMarginRPM
+                            && shooterInputs.shooterLeftVelocityRPM
+                                    > ScoringConstants.shooterLowerVelocityMarginRPM;
+            aimReady =
+                    Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad) < 0.1
+                            && Math.abs(aimerInputs.aimVelocityErrorRadPerSec)
+                                    < ScoringConstants.aimAngleVelocityMargin;
+        } else {
+            shooterReady =
+                    shooterInputs.shooterLeftVelocityRPM
+                                    < (shooterInputs.shooterLeftGoalVelocityRPM
+                                            + ScoringConstants.shooterUpperVelocityMarginRPM)
+                            && shooterInputs.shooterLeftVelocityRPM
+                                    > (shooterInputs.shooterLeftGoalVelocityRPM
+                                            - ScoringConstants.shooterLowerVelocityMarginRPM);
+            aimReady =
+                    Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad)
+                                    < aimerAngleTolerance.getValue(distanceToGoal)
+                            && Math.abs(aimerInputs.aimVelocityErrorRadPerSec)
+                                    < ScoringConstants.aimAngleVelocityMargin;
+        }
+        boolean driveReady = demo || driveAllignedSupplier.get();
         boolean fieldLocationReady = true;
         if (!DriverStation.getAlliance().isPresent()) {
             fieldLocationReady = true;
-        } else {
+        } else if (!demo) {
             switch (DriverStation.getAlliance().get()) {
                 case Blue:
                     fieldLocationReady =
@@ -322,11 +345,16 @@ private void ampPrime() {
     }
 
     private void shoot() {
-        double distancetoGoal = findDistanceToGoal();
-
-        double shootRPM = shooterInterpolated.getValue(distancetoGoal);
-        shooterIo.setShooterVelocityRPM(shootRPM);
-        double aimAngleRad = aimerInterpolated.getValue(distancetoGoal);
+        double shootRPM;
+        double aimAngleRad;
+        if (!demo) {
+            double distancetoGoal = findDistanceToGoal();
+            shootRPM = shooterInterpolated.getValue(distancetoGoal);
+            aimAngleRad = aimerInterpolated.getValue(distancetoGoal);
+        } else {
+            shootRPM = ScoringConstants.demoShooterRPM;
+            aimAngleRad = ScoringConstants.demoAimAngle;
+        }
         aimerIo.setAimAngleRad(aimAngleRad, false);
 
         shooterIo.setKickerVolts(10);
@@ -423,7 +451,6 @@ public boolean hasNote() {
 
     public boolean aimerAtIntakePosition() {
         return aimerInputs.aimAngleRad > ScoringConstants.intakeAngleToleranceRadians;
-        // return true;\][]
     }
 
     public boolean canIntake() {
@@ -619,6 +646,10 @@ public void setOverrideBeamBrake(boolean overrideBeamBrake) {
         this.overrideBeamBreak = overrideBeamBrake;
     }
 
+    public void setDemo(boolean demo) {
+        this.demo = demo;
+    }
+
     public void setArmDisabled(boolean disabled) {
         aimerIo.setMotorDisabled(disabled);
     }
@@ -688,15 +719,15 @@ public void setVolts(double volts, int slot) {
         switch (slot) {
                 // Aimer
             case 0:
-                aimerIo.setOverrideVolts(volts);
+                aimerIo.setOverrideVolts(demo ? volts * 0.5 : volts);
                 break;
                 // Hood
             case 1:
-                hoodIo.setOverrideVolts(volts);
+                hoodIo.setOverrideVolts(demo ? volts * 0.5 : volts);
                 break;
                 // Shooter
             case 2:
-                shooterIo.setOverrideVolts(volts);
+                shooterIo.setOverrideVolts(demo ? volts * 0.5 : volts);
                 break;
             default:
                 throw new IllegalArgumentException("Invalid slot");