Completed LobShotCommand (#272)

* Completed LobShotCommand * A little comment * Made command extend ParallelCommandGroup * Mapped to a button --------- Co-authored-by: Alex Schokking <[email protected]>
Team488 · Mar 29, 2024 · 90255b0 · 90255b0
1 parent 498baf7
commit 90255b0
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 5 deletions.
diff --git a/src/main/java/competition/commandgroups/PrepareToLobShotCommand.java b/src/main/java/competition/commandgroups/PrepareToLobShotCommand.java
@@ -0,0 +1,22 @@
+package competition.commandgroups;
+
+import competition.subsystems.arm.ArmSubsystem;
+import competition.subsystems.arm.commands.SetArmExtensionCommand;
+import competition.subsystems.shooter.ShooterWheelSubsystem;
+import competition.subsystems.shooter.commands.WarmUpShooterCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+import javax.inject.Inject;
+
+public class PrepareToLobShotCommand extends ParallelCommandGroup {
+
+    @Inject
+    public PrepareToLobShotCommand(SetArmExtensionCommand setArmExtension, WarmUpShooterCommand warmUpShooter,
+                                   ArmSubsystem arm) {
+        warmUpShooter.setTargetRpm(ShooterWheelSubsystem.TargetRPM.LOB_SHOT);
+        setArmExtension.setTargetExtension(ArmSubsystem.UsefulArmPosition.LOB_SHOT);
+
+        this.addCommands(warmUpShooter, setArmExtension);
+    }
+
+}
diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java
@@ -10,6 +10,7 @@
 import competition.auto_programs.SubwooferShotFromTopShootThenShootTopSpikeThenShootTwoCenter;
 import competition.commandgroups.PrepareToFireAtSpeakerFromPodiumCommand;
 import competition.commandgroups.PrepareToFireNearestGoodScoringPositionCommand;
+import competition.commandgroups.PrepareToLobShotCommand;
 import competition.subsystems.arm.ArmSubsystem;
 import competition.subsystems.arm.commands.CalibrateArmsManuallyCommand;
 import competition.subsystems.arm.commands.ContinuouslyPointArmAtSpeakerCommand;
@@ -126,7 +127,8 @@ public void setupAdvancedOperatorCommands(
             RemoveForcedBrakingCommand removeForcedBrakingCommand,
             PrepareForHangingCommand prepareForHangingCommand,
             AmpSignalToggleCommand ampSignalCommand,
-            ToggleFlipperCommand toggleFlipperCommand
+            ToggleFlipperCommand toggleFlipperCommand,
+            PrepareToLobShotCommand prepareToLobShotCommand
     ) {
         //Useful arm positions
         var armToCollection = setArmExtensionCommandProvider.get();
@@ -154,7 +156,6 @@ public void setupAdvancedOperatorCommands(
         // Combine into useful actions
         // Note manipulation:
         var collectNoteFromGround = intakeCollectorProvider.get().alongWith(armToCollection);
-        var collectNoteFromSource = intakeCollectorProvider.get().alongWith(armToSource);
         var scoochNote = intakeScoocher.alongWith(armToScooch);
 
         // Preparing to score:
@@ -176,7 +177,7 @@ public void setupAdvancedOperatorCommands(
         oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Y).whileTrue(prepareToFireAtSubwoofer);
         oi.operatorGamepadAdvanced.getXboxButton(XboxButton.RightJoystickYAxisPositive).onTrue(forceEngageBrakeCommand);
         oi.operatorGamepadAdvanced.getXboxButton(XboxButton.RightJoystickYAxisNegative).onTrue(removeForcedBrakingCommand);
-        oi.operatorGamepadAdvanced.getPovIfAvailable(0).whileTrue(collectNoteFromSource);
+        oi.operatorGamepadAdvanced.getPovIfAvailable(0).whileTrue(prepareToLobShotCommand);
         oi.operatorGamepadAdvanced.getPovIfAvailable(180).whileTrue(manualHangingModeCommand);
         oi.operatorGamepadAdvanced.getPovIfAvailable(270).whileTrue(ampSignalCommand);
         oi.operatorGamepadAdvanced.getPovIfAvailable(90).whileTrue(toggleFlipperCommand);

diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java
@@ -119,7 +119,8 @@ public enum UsefulArmPosition {
         HANG_APPROACH,
         PROTECTED_FAR_AMP_SHOT,
         PROTECTED_PODIUM_SHOT,
-        COLLECT_DIRECTLY_FROM_SOURCE
+        COLLECT_DIRECTLY_FROM_SOURCE,
+        LOB_SHOT
     }
 
     private DoubleInterpolator speakerDistanceToExtensionInterpolator;
@@ -492,6 +493,10 @@ public double getUsefulArmPositionExtensionInMm(UsefulArmPosition usefulArmPosit
             case COLLECT_DIRECTLY_FROM_SOURCE:
                 extension = 180;
                 break;
+            case LOB_SHOT:
+                //This still needs to be found
+                extension = 55;
+                break;
             case HANG_APPROACH:
                 extension = 100;
                 break;

diff --git a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java
@@ -24,7 +24,8 @@ public enum TargetRPM {
         STOP,
         TYPICAL,
         MELEE,
-        INTO_AMP
+        INTO_AMP,
+        LOB_SHOT
     }
 
     //need pose for real time calculations
@@ -42,6 +43,7 @@ public enum TargetRPM {
     private double iMaxAccumValueForShooter;
     private final DoubleProperty typicalShotRpm;
     private final DoubleProperty meleeShotRpm;
+    private final DoubleProperty lobShotRpm;
 
     //DEFINING MOTORS
     public XCANSparkMax upperWheelMotor;
@@ -59,6 +61,8 @@ public ShooterWheelSubsystem(XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, P
         typicalShotRpm = pf.createPersistentProperty("TypicalShotRpm", 4000);
         meleeShotRpm = pf.createPersistentProperty("MeeleShotRpm", 3500);
         intoAmpShotRpm = pf.createPersistentProperty("IntoAmpShotRpm", 800);
+        //Value still needs to be found
+        lobShotRpm = pf.createPersistentProperty("LobShotRpm", 3800);
 
         this.pose = pose;
         this.converter = new DoubleInterpolator();
@@ -109,6 +113,7 @@ public void setTargetRPM(TargetRPM target) {
             case TYPICAL -> setTargetValue(typicalShotRpm.get());
             case MELEE -> setTargetValue(meleeShotRpm.get());
             case INTO_AMP -> setTargetValue(intoAmpShotRpm.get());
+            case LOB_SHOT ->  setTargetValue(lobShotRpm.get());
             default -> setTargetValue(0.0);
         }
     }