Creates PID for Shooter

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

@@ -8,7 +8,6 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
-import frc.robot.rhr.RHRPIDFFController;
 import frc.robot.subsystems.swerveIO.module.ModuleInfo;
 import frc.robot.subsystems.swerveIO.module.SwerveModuleName;
 import frc.robot.subsystems.visionIO.VisionInfo;
@@ -134,6 +133,7 @@ public static final class ElevatorConstants {
     public static final double STARTING_HEIGHT_METERS = Units.inchesToMeters(2);
     public static final boolean SIMULATE_GRAVITY = true;
     public static final int ELEVATOR_CURRENT_LIMIT = 30;
+    public static final double FLOOR_TO_ELEVATOR_BASE_METRES = 0.0;
   }
 
   public static final class SuperStructure {
@@ -169,14 +169,8 @@ public static final class ShooterConstants {
     public static final double RADIUS_METERS = Units.inchesToMeters(2);
     public static final double MASS_KG = 0.83461;
     public static final double MOI = 0.001;
-    public static final RHRPIDFFController MOTOR_GAINS =
-        PIDFFGains.builder()
-            .name("Shooter Controller")
-            .kP(0.030)
-            .kD(0.0)
-            .kG(0.0)
-            .build()
-            .createRHRController();
+    public static final PIDFFGains SHOOTER_GAINS =
+        PIDFFGains.builder().name("Shooter Controller").kP(0.0).kD(0.0).build();
   }
 
   @UtilityClass

src/main/java/frc/robot/commands/otf/RotateScore.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
 import frc.robot.Robot;
 import frc.robot.subsystems.elevatorIO.Elevator;
 import frc.robot.util.InterpolatingTreeMap;
@@ -50,11 +51,18 @@ public static Command optimalShoot() {
 
   public static Rotation2d getOptimalShooterAngle(Pose2d position, double elevatorHeight) {
     var distance = position.getTranslation().getDistance(Translation3dTo2d(speakerLoc));
-    var FLOOR_TO_ELEVATOR_BASE_METRES = 0.0;
-    Logger.recordOutput("OTF/Offset Speaker Height", speakerLoc.getZ() - FLOOR_TO_ELEVATOR_BASE_METRES - Units.inchesToMeters(elevatorHeight));
+    Logger.recordOutput(
+        "OTF/Offset Speaker Height",
+        speakerLoc.getZ()
+            - Constants.ElevatorConstants.FLOOR_TO_ELEVATOR_BASE_METRES
+            - Units.inchesToMeters(elevatorHeight));
     Logger.recordOutput("OTF/DIST", distance);
     return new Rotation2d(
-        Math.atan((speakerLoc.getZ() - FLOOR_TO_ELEVATOR_BASE_METRES - elevatorHeight) / distance));
+        Math.atan(
+            (speakerLoc.getZ()
+                    - Constants.ElevatorConstants.FLOOR_TO_ELEVATOR_BASE_METRES
+                    - elevatorHeight)
+                / distance));
   }
 
   private static InterpolatingTreeMap<Double, Double> heights =

src/main/java/frc/robot/subsystems/elevatorIO/Elevator.java

@@ -84,7 +84,8 @@ public static class Commands {
 
     public static Command setToHeightAndWait(double targetHeightInches) {
       return new SequentialCommandGroup(
-          setToHeight(targetHeightInches), new WaitUntilCommand(() -> Robot.elevator.atTargetHeight()));
+          setToHeight(targetHeightInches),
+          new WaitUntilCommand(() -> Robot.elevator.atTargetHeight()));
     }
 
     public static Command setToHeight(double height) {

src/main/java/frc/robot/subsystems/shooterIO/Shooter.java

@@ -1,14 +1,10 @@
 package frc.robot.subsystems.shooterIO;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
 import frc.robot.Robot;
-import frc.robot.rhr.RHRPIDFFController;
 import frc.robot.util.LoggableMotor;
 import frc.robot.util.SuperStructureBuilder;
 import org.littletonrobotics.junction.Logger;
@@ -19,17 +15,11 @@ public class Shooter extends SubsystemBase {
   private final ShooterInputsAutoLogged inputs;
   private LoggableMotor leftMotor;
   private LoggableMotor rightMotor;
-  private static final RHRPIDFFController leftFlyWheelController =
-      Constants.ShooterConstants.MOTOR_GAINS;
-  private static final RHRPIDFFController rightFlyWheelController =
-      Constants.ShooterConstants.MOTOR_GAINS;
 
   public Shooter(ShooterIO IO) {
     this.IO = IO;
     this.inputs = new ShooterInputsAutoLogged();
     this.IO.updateInputs(inputs);
-    SmartDashboard.putData(leftFlyWheelController);
-    SmartDashboard.putData(rightFlyWheelController);
     leftMotor = new LoggableMotor("LeftMotor", DCMotor.getNeoVortex(1));
     rightMotor = new LoggableMotor("rightMotor", DCMotor.getNeoVortex(1));
   }
@@ -38,45 +28,24 @@ public Shooter(ShooterIO IO) {
   public void periodic() {
     Logger.processInputs("Shooter", inputs);
     IO.updateInputs(inputs);
-    // IO.setRightMotorRPMSetPoint(rightFlyWheelTargetRPM);
-    // IO.setLeftMotorRPMSetPoint(leftFlyWheelTargetRPM);
-
-    double effortRight =
-        rightFlyWheelController.calculate(
-            inputs.rightSpeedRPM, rightFlyWheelController.getSetpoint());
-    double effortLeft =
-        rightFlyWheelController.calculate(
-            inputs.leftSpeedRPM, leftFlyWheelController.getSetpoint());
-
-    effortLeft = MathUtil.clamp(effortLeft, -12, 12);
-    effortRight = MathUtil.clamp(effortRight, -12, 12);
-
-    IO.setLeftVoltage(effortLeft);
-    IO.setRightVoltage(effortRight);
-  }
-
-  public void setLeftFlyWheelTargetRPM(double leftFlyTargetRPM) {
-    leftFlyWheelController.setSetpoint(leftFlyTargetRPM);
   }
 
-  public void setRightFLyWheelTargetRPM(double rightTargetRPM) {
-    rightFlyWheelController.setSetpoint(rightTargetRPM);
+  public void setFlyWheelTargetRPM(double targetRPM) {
+    IO.setMotorSetPoint(targetRPM);
   }
 
   public static class Commands {
     public static Command setTargetRPM(double targetRPM) {
       return new InstantCommand(
           () -> {
-            Robot.shooter.setRightFLyWheelTargetRPM(targetRPM);
-            Robot.shooter.setLeftFlyWheelTargetRPM(targetRPM);
+            Robot.shooter.setFlyWheelTargetRPM(targetRPM);
           });
     }
 
     public static Command toRPM(SuperStructureBuilder builder) {
       return new InstantCommand(
           () -> {
-            Robot.shooter.setLeftFlyWheelTargetRPM(builder.getShooterMotorSpeed());
-            Robot.shooter.setRightFLyWheelTargetRPM(builder.getShooterMotorSpeed());
+            Robot.shooter.setFlyWheelTargetRPM(builder.getShooterMotorSpeed());
           });
     }
   }

src/main/java/frc/robot/subsystems/shooterIO/ShooterIO.java

@@ -23,7 +23,5 @@ public static class ShooterInputs {
 
   public void setRightVoltage(double voltage);
 
-  // public void setLeftMotorRPMSetPoint(double rPM);
-
-  // public void setRightMotorRPMSetPoint(double rPM);
+  public void setMotorSetPoint(double setpointRPM);
 }

src/main/java/frc/robot/subsystems/shooterIO/ShooterIOSim.java

@@ -3,8 +3,11 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import frc.robot.Constants;
+import frc.robot.Constants.ShooterConstants;
+import frc.robot.rhr.RHRPIDFFController;
 
 public class ShooterIOSim implements ShooterIO {
+  RHRPIDFFController shooterController;
 
   private static final FlywheelSim leftFlyWheel =
       new FlywheelSim(
@@ -22,14 +25,15 @@ public class ShooterIOSim implements ShooterIO {
 
   private double rightVolts;
 
+  public ShooterIOSim() {
+    shooterController = ShooterConstants.SHOOTER_GAINS.createRHRController();
+  }
+
   @Override
   public void updateInputs(ShooterInputsAutoLogged inputs) {
-    // setLeftVoltage(MathUtil.clamp(0, 0, 0));
     leftFlyWheel.update(0.02);
     rightFlyWheel.update(0.02);
 
-    // inputs.leftOutputVoltage = MathUtil.clamp(leftFlyWheel.getOutput(0), -12, 12);
-    // inputs.rightOutputVoltage = MathUtil.clamp(rightFlyWheel.getOutput(0), -12, 12);
     inputs.leftOutputVoltage = this.leftVolts;
     inputs.rightOutputVoltage = this.rightVolts;
 
@@ -57,4 +61,9 @@ public void setRightVoltage(double voltage) {
     rightFlyWheel.setInputVoltage(voltage);
     this.rightVolts = voltage;
   }
+
+  @Override
+  public void setMotorSetPoint(double setpointRPM) {
+    shooterController.setSetpoint(setpointRPM);
+  }
 }

src/main/java/frc/robot/subsystems/shooterIO/ShooterIOVortex.java

@@ -1,18 +1,13 @@
 package frc.robot.subsystems.shooterIO;
 
+import com.revrobotics.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants;
+import frc.robot.Constants.ShooterConstants;
 
 public class ShooterIOVortex implements ShooterIO {
-
-  /*CANSparkFlex leftFlyWheel;
-  CANSparkFlex rightFlyWheel;
-
-  RHRPIDFFController leftFlyWheelController;
-  RHRPIDFFController rightFlyWheelController;*/
-
   private static final CANSparkFlex leftMotor =
       new CANSparkFlex(Constants.RobotMap.SHOOTER_LEFT_FLYWHEEL_ID, MotorType.kBrushless);
   private static final CANSparkFlex rightMotor =
@@ -21,6 +16,12 @@ public class ShooterIOVortex implements ShooterIO {
   public ShooterIOVortex() {
     rightMotor.setInverted(true);
     leftMotor.setInverted(false);
+
+    leftMotor.getPIDController().setP(ShooterConstants.SHOOTER_GAINS.getKP());
+    leftMotor.getPIDController().setD(ShooterConstants.SHOOTER_GAINS.getKD());
+
+    rightMotor.getPIDController().setP(ShooterConstants.SHOOTER_GAINS.getKP());
+    rightMotor.getPIDController().setD(ShooterConstants.SHOOTER_GAINS.getKD());
   }
 
   @Override
@@ -51,13 +52,9 @@ public void setRightVoltage(double voltage) {
     rightMotor.setVoltage(voltage);
   }
 
-  /*
-  public void setLeftMotorRPMSetPoint(double rPM) {
-    leftFlyWheelController.setSetpoint(rPM);
+  @Override
+  public void setMotorSetPoint(double setpointRPM) {
+    leftMotor.getPIDController().setReference(setpointRPM, ControlType.kVelocity, 0);
+    rightMotor.getPIDController().setReference(setpointRPM, ControlType.kVelocity, 0);
   }
-
-  public void setRightMotorRPMSetPoint(double rPM) {
-    rightFlyWheelController.setSetpoint(rPM);
-  }*/
-
 }