diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index f58efb1..5b12326 100644 --- 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