Shooter hw 6

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

@@ -730,6 +730,9 @@ public static final class Hood {
       public static final double MIN_HOOD_POSITION = 3.8;
       public static final double MAX_HOOD_POSITION = 18.369;
 
+      public static final double MIN_HOOD_ANGLE = 3.8;
+      public static final double MAX_HOOD_ANGLE = 18.369;
+
       public static double ENCODER_OFFSET = 0.296; // TODO
       public static final int ENCODER_PORT = 18;
 
@@ -748,14 +751,17 @@ 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 = 5.0 / 12.0;
+      ;
     }
   }
 

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

@@ -10,6 +10,7 @@
 // import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 // * KEEP FOR WIN COMMAND TESTING
 import frc.robot.commands.SwerveCommands.SwerveJoystickCommand;
@@ -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;
@@ -41,6 +43,7 @@ public class RobotContainer {
       Constants.hopperOnRobot ? new HopperSubsystem() : null;
   public final IntakeSubsystem intakeSubsystem =
       Constants.intakeOnRobot ? new IntakeSubsystem() : null;
+  public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
 
   //   private final AutoRoutines autoRoutines;
   //   private final AutoChooser autoChooser;
@@ -136,48 +139,31 @@ private void configureBindings() {
                     hopperSubsystem.runHopperUntilInterruptedCommand(
                         -Constants.Hopper.TARGET_SURFACE_SPEED_MPS)));
 
-    // * KEEP FOR WIN COMMAND
-    // joystick
-    //     .a()
-    //     .whileTrue(
-    //         new DriveToAndShoot(
-    //             () -> (new Pose2d(new Translation2d(3.0, 5.0), new Rotation2d())),
-    //             lebron,
-    //             intakeSubsystem,
-    //             hopperSubsystem,
-    //             drivetrain,
-    //             redside));
-
-    // joystick
-    //     .a()
-    //     .whileTrue(
-    //         new DriveToPose(
-    //             drivetrain,
-    //             () ->
-    //                 new Pose2d(
-    //                     new Translation2d(2.462480068206787, 2.26101016998291), new
-    // Rotation2d())));
-
-    // * KEEP FOR INTERMAP TESTING
-    // joystick
-    //     .rightTrigger()
-    //     .whileTrue(
-    //         lebron
-    //             .shootAtSpeedHoodCommand(() -> shooterSpeed, () -> hoodAngle)
-    //             .alongWith(Commands.waitUntil(lebron::isAtSpeed).andThen
-    //
-    // (hopperSubsystem.runHopperUntilInterruptedCommand().alongWith(Commands.waitSeconds(0.4).andThen(intakeSubsystem.powerRetractRollersCommand())))));
+    // Shooter
+    shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter));
+
+    joystick
+        .rightBumper()
+        .whileTrue(
+            shooterSubsystem
+                .shootWithHood(44.2, Constants.Shooter.Hood.MIN_HOOD_POSITION)
+                .alongWith(
+                    Commands.waitUntil(shooterSubsystem::isShooterAtSpeed)
+                        .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())));
+
+    joystick
+        .rightTrigger()
+        .whileTrue(
+            shooterSubsystem
+                .shootWithHood(58.2, Constants.Shooter.Hood.MAX_HOOD_POSITION)
+                .alongWith(
+                    Commands.waitUntil(shooterSubsystem::isShooterAtSpeed)
+                        .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())));
 
     secondController.intakeOverride().whileTrue(intakeSubsystem.retractIntakeCommand());
 
     // Hopper
     hopperSubsystem.setDefaultCommand(hopperSubsystem.run(hopperSubsystem::stop));
-
-    // * KEEP FOR INTERMAP TESTING
-    // joystick.x().onTrue(new InstantCommand(() -> hoodAngle+=0.2));
-    // joystick.y().onTrue(new InstantCommand(() -> hoodAngle-=0.2));
-    // joystick.a().onTrue(new InstantCommand(() -> shooterSpeed+=0.5));
-    // joystick.b().onTrue(new InstantCommand(() -> shooterSpeed-=0.5));
   }
 
   public static boolean isRedAlliance() {

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

@@ -0,0 +1,16 @@
+package frc.robot.commandGroups.ShootCommandGroups;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class ShootBasicHood extends ParallelCommandGroup {
+  public ShootBasicHood(
+      double speed, double angle, ShooterSubsystem shooter, HopperSubsystem hopper) {
+    addCommands(
+        shooter.shootWithHood(speed, angle),
+        Commands.waitUntil(shooter::isShooterAtSpeed)
+            .andThen(hopper.runHopperUntilInterruptedCommand()));
+  }
+}

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

@@ -1,88 +1,165 @@
 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 MotorOutputConfigs 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 MotorOutputConfigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast
-
-        FeedbackConfigs hoodFeedbackConfigs =
+  private final LoggedTalonFX warmup1;
+  private final LoggedTalonFX warmup2;
+  private final LoggedTalonFX warmup3;
+  private final LoggedTalonFX shooter;
+  private final LoggedTalonFX hood;
+  private final CANcoder hoodEncoder;
+
+  private final VelocityVoltage velocityVoltage = new VelocityVoltage(0.0);
+  private final PositionVoltage positionVoltage = new PositionVoltage(0.0);
+  private double targetShooterSpeed = 0.0;
+  private double targetHoodAngle = 0.0;
+
+  public void setHoodAngle(double degrees) {
+    targetHoodAngle = degrees;
+    hood.setControl(
+        positionVoltage.withPosition(
+            MathUtil.clamp(
+                    degrees,
+                    Constants.Shooter.Hood.MIN_HOOD_ANGLE,
+                    Constants.Shooter.Hood.MAX_HOOD_ANGLE)
+                / 360.0));
+  }
+
+  public void setShooterSpeed(double velocityRps) {
+    targetShooterSpeed = velocityRps;
+    shooter.setControl(
+        velocityVoltage.withVelocity(
+            velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT));
+  }
+
+  public void stopShooter() {
+    targetShooterSpeed = 0.0;
+    shooter.stopMotor();
+  }
+
+  public boolean isShooterAtSpeed() {
+    double currentWheelSpeed =
+        shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT;
+    return MathUtil.isNear(targetShooterSpeed, currentWheelSpeed, 1.0);
+  }
+
+  public Command shootWithHood(double shooterSpeedRps, double hoodAngle) {
+    return runEnd(
+        () -> {
+          setShooterSpeed(shooterSpeedRps);
+          setHoodAngle(hoodAngle);
+        },
+        this::stopShooter);
+  }
+
+  public ShooterSubsystem() {
+    CANBus canbus = Constants.Swerve.CAN_BUS;
+
+    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;
+    hood = new LoggedTalonFX("Hood", Constants.Shooter.Hood.HOOD_ID, canbus);
+
+    Follower follower =
+        new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned);
+    warmup1.setControl(follower);
+    warmup2.setControl(follower);
+
+    hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, canbus);
+
+    // Rollers Configuration
+    Slot0Configs rollersSlot0Configs = new Slot0Configs();
+    rollersSlot0Configs.kP = Constants.Shooter.Rollers.KP;
+    rollersSlot0Configs.kI = Constants.Shooter.Rollers.KI;
+    rollersSlot0Configs.kD = Constants.Shooter.Rollers.KD;
+    rollersSlot0Configs.kV = Constants.Shooter.Rollers.KV;
+    rollersSlot0Configs.kS = Constants.Shooter.Rollers.KS;
+
+    CurrentLimitsConfigs rollersCLConfigs = new CurrentLimitsConfigs();
+    rollersCLConfigs.StatorCurrentLimit = Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT;
+    rollersCLConfigs.StatorCurrentLimitEnable = true;
+    rollersCLConfigs.SupplyCurrentLimit = Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT;
+    rollersCLConfigs.SupplyCurrentLimitEnable = true;
+
+    MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs();
+    rollersOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
+    rollersOutputConfigs.NeutralMode = NeutralModeValue.Coast;
+
+    TalonFXConfiguration rollersConfig = new TalonFXConfiguration();
+    rollersConfig.Slot0 = rollersSlot0Configs;
+    rollersConfig.CurrentLimits = rollersCLConfigs;
+    rollersConfig.MotorOutput = rollersOutputConfigs;
+
+    warmup1.getConfigurator().apply(rollersConfig);
+    warmup2.getConfigurator().apply(rollersConfig);
+    warmup3.getConfigurator().apply(rollersConfig);
+
+    // Hood Configuration
+    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;
+
+    CurrentLimitsConfigs hoodClConfigs = new CurrentLimitsConfigs();
+    hoodClConfigs.StatorCurrentLimit = Constants.Shooter.Hood.STATOR_CURRENT_LIMIT;
+    hoodClConfigs.StatorCurrentLimitEnable = true;
+    hoodClConfigs.SupplyCurrentLimit = Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT;
+    hoodClConfigs.SupplyCurrentLimitEnable = true;
+
+    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
+    TalonFXConfiguration hoodConfig = new TalonFXConfiguration();
+    hoodConfig.Slot0 = hoodSlot0Configs;
+    hoodConfig.CurrentLimits = hoodClConfigs;
+    hoodConfig.MotorOutput = hoodOutputConfigs;
+    hoodConfig.Feedback = hoodFeedbackConfigs;
 
-        // Apply the created configuration to the hood motor
+    hood.getConfigurator().apply(hoodConfig);
 
-        MagnetSensorConfigs hoodCANcoderConfig =
+    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
-
-    }
+    hoodEncoder.getConfigurator().apply(hoodCANcoderConfig);
+  }
 }