Shooter omri

.vscode/settings.json

@@ -1,3 +1,4 @@
 {
-    "java.configuration.updateBuildConfiguration": "automatic"
+    "java.configuration.updateBuildConfiguration": "automatic",
+    "C_Cpp.errorSquiggles": "enabled"
 }

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

@@ -705,12 +705,12 @@ public static final class Shooter {
     public static final class Hood {
       public static final int HOOD_ID = 12;
 
-      public static final double KP = 0.0;
+      public static final double KP = 2000;
       public static final double KI = 0.0;
-      public static final double KD = 0.0;
+      public static final double KD = 80.0;
       public static final double KV = 0.0;
-      public static final double KS = 0.0;
-      public static final double KG = 0.0;
+      public static final double KS = 0.124;
+      public static final double KG = 0.4;
 
       public static final double STATOR_CURRENT_LIMIT = 40.0;
       public static final double SUPPLY_CURRENT_LIMIT = 40.0;
@@ -748,14 +748,16 @@ public static final class Rollers {
       public static final int WARMUP_2_ID = 9;
       public static final int WARMUP_3_ID = 11;
 
-      public static final double KP = 0.0;
+      public static final double KP = 0.85;
       public static final double KI = 0.0;
       public static final double KD = 0.0;
-      public static final double KV = 0.0;
+      public static final double KV = 0.124;
       public static final double KS = 0.0;
 
       public static final double STATOR_CURRENT_LIMIT = 120.0;
       public static final double SUPPLY_CURRENT_LIMIT = 40.0;
+
+      public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0 / 12.0;
     }
   }
 

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

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commandGroups.ShootBasicHood;
 // * KEEP FOR WIN COMMAND TESTING
 import frc.robot.commands.SwerveCommands.SwerveJoystickCommand;
 import frc.robot.generated.TunerConstants;
@@ -19,6 +20,7 @@
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeVisionDetection;
+import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
 import frc.robot.util.CustomController;
 import frc.robot.util.VisionUtils;
@@ -70,6 +72,7 @@ public class RobotContainer {
       Constants.intakeVisionOnRobot
           ? new IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera.INTAKE_CAM)
           : null;
+  public final ShooterSubsystem shooter = new ShooterSubsystem();
 
   // * KEEP FOR INTERMAP TESTING
   //   private double hoodAngle = 18.369;
@@ -100,6 +103,27 @@ public RobotContainer() {
   //   }
 
   private void configureBindings() {
+    // Shooter
+    shooter.setDefaultCommand(shooter.runOnce(shooter::stopShooter));
+    joystick
+        .rightBumper()
+        .whileTrue(
+            new ShootBasicHood(
+                () -> 44.2,
+                () -> Constants.Shooter.Hood.MIN_HOOD_POSITION,
+                shooter,
+                intakeSubsystem,
+                hopperSubsystem));
+    joystick
+        .rightTrigger()
+        .whileTrue(
+            new ShootBasicHood(
+                () -> 58.2,
+                () -> Constants.Shooter.Hood.MAX_HOOD_POSITION,
+                shooter,
+                intakeSubsystem,
+                hopperSubsystem));
+
     // Swerve
     DoubleSupplier frontBackFunction = () -> -joystick.getLeftY();
     DoubleSupplier leftRightFunction = () -> -joystick.getLeftX();

src/main/java/frc/robot/commandGroups/ShootBasicHood.java

@@ -0,0 +1,22 @@
+package frc.robot.commandGroups;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import java.util.function.DoubleSupplier;
+
+public class ShootBasicHood extends ParallelCommandGroup {
+  public ShootBasicHood(
+      DoubleSupplier speed,
+      DoubleSupplier angle,
+      ShooterSubsystem shooterSubsystem,
+      IntakeSubsystem intakeSubsystem,
+      HopperSubsystem hopperSubsystem) {
+    addCommands(
+        shooterSubsystem.shootWithHood(speed.getAsDouble(), angle.getAsDouble()),
+        Commands.waitUntil(shooterSubsystem::isShooterAtSpeed)
+            .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()));
+  }
+}

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

@@ -1,88 +1,197 @@
 package frc.robot.subsystems;
 
 import static edu.wpi.first.units.Units.Rotations;
+
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
-
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.util.LoggedTalonFX;
 
-
 public class ShooterSubsystem extends SubsystemBase {
-
-    private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood;
-    private final CANcoder hoodEncoder;
-
-
-    /*
-     * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0.
-     * make instance variables of type double for the target roller speed and target hood angle
-     */
-
-    public ShooterSubsystem() {
-        // The canbus is a communication system that can connect devices like the roboRIO, pdh, and motors.
-        CANBus canbus = Constants.Swerve.CAN_BUS;
-
-        // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some motor information
-        warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus);
-        warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus);
-        warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus);
-
-        shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later we can just reference the `shooter` variable
-
-        // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants.
-
-        // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants
-
-        // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants
 
-        // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast
-
-        TalonFXConfiguration rollersConfig = new TalonFXConfiguration();
-        // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created.
-
-        warmup1.getConfigurator().apply(rollersConfig);
-        // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and warmup3 as well.
-
-        Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned);
-        // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration.
-
-        hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS);
-
-        // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the pid and feedforward gains found in Constants
-
-        // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants
-
-        // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast
-
-        FeedbackConfigs hoodFeedbackConfigs =
+  private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood;
+  private final CANcoder hoodEncoder;
+  private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0);
+  private final PositionVoltage positionRequest = new PositionVoltage(0.0);
+  private double targetRollerSpeed;
+  private double targetHoodAngle;
+
+  /*
+   * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0.
+   * make instance variables of type double for the target roller speed and target hood angle
+   */
+
+  public ShooterSubsystem() {
+    // The canbus is a communication system that can connect devices like the roboRIO, pdh, and
+    // motors.
+    CANBus canbus = Constants.Swerve.CAN_BUS;
+
+    // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some
+    // motor information
+    warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus);
+    warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus);
+    warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus);
+
+    shooter =
+        warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later
+    // we can just reference the `shooter` variable
+
+    // Initialize the hood motor just like the warmup motors. You can find the necessary ID in
+    // Constants.
+    hood = new LoggedTalonFX("ShooterHood", Constants.Shooter.Hood.HOOD_ID, canbus);
+
+    // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the
+    // pid and feedforward gains found in Constants
+    Slot0Configs rollersSlot0Configs = new Slot0Configs();
+    rollersSlot0Configs.kV = Constants.Shooter.Rollers.KV;
+    rollersSlot0Configs.kP = Constants.Shooter.Rollers.KP;
+    rollersSlot0Configs.kI = Constants.Shooter.Rollers.KI;
+    rollersSlot0Configs.kD = Constants.Shooter.Rollers.KD;
+
+    // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it
+    // with the stator and supply limits found in Constants
+    CurrentLimitsConfigs rollersClConfigs = new CurrentLimitsConfigs();
+    rollersClConfigs.StatorCurrentLimit = Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT;
+    rollersClConfigs.SupplyCurrentLimit = Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT;
+
+    // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it
+    // to treat clockwise as positive, and neutral mode as coast
+    MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs();
+    rollersOutputConfigs.NeutralMode = NeutralModeValue.Coast;
+    rollersOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
+
+    TalonFXConfiguration rollersConfig = new TalonFXConfiguration();
+    // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs
+    // you just created.
+    rollersConfig.Slot0 = rollersSlot0Configs;
+    rollersConfig.CurrentLimits = rollersClConfigs;
+    rollersConfig.MotorOutput = rollersOutputConfigs;
+
+    warmup1.getConfigurator().apply(rollersConfig);
+    // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and
+    // warmup3 as well.
+    warmup2.getConfigurator().apply(rollersConfig);
+    warmup3.getConfigurator().apply(rollersConfig);
+
+    Follower follower =
+        new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned);
+    // use the `setControl` method of warmup1 and 2 to configure them to use this follower
+    // configuration.
+    warmup1.setControl(follower);
+    warmup2.setControl(follower);
+
+    hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS);
+
+    // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the
+    // pid and feedforward gains found in Constants
+    Slot0Configs hoodSlot0Configs = new Slot0Configs();
+    hoodSlot0Configs.kP = Constants.Shooter.Hood.KP;
+    hoodSlot0Configs.kI = Constants.Shooter.Hood.KI;
+    hoodSlot0Configs.kD = Constants.Shooter.Hood.KD;
+    hoodSlot0Configs.kV = Constants.Shooter.Hood.KV;
+    hoodSlot0Configs.kS = Constants.Shooter.Hood.KS;
+    hoodSlot0Configs.kG = Constants.Shooter.Hood.KG;
+
+    // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with
+    // the stator and supply limits found in Constants
+    CurrentLimitsConfigs hoodCLConfigs = new CurrentLimitsConfigs();
+    hoodCLConfigs.StatorCurrentLimit = Constants.Shooter.Hood.STATOR_CURRENT_LIMIT;
+    hoodCLConfigs.SupplyCurrentLimit = Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT;
+
+    // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to
+    // treat counterclockwise as positive, and neutral mode as coast
+    MotorOutputConfigs hoodOutputConfigs = new MotorOutputConfigs();
+    hoodOutputConfigs.Inverted = InvertedValue.CounterClockwise_Positive;
+    hoodOutputConfigs.NeutralMode = NeutralModeValue.Coast;
+
+    FeedbackConfigs hoodFeedbackConfigs =
         new FeedbackConfigs()
             .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID())
             .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder)
             .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT)
             .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT);
 
-        // Create a TalonFXConfiguration called hoodConfig
-        // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above
+    // Create a TalonFXConfiguration called hoodConfig
+    TalonFXConfiguration hoodConfig = new TalonFXConfiguration();
 
-        // Apply the created configuration to the hood motor
+    // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs
+    // above
+    hoodConfig.Slot0 = hoodSlot0Configs;
+    hoodConfig.CurrentLimits = hoodCLConfigs;
+    hoodConfig.MotorOutput = hoodOutputConfigs;
+    hoodConfig.Feedback = hoodFeedbackConfigs;
 
-        MagnetSensorConfigs hoodCANcoderConfig =
+    // Apply the created configuration to the hood motor
+    hood.getConfigurator().apply(hoodConfig);
+
+    MagnetSensorConfigs hoodCANcoderConfig =
         new CANcoderConfiguration()
             .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1))
                 .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
                 .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET));
 
-        // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar between motors and other devices
-
+    // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar
+    // between motors and other devices
+    hoodEncoder.getConfigurator().apply(hoodCANcoderConfig);
+  }
+
+  public void setHoodAngle(double degrees) {
+    this.targetHoodAngle = degrees;
+    hood.setControl(positionRequest);
+    double clampedDegrees =
+        MathUtil.clamp(
+            degrees,
+            Constants.Shooter.Hood.MIN_HOOD_POSITION,
+            Constants.Shooter.Hood.MAX_HOOD_POSITION);
+    double targetRotations = clampedDegrees / 360;
+  }
+
+  public void setShooterSpeed(double velocityRps) {
+    this.targetRollerSpeed = velocityRps;
+    shooter.setControl(
+        velocityRequest.withVelocity(
+            velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT));
+  }
+
+  public void stopShooter() {
+    targetRollerSpeed = 0.0;
+    shooter.stopMotor();
+  }
+
+  public boolean isShooterAtSpeed() {
+    double currentShooterSpeed =
+        shooter.getCachedVelocityRps() * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT;
+    if (currentShooterSpeed - 1 <= targetRollerSpeed
+        || currentShooterSpeed + 1 >= targetRollerSpeed) {
+      return false;
     }
+    return true;
+  }
+
+  public Command shootWithHood(Double targetRollerSpeed, Double targetHoodAngle) {
+    return runEnd(
+        () -> {
+          setShooterSpeed(targetRollerSpeed);
+          setHoodAngle(targetHoodAngle);
+        },
+        this::stopShooter);
+  }
 }