Shooter stuff

RegisJesuitRobotics · Feb 18, 2024 · 98b86d2 · 98b86d2
1 parent add8c1b
commit 98b86d2
Showing 1 changed file with 8 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
@@ -31,12 +31,13 @@ public class ShooterSubsystem extends SubsystemBase {
       new TunableTelemetryPIDController("/shooter/pid", SHOOTER_PID_GAINS);
   private final SimpleMotorFeedforward feedforward = SHOOTER_FF_GAINS.createFeedforward();
 
-  private final DoubleTelemetryEntry topFlyVoltageReq =
+  private final DoubleTelemetryEntry flyVoltageReq =
       new DoubleTelemetryEntry("/shooter/topVoltage", false);
   private final EventTelemetryEntry shooterEventEntry = new EventTelemetryEntry("/shooter/events");
 
   public ShooterSubsystem() {
     configMotor();
+    setDefaultCommand(setVoltageCommand(0.0));
   }
 
   public void configMotor() {
@@ -82,18 +83,17 @@ public void configMotor() {
     flywheelMotorAlert.set(faultInitializing);
   }
 
-  public void setFlyVoltage(double voltage) {
+  public void setVoltage(double voltage) {
+    flyVoltageReq.append(voltage);
     flywheelMotor.setVoltage(voltage);
   }
 
-  public void setRPM(double rpm) {
-    double forwardVol = feedforward.calculate(rpm);
-
-    setFlyVoltage(forwardVol);
+  public Command setVoltageCommand(double voltage) {
+    return this.run(() -> setVoltage(voltage));
   }
 
-  public Command runRPMCommand(double RPM) {
-    return this.run(() -> this.setRPM(RPM));
+  public Command runVelocityCommand(double setpointRotationsPerSecond) {
+    return this.run(() -> setVoltage(pidController.calculate(flywheelEncoder.getVelocity(), setpointRotationsPerSecond) + feedforward.calculate(setpointRotationsPerSecond)));
   }
 
   @Override