Merge pull request #131 from LakotaRobotics1038/130-make-passer

LakotaRobotics1038 · Mar 26, 2024 · 53becc1 · 53becc1
2 parents d0728c5 + 574f83e
commit 53becc1
Show file tree

Hide file tree

Showing 14 changed files with 174 additions and 52 deletions.
diff --git a/src/main/java/frc/robot/OperatorJoystick.java b/src/main/java/frc/robot/OperatorJoystick.java
@@ -11,6 +11,7 @@
 import frc.robot.commands.UnacquireCommand;
 import frc.robot.commands.ScoringElevatorPositionCommand.FinishActions;
 import frc.robot.commands.ScoringElevatorPositionCommand;
+import frc.robot.commands.ShootPasserCommand;
 import frc.robot.commands.ScoreNoteCommand;
 import frc.robot.subsystems.ScoringElevator;
 import frc.robot.subsystems.Storage;
@@ -62,6 +63,9 @@ private OperatorJoystick() {
         new Trigger(() -> operatorJoystick.getPOVPosition() == PovPositions.Right)
                 .and(() -> !scoringElevatorLock)
                 .toggleOnTrue(new ScoringElevatorPositionCommand(ElevatorSetpoints.Amp, FinishActions.NoFinish));
+        new Trigger(() -> operatorJoystick.getPOVPosition() == PovPositions.Up)
+                .and(() -> !scoringElevatorLock)
+                .toggleOnTrue(new ScoringElevatorPositionCommand(ElevatorSetpoints.Passer, FinishActions.NoFinish));
 
         // Amp buttons
         rightTrigger
@@ -79,6 +83,11 @@ private OperatorJoystick() {
                 .and(() -> scoringElevator.getSetpoint() == ElevatorSetpoints.Trap.value)
                 .whileTrue(new FeedNoteCommand(ScoringLocation.Trap));
 
+        // Passer button
+        leftTrigger
+                .and(() -> scoringElevator.getSetpoint() == ElevatorSetpoints.Passer.value)
+                .whileTrue(new ShootPasserCommand());
+
         leftBumper.onTrue(new DrawbridgeUpCommand());
         rightBumper.onTrue(new DrawbridgeDownCommand());
     }

diff --git a/src/main/java/frc/robot/commands/FeedNoteCommand.java b/src/main/java/frc/robot/commands/FeedNoteCommand.java
@@ -46,7 +46,7 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        scoring.stopRoller();
+        scoring.stop();
         timer.stop();
         timer.reset();
     }

diff --git a/src/main/java/frc/robot/commands/FeedNoteFineAdjCommand.java b/src/main/java/frc/robot/commands/FeedNoteFineAdjCommand.java
@@ -28,6 +28,6 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        scoring.stopRoller();
+        scoring.stop();
     }
 }
diff --git a/src/main/java/frc/robot/commands/ScoreNoteCommand.java b/src/main/java/frc/robot/commands/ScoreNoteCommand.java
@@ -61,7 +61,7 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        scoring.stopRoller();
+        scoring.stop();
         storage.stopStorage();
         timer.stop();
         timer.reset();

diff --git a/src/main/java/frc/robot/commands/ScoringElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ScoringElevatorPositionCommand.java
@@ -32,17 +32,7 @@ public ScoringElevatorPositionCommand(ElevatorSetpoints scoringState) {
     @Override
     public void initialize() {
         scoring.enable();
-        switch (scoringState) {
-            case Amp:
-                scoring.setSetpoint(ElevatorSetpoints.Amp);
-                break;
-            case Ground:
-                scoring.setSetpoint(ElevatorSetpoints.Ground);
-                break;
-            default:
-                scoring.setSetpoint(ElevatorSetpoints.Trap);
-                break;
-        }
+        scoring.setSetpoint(scoringState);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/commands/ShootPasserCommand.java b/src/main/java/frc/robot/commands/ShootPasserCommand.java
@@ -0,0 +1,54 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Passer;
+import frc.robot.subsystems.Scoring;
+import frc.robot.subsystems.Storage;
+
+public class ShootPasserCommand extends Command {
+    private Scoring scoring = Scoring.getInstance();
+    private Storage storage = Storage.getInstance();
+    private Passer passer = Passer.getInstance();
+    private Timer timer = new Timer();
+    private double secondsToScore = 0;
+
+    public ShootPasserCommand(double secondsToScore) {
+        this.addRequirements(scoring);
+        this.secondsToScore = secondsToScore;
+    }
+
+    public ShootPasserCommand() {
+        this.addRequirements(scoring);
+    }
+
+    @Override
+    public void initialize() {
+        timer.start();
+    }
+
+    @Override
+    public void execute() {
+        scoring.feedForPasser();
+        passer.shoot();
+        if (!storage.noteExitingStorage()) {
+            storage.stopStorage();
+        } else {
+            storage.runStorage();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return this.secondsToScore != 0 ? timer.get() > this.secondsToScore : false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        scoring.stop();
+        storage.stopStorage();
+        passer.stop();
+        timer.stop();
+        timer.reset();
+    }
+}
diff --git a/src/main/java/frc/robot/constants/PasserConstants.java b/src/main/java/frc/robot/constants/PasserConstants.java
@@ -0,0 +1,7 @@
+package frc.robot.constants;
+
+public class PasserConstants {
+    public static final int passerMotorPort = 11;
+
+    public static final double shooterSpeed = 1.0;
+}
diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java
@@ -1,11 +1,12 @@
 package frc.robot.constants;
 
 public class ScoringConstants {
-    public static final int rollerMotorPort = 12;
+    public static final int scoringMotorPort = 12;
     public static final double feedTrapSpeed = -0.32;
     public static final double scoreTrapSpeed = 1.0;
     public static final double feedAmpSpeed = -0.32;
     public static final double scoreAmpSpeed = 0.75;
+    public static final double feedPasserSpeed = -0.32;
 
     public enum ScoringLocation {
         Amp,

diff --git a/src/main/java/frc/robot/constants/ScoringElevatorConstants.java b/src/main/java/frc/robot/constants/ScoringElevatorConstants.java
@@ -10,6 +10,7 @@ public class ScoringElevatorConstants {
 
     public static final double groundSetpoint = 0;
     public static final double ampSetpoint = 4.0;
+    public static final double passerSetpoint = 6.0;
     public static final double trapSetpoint = 15.0;
 
     public static final double maxDistance = 0;

diff --git a/src/main/java/frc/robot/constants/StorageConstants.java b/src/main/java/frc/robot/constants/StorageConstants.java
@@ -3,8 +3,7 @@
 public class StorageConstants {
 
     public static final int transitionMotorPort = 7;
-    public static final int leftStoragePort = 11;
-    public static final int rightStoragePort = 8;
+    public static final int storagePort = 8;
 
     public static final int enterStorageLaserPort = 1;
     public static final int exitStorageLaserPort = 0;

diff --git a/src/main/java/frc/robot/subsystems/Passer.java b/src/main/java/frc/robot/subsystems/Passer.java
@@ -0,0 +1,61 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.NeoMotorConstants;
+import frc.robot.constants.PasserConstants;
+
+public class Passer extends SubsystemBase {
+    private final CANSparkFlex passerMotor = new CANSparkFlex(PasserConstants.passerMotorPort, MotorType.kBrushless);
+    private final RelativeEncoder passerEncoder = passerMotor.getEncoder();
+
+    private static Passer instance;
+
+    /**
+     * Creates an instance of the Passer subsystem if it does not already exist.
+     *
+     * @return An instance of the Passer subsystem.
+     */
+    public static Passer getInstance() {
+        if (instance == null) {
+            instance = new Passer();
+        }
+        return instance;
+    }
+
+    private Passer() {
+        passerMotor.restoreFactoryDefaults();
+
+        passerMotor.setIdleMode(IdleMode.kBrake);
+        passerMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxVortexCurrent);
+        passerMotor.setInverted(true);
+
+        passerMotor.burnFlash();
+        passerEncoder.setPosition(0);
+    }
+
+    /**
+     * Get the encoder value of the passer encoder
+     */
+    public double getPosition() {
+        return passerEncoder.getPosition();
+    }
+
+    /**
+     * Runs the passer at a constant speed
+     */
+    public void shoot() {
+        passerMotor.set(PasserConstants.shooterSpeed);
+    }
+
+    /**
+     * Stops the passer
+     */
+    public void stop() {
+        passerMotor.stopMotor();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Scoring.java b/src/main/java/frc/robot/subsystems/Scoring.java
@@ -11,8 +11,8 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Scoring extends SubsystemBase {
-    private final CANSparkFlex rollerMotor = new CANSparkFlex(ScoringConstants.rollerMotorPort, MotorType.kBrushless);
-    private final RelativeEncoder rollerEncoder = rollerMotor.getEncoder();
+    private final CANSparkFlex scoringMotor = new CANSparkFlex(ScoringConstants.scoringMotorPort, MotorType.kBrushless);
+    private final RelativeEncoder scoringEncoder = scoringMotor.getEncoder();
     private double feedAmpSpeed = ScoringConstants.feedAmpSpeed;
 
     private static Scoring instance;
@@ -26,21 +26,21 @@ public static Scoring getInstance() {
     }
 
     public Scoring() {
-        rollerMotor.restoreFactoryDefaults();
+        scoringMotor.restoreFactoryDefaults();
 
-        rollerMotor.setIdleMode(IdleMode.kBrake);
-        rollerMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxVortexCurrent);
-        rollerMotor.setInverted(false);
+        scoringMotor.setIdleMode(IdleMode.kBrake);
+        scoringMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxVortexCurrent);
+        scoringMotor.setInverted(false);
 
-        rollerMotor.burnFlash();
-        rollerEncoder.setPosition(0);
+        scoringMotor.burnFlash();
+        scoringEncoder.setPosition(0);
     }
 
     /**
-     * Get the encoder value of the roller encoder
+     * Get the encoder value of the scoring encoder
      */
     public double getPosition() {
-        return rollerEncoder.getPosition();
+        return scoringEncoder.getPosition();
     }
 
     /**
@@ -57,35 +57,43 @@ public void setFeedAmpSpeed(double speed) {
      * ready to score in amp.
      */
     public void feedForAmp() {
-        rollerMotor.set(feedAmpSpeed);
+        scoringMotor.set(feedAmpSpeed);
     }
 
     /**
      * Runs the scoring roller at the constant speed designated for scoring in amp.
      */
     public void scoreAmp() {
-        rollerMotor.set(ScoringConstants.scoreAmpSpeed);
+        scoringMotor.set(ScoringConstants.scoreAmpSpeed);
     }
 
     /**
      * Runs the scoring roller at the constant speed designated for getting the note
      * ready to score in trap.
      */
     public void feedForTrap() {
-        rollerMotor.set(feedAmpSpeed);
+        scoringMotor.set(feedAmpSpeed);
     }
 
     /**
      * Runs the scoring roller at the constant speed designated for scoring in amp.
      */
     public void scoreTrap() {
-        rollerMotor.set(ScoringConstants.scoreTrapSpeed);
+        scoringMotor.set(ScoringConstants.scoreTrapSpeed);
+    }
+
+    /**
+     * Runs the scoring roller at the constant speed designated for getting the note
+     * ready to shoot in passer.
+     */
+    public void feedForPasser() {
+        scoringMotor.set(ScoringConstants.feedPasserSpeed);
     }
 
     /**
      * Stops the scoring roller.
      */
-    public void stopRoller() {
-        rollerMotor.stopMotor();
+    public void stop() {
+        scoringMotor.stopMotor();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/ScoringElevator.java b/src/main/java/frc/robot/subsystems/ScoringElevator.java
@@ -41,6 +41,7 @@ public class ScoringElevator extends SubsystemBase {
     public enum ElevatorSetpoints {
         Ground(ScoringElevatorConstants.groundSetpoint),
         Amp(ScoringElevatorConstants.ampSetpoint),
+        Passer(ScoringElevatorConstants.passerSetpoint),
         Trap(ScoringElevatorConstants.trapSetpoint);
 
         public final double value;

diff --git a/src/main/java/frc/robot/subsystems/Storage.java b/src/main/java/frc/robot/subsystems/Storage.java
@@ -13,10 +13,8 @@ public class Storage extends SubsystemBase {
 
     private final CANSparkMax transitionMotor = new CANSparkMax(StorageConstants.transitionMotorPort,
             MotorType.kBrushless);
-    private final CANSparkMax leftStorageMotor = new CANSparkMax(
-            StorageConstants.leftStoragePort, MotorType.kBrushless);
-    private final CANSparkMax rightStorageMotor = new CANSparkMax(
-            StorageConstants.rightStoragePort, MotorType.kBrushless);
+    private final CANSparkMax storageMotor = new CANSparkMax(
+            StorageConstants.storagePort, MotorType.kBrushless);
 
     private final DigitalInput enterStorageLaser = new DigitalInput(StorageConstants.enterStorageLaserPort);
     private final DigitalInput exitStorageLaser = new DigitalInput(StorageConstants.exitStorageLaserPort);
@@ -37,26 +35,19 @@ public static Storage getInstance() {
 
     private Storage() {
         transitionMotor.restoreFactoryDefaults();
-        leftStorageMotor.restoreFactoryDefaults();
-        rightStorageMotor.restoreFactoryDefaults();
+        storageMotor.restoreFactoryDefaults();
 
-        leftStorageMotor.setIdleMode(IdleMode.kBrake);
-        rightStorageMotor.setIdleMode(IdleMode.kBrake);
+        storageMotor.setIdleMode(IdleMode.kBrake);
         transitionMotor.setIdleMode(IdleMode.kBrake);
 
-        leftStorageMotor.setInverted(false);
-        rightStorageMotor.setInverted(false);
+        storageMotor.setInverted(true);
         transitionMotor.setInverted(false);
 
-        rightStorageMotor.follow(leftStorageMotor, true);
-
-        rightStorageMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current);
-        leftStorageMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current);
+        storageMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current);
         transitionMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current);
 
         transitionMotor.burnFlash();
-        leftStorageMotor.burnFlash();
-        rightStorageMotor.burnFlash();
+        storageMotor.burnFlash();
     }
 
     public void runTransition() {
@@ -68,15 +59,15 @@ public void reverseTransition() {
     }
 
     public void runStorage() {
-        leftStorageMotor.set(StorageConstants.storageSpeed);
+        storageMotor.set(StorageConstants.storageSpeed);
     }
 
     public void reverseStorage() {
-        leftStorageMotor.set(StorageConstants.reverseStorageSpeed);
+        storageMotor.set(StorageConstants.reverseStorageSpeed);
     }
 
     public void stopStorage() {
-        leftStorageMotor.stopMotor();
+        storageMotor.stopMotor();
     }
 
     public void stopTransition() {