Shooter

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

@@ -4,12 +4,22 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 
+// import org.jcp.xml.dsig.internal.dom.Utils;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
+import frc.robot.subsystems.shooter.feederWheel.FeederWheelIOSparkMax;
 
 public class RobotContainer {
 
+  private ShooterSubsystem shooter;
+
   private final Alert driverDisconnected =
       new Alert("Driver controller disconnected (port 0).", AlertType.kWarning);
   private final Alert operatorDisconnected =
@@ -23,8 +33,18 @@ public class RobotContainer {
   private final CommandXboxController driverCon = new CommandXboxController(0);
   private final CommandXboxController operatorCon = new CommandXboxController(1);
 
+  Trigger xButton = operatorCon.x();
+
   public RobotContainer(){
       //add things here
+      shooter = new ShooterSubsystem(
+          new FeederWheelIOSparkMax(23),
+          new double[] { 0, 25, 12, 52},
+          new double[] { 1, 0.4, 1, 0.4},
+          new double[] { 1, 0.4, 0, 1}
+      );
+
+      configureBindings();
    }
 
   public Command getAutonomousCommand() {
@@ -37,4 +57,57 @@ public void updateAlerts() {
     operatorDisconnected.set(!DriverStation.isJoystickConnected(operatorCon.getHID().getPort()));
 
   }
-}
+
+  private void configureBindings() {
+
+    var operatorHID = operatorCon.getHID();
+    var driverHID = driverCon.getHID();
+
+    // SHOOTER COMMANDS
+
+    // Bottom
+    var oATrigger = new Trigger(() -> operatorHID.getAButton());
+    oATrigger.whileTrue(Commands.run(() -> shooter.goToSetpoint(0)));
+
+    // Protected Shot
+    var oBTrigger = new Trigger(() -> operatorHID.getBButton());
+    oBTrigger.whileTrue(Commands.run(() -> shooter.goToSetpoint(1)));
+
+    // Source Intake
+    var oXTrigger = new Trigger(() -> operatorHID.getXButton());
+    oXTrigger.whileTrue(Commands.run(() -> shooter.goToSetpoint(2)));
+
+    // Speaker
+    var oYTrigger = new Trigger(() -> operatorHID.getYButton());
+    oYTrigger.onTrue(Commands.runOnce(() -> shooter.goToSetpoint(3)));
+
+    // Reset Shooter Angle
+    // var oStartTrigger = new Trigger(() -> operatorHID.getStartButton());
+    // oStartTrigger.whileTrue(new MoveShooterToBottomAndResetCmd(shooter, 0.2));
+
+    // Adjust the shooter angle of this setpoint
+    var oPOVUpTrigger = new Trigger(() -> operatorHID.getPOV() == 0);
+    oPOVUpTrigger.onTrue(Commands.runOnce(() -> shooter.modifyAngleSetpoint(1)));
+
+    var oPOVDownTrigger = new Trigger(() -> operatorHID.getPOV() == 180);
+    oPOVDownTrigger.onTrue(Commands.runOnce(() -> shooter.modifyAngleSetpoint(-1)));
+
+
+
+    // Adjust the shooter wheel speed of this setpoint
+
+    var oPOVRightTrigger = new Trigger(() -> operatorHID.getPOV() == 90);
+    oPOVRightTrigger.onTrue(Commands.runOnce(() -> shooter.modifyShooterSpeedSetpoint(0.01)));
+
+    var oPOVLeftTrigger = new Trigger(() -> operatorHID.getPOV() == 270);
+    oPOVLeftTrigger.onTrue(Commands.runOnce(() -> shooter.modifyShooterSpeedSetpoint(-0.01)));
+
+    // Adjust the feeder wheel speed of this setpoint
+    var oRightUpTrigger = new Trigger(() -> operatorHID.getRightY() > 0.7);
+    oRightUpTrigger.onTrue(Commands.runOnce(() -> shooter.modifyFeederSetpoint(-0.01)));
+
+    var oRightDownTrigger = new Trigger(() -> operatorHID.getRightY() < -0.7);
+    oRightDownTrigger.onTrue(Commands.runOnce(() -> shooter.modifyFeederSetpoint(0.01)));
+
+  }
+}

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -0,0 +1,205 @@
+package frc.robot.subsystems.shooter;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.shooter.feederWheel.FeederWheelIO;
+
+public class ShooterSubsystem extends SubsystemBase {
+
+    private final double[] angleSetpoints;
+
+    private final double[] wheelSpeedSetpoints;
+
+    private final double[] feederSpeedSetpoints;
+
+    private int setpointIndex = 0;
+
+
+    private final FeederWheelIO feeder;
+
+    private final boolean feederReversed = true;
+
+    private double angleSetpoint = 0.0; 
+    private double wheelSpeedSetpoint = 0.1;
+    private double feederSpeedSetpoint = 0.1;
+
+
+    public ShooterSubsystem() {
+        throw new IllegalArgumentException("You must pass in valid hardware for a subsystem to work");
+    }
+
+    /*
+     * Create a new shooter subsystem
+     * @param leftShooter The left shooter wheel
+     * @param rightShooter The right shooter wheel
+     * @param feeder The feeder wheel
+     * @param angleMechanism The angle mechanism
+     * @param noteSensor The note sensor
+     * @param angleSetpoints in degrees
+     * @param wheelSpeedSetpoints normalized
+     * @param feederSpeedSetpoints normalized
+     */
+    public ShooterSubsystem(FeederWheelIO feeder, double[] angleSetpoints
+    , double[] wheelSpeedSetpoints, double[] feederSpeedSetpoints) {
+
+        if (feeder == null || angleSetpoints == null || wheelSpeedSetpoints == null || feederSpeedSetpoints == null) {
+            throw new IllegalArgumentException("You must pass in valid hardware for a subsystem to work");
+        }
+
+        if (angleSetpoints.length != wheelSpeedSetpoints.length || angleSetpoints.length != feederSpeedSetpoints.length) {
+            throw new IllegalArgumentException("The length of the angle setpoints, wheel speed setpoints, and feeder speed setpoints must be the same");
+        }
+
+        // Initialize things
+        this.feeder = feeder;
+
+        this.wheelSpeedSetpoints = wheelSpeedSetpoints;
+
+        this.angleSetpoints = angleSetpoints;
+        this.feederSpeedSetpoints = feederSpeedSetpoints;
+
+
+        // Turn on brake mode for the feeder
+        this.feeder.setBrakeMode(true);
+
+
+
+
+    }
+
+    @Override
+    public void periodic() {
+        feeder.periodic();
+    }
+
+
+    /*
+     * Spin the shooter wheels
+     */
+
+    public void setManualAngleSetpoint(double angle, double wheelSpeed, double feederSpeed) {
+        this.angleSetpoint = angle;
+        this.wheelSpeedSetpoint = wheelSpeed;
+        this.feederSpeedSetpoint = feederSpeed;
+    }
+
+    /*
+     * Get the percent at which the shooter wheels are spinning
+     */
+
+    public double getFeederVelocityPercent() {
+        return feeder.getVelocityPercent();
+    }   
+
+
+    /*
+     * Spin the feeder wheel
+     */
+    public void spinFeederWheel(boolean reversed) {
+        if (reversed) {
+            feeder.spinWheel(feederReversed ? -feederSpeedSetpoint : feederSpeedSetpoint);
+        } else {
+            feeder.spinWheel( feederReversed ? feederSpeedSetpoint : -feederSpeedSetpoint);
+        }
+    }
+
+
+    /*
+     * Spin the feeder wheel manually
+     * This is used when not moving to a specific setpoint
+     */
+    public void spinFeederWheelManual(double normalizedSpeed) {
+        feeder.spinWheel(feederReversed ? -normalizedSpeed : normalizedSpeed);
+    }
+
+
+
+
+    /*
+     * Get the current angle of the shooter in degrees
+     */
+
+    /*
+     * Enable manual control of the angle mechanism
+     * This disables all internal control loops
+     * USE WITH CAUTION
+     */
+
+
+    /*
+     * Get whether manual control of the angle mechanism is enabled
+     */
+
+
+    /*
+     * Set the velocity of the angle mechanism
+     * This is only used when manual control is enabled
+     */
+
+
+
+    /*
+     * Reset the angle of the shooter to the bottom
+     */
+
+
+    /*
+     * Move to a specific setpoint
+     * This is used to change the shooter angle, wheel speed, and feeder speed
+     */
+    public void goToSetpoint(int setpointIndex) {
+        if (setpointIndex < 0 || setpointIndex >= angleSetpoints.length) {
+            throw new IllegalArgumentException("Invalid setpoint.  Setpoint must be between 0 and " + (angleSetpoints.length - 1) + " inclusive.");
+        }
+        this.angleSetpoint = angleSetpoints[setpointIndex];
+        this.wheelSpeedSetpoint = wheelSpeedSetpoints[setpointIndex];
+        this.feederSpeedSetpoint = feederSpeedSetpoints[setpointIndex];
+        this.setpointIndex = setpointIndex;
+    }
+
+
+    /*
+     * Modify the current setpoint
+     */
+    public void modifyAngleSetpoint(double delta) {
+        angleSetpoints[setpointIndex] += delta;
+        goToSetpoint(setpointIndex);
+    }
+
+
+    public void modifyFeederSetpoint(double delta) {
+        feederSpeedSetpoints[setpointIndex] += delta;
+        goToSetpoint(setpointIndex);
+    }
+
+    public void modifyShooterSpeedSetpoint(double delta) {
+        wheelSpeedSetpoints[setpointIndex] += delta;
+        goToSetpoint(setpointIndex);
+    }
+
+    public int getSetpointIndex() {
+        return setpointIndex;
+    }
+
+    /*
+     * Degrees
+     */
+    public double getAngleSetpoint() {
+        return angleSetpoint;
+    }
+
+    /*
+     * Normalized wheel speed setpoint
+
+     */
+    public double getWheelSpeedSetpoint() {
+        return wheelSpeedSetpoint;
+    }
+
+    /*
+     * Normalized feeder speed setpoint
+     */
+    public double getFeederSpeedSetpoint() {
+        return feederSpeedSetpoint;
+    }
+
+}

src/main/java/frc/robot/subsystems/shooter/feederWheel/FeederWheelIO.java

@@ -0,0 +1,12 @@
+package frc.robot.subsystems.shooter.feederWheel;
+
+public interface FeederWheelIO {
+
+    public void spinWheel(double normalizedVelocity);
+
+    public double getVelocityPercent();
+
+    public void periodic();
+
+    public void setBrakeMode(boolean brake);
+}

src/main/java/frc/robot/subsystems/shooter/feederWheel/FeederWheelIOSparkMax.java

@@ -0,0 +1,68 @@
+package frc.robot.subsystems.shooter.feederWheel;
+
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+// import com.revrobotics.can;
+
+public class FeederWheelIOSparkMax implements FeederWheelIO {
+
+    private final SparkMax feederMotor;
+
+    private final SparkMaxConfig config = new SparkMaxConfig();
+
+    private boolean newInput = false;
+
+    private double normalizedVelocity = 0.0;
+
+    private double maxSpeedRPM = 5000.0;
+
+    final int maxCurrentA = 40;
+
+
+    public FeederWheelIOSparkMax(int canID) {
+        this.feederMotor = new SparkMax(canID, MotorType.kBrushless);
+        this.config.smartCurrentLimit(maxCurrentA);
+        this.feederMotor.configure(config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters);       
+        Shuffleboard.getTab("Current").addDouble("Feeder Motor Output Current", () -> feederMotor.getOutputCurrent());
+
+
+    }
+
+    @Override
+    public void spinWheel(double normalizedVelocity) {
+        this.normalizedVelocity = Math.max(-1.0, Math.min(1.0, normalizedVelocity));
+        newInput = true;
+    }
+
+    @Override
+    public double getVelocityPercent() {
+        return feederMotor.getEncoder().getVelocity() / maxSpeedRPM;
+    }
+
+    @Override
+    public void periodic() {
+        if (newInput) {
+            feederMotor.set(normalizedVelocity);
+            newInput = false;
+        } else {
+            feederMotor.set(0.0);
+        }
+
+
+    }
+
+
+
+    @Override
+    public void setBrakeMode(boolean brake) {
+        config.idleMode(brake ? IdleMode.kBrake : IdleMode.kCoast);
+        feederMotor.configure(config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters);
+
+    }
+}