From 95801012aaa64c4689f6a598595c0099e698e30d Mon Sep 17 00:00:00 2001 From: Maciej Wiczynski Date: Tue, 30 Jan 2024 21:34:37 -0500 Subject: [PATCH] Address some review comments --- .../frc/robot/subsystems/shooter/Shooter.java | 3 +- .../robot/subsystems/shooter/ShooterIO.java | 3 +- .../subsystems/shooter/ShooterIOSparkMax.java | 32 +++++++++++++++---- .../subsystems/shooter/ShooterSubsystem.java | 14 ++++---- 4 files changed, 37 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 15c42a1c..cd32d728 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -14,7 +14,8 @@ public default void enable() {} */ public default void runVoltage(double volts) {} - public default void runVelocity(double setPoint) {} + /** Run closed loop at the specified velocity */ + public default void runVelocity(double velocityRPM) {} public double getCurrentSpeed(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 3f94b058..1f286841 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -18,7 +18,8 @@ public default void updateInputs(ShooterIOInputs inputs) {} /** Run open loop at the specified voltage. */ public default void setVoltage(double volts) {} - public default void setVelocity(double setPoint, double ff) {} + /** Run closed loop at the specified velocity */ + public default void setVelocity(double velocityRadPerSec, double ffVolts) {} public default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index da892d8d..237b3e63 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -14,9 +15,13 @@ public class ShooterIOSparkMax implements ShooterIO { // define the 2 SparkMax Controllers. A top, and a bottom private final CANSparkMax top = new CANSparkMax(2, MotorType.kBrushless); private final CANSparkMax bottom = new CANSparkMax(1, MotorType.kBrushless); + private SparkPIDController topPid = top.getPIDController(); private SparkPIDController bottomPid = bottom.getPIDController(); + public double tkP, tkI, tkD, tkIz, tkFF, tkMaxOutput, tkMinOutput, tmaxRPS; + + // TODO: probably remove bottom since shooter will have one motor, not two independent motors public double bkP, bkI, bkD, bkIz, bkFF, bkMaxOutput, bkMinOutput, bmaxRPS; // Gets the NEO encoder @@ -34,6 +39,8 @@ public ShooterIOSparkMax() { top.burnFlash(); + // TODO: these values are samples picked from REV example PID code. Need to tune PID and choose + // real values. tkP = 6e-5; tkI = 0; tkD = 0; @@ -43,6 +50,7 @@ public ShooterIOSparkMax() { tkMinOutput = -1; tmaxRPS = 300; + // TODO: probably remove bottom since shooter will have one motor, not two independent motors bkP = 6e-5; bkI = 0; bkD = 0; @@ -59,6 +67,7 @@ public ShooterIOSparkMax() { topPid.setFF(tkFF); topPid.setOutputRange(tkMinOutput, tkMaxOutput); + // TODO: probably remove bottom since shooter will have one motor, not two independent motors bottomPid.setP(bkP); bottomPid.setI(bkI); bottomPid.setD(bkD); @@ -74,6 +83,8 @@ public ShooterIOSparkMax() { SmartDashboard.putNumber("Shooter/top/Max Output", tkMaxOutput); SmartDashboard.putNumber("Shooter/top/Min Output", tkMinOutput); + // TODO: probably remove this since shooter will have one motor, not two independent motors + // // SmartDashboard.putNumber("Shooter/bot/P Gain", bkP); // SmartDashboard.putNumber("Shooter/bot/I Gain", bkI); // SmartDashboard.putNumber("Shooter/bot/D Gain", bkD); @@ -111,6 +122,8 @@ public void updateInputs(ShooterIOInputs inputs) { double tmax = SmartDashboard.getNumber("Shooter/top/Max Output", 0); double tmin = SmartDashboard.getNumber("Shooter/top/Min Output", 0); + // TODO: probably remove this since shooter will have one motor, not two independent motors + // // double bp = SmartDashboard.getNumber("Shooter/bot/P Gain", 0); // double bi = SmartDashboard.getNumber("Shooter/bot/I Gain", 0); // double bd = SmartDashboard.getNumber("Shooter/bot/D Gain", 0); @@ -145,6 +158,8 @@ public void updateInputs(ShooterIOInputs inputs) { tkMaxOutput = tmax; } + // TODO: probably remove bottom since shooter will have one motor, not two independent motors + // // if ((bp != bkP)) { // bottomPid.setP(bp); // bkP = bp; @@ -173,16 +188,21 @@ public void updateInputs(ShooterIOInputs inputs) { } @Override - public void setVelocity(double setPoint, double ff) { - // topPid.setReference(setPoint, CANSparkMax.ControlType.kVelocity); + public void setVelocity(double velocityRadPerSec, double ffVolts) { + topPid.setReference( - Units.radiansPerSecondToRotationsPerMinute(setPoint) * GEAR_RATIO, - CANSparkMax.ControlType.kVelocity); - SmartDashboard.putNumber("Shooter/top/SetPoint", setPoint); + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * GEAR_RATIO, + CANSparkMax.ControlType.kVelocity, + 0, + ffVolts, + ArbFFUnits.kVoltage); + + SmartDashboard.putNumber("Shooter/top/velocityRadPerSec", velocityRadPerSec); SmartDashboard.putNumber("Shooter/top/ProcessVariable", topEncoder.getVelocity()); + // TODO: probably remove bottom since shooter will have one motor, not two independent motors + // // bottomPid.setReference(setPoint, CANSparkMax.ControlType.kVelocity); - // SmartDashboard.putNumber("Shooter/bot/SetPoint", setPoint); // SmartDashboard.putNumber("Shooter/bot/ProcessVariable", bottomEncoder.getVelocity()); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b4a46dcd..5bf7885d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -12,14 +12,14 @@ public class ShooterSubsystem extends SubsystemBase implements Shooter { private final SimpleMotorFeedforward ffModel; private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); @AutoLogOutput private double voltage; - @AutoLogOutput private double setPoint; + @AutoLogOutput private double velocityRPM; public ShooterSubsystem(ShooterIO io) { this.io = io; - // TODO: need to run sysid on shooter and get these values. + // TODO: These are sample values. Need to run sysid on shooter and get real values. ffModel = new SimpleMotorFeedforward(0.1, 0.05); voltage = 0; - setPoint = 0; + velocityRPM = 0.0; } @Override @@ -41,11 +41,11 @@ public void runVoltage(double volts) { } @Override - public void runVelocity(double setPoint) { - this.setPoint = setPoint; - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(setPoint); + public void runVelocity(double velocityRPM) { + this.velocityRPM = velocityRPM; + var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(setPoint, ffModel.calculate(velocityRadPerSec)); + io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); } @Override