From 5cfe694be8cb02648132caeff3aaaeeb5308c3cc Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Fri, 26 Jan 2024 21:03:16 -0500 Subject: [PATCH 1/3] Bringup of shooter/intake --- src/main/java/frc/robot/RobotContainer.java | 39 +++++++++++-------- .../subsystems/intake/IntakeIOSparkMax.java | 4 +- .../subsystems/intake/IntakeIOTalonSRX.java | 33 ++++++++++++++++ .../robot/subsystems/shooter/ShooterIO.java | 7 +++- .../subsystems/shooter/ShooterIOSim.java | 4 +- .../subsystems/shooter/ShooterIOSparkMax.java | 35 ++++++++++------- .../subsystems/shooter/ShooterSubsystem.java | 4 +- 7 files changed, 88 insertions(+), 38 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIOTalonSRX.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6080f156..0f768418 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,7 @@ import frc.robot.subsystems.drive.DriveSwerveYAGSL; import frc.robot.subsystems.intake.IntakeBase; import frc.robot.subsystems.intake.IntakeIOSim; -import frc.robot.subsystems.intake.IntakeIOSparkMax; +import frc.robot.subsystems.intake.IntakeIOTalonSRX; import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOSparkMax; import frc.robot.subsystems.shooter.ShooterSubsystem; @@ -29,8 +29,9 @@ public class RobotContainer { public RobotContainer() { controller = new CommandXboxController(0); - boolean hasIntake = false; - boolean hasShooter = false; + boolean hasIntake = true; + boolean hasShooter = true; + boolean hasDrive = false; if (hasShooter) { shooter = new ShooterSubsystem(new ShooterIOSparkMax()); @@ -39,12 +40,16 @@ public RobotContainer() { } if (hasIntake) { - intake = new IntakeBase(new IntakeIOSparkMax()); + intake = new IntakeBase(new IntakeIOTalonSRX()); } else { intake = new IntakeBase(new IntakeIOSim()); } - drive = new DriveSwerveYAGSL(); + if (hasDrive) { + drive = new DriveSwerveYAGSL(); + } else { + drive = null; + } configureBindings(); } @@ -59,18 +64,20 @@ private void configureBindings() { () -> controller.rightBumper().getAsBoolean(), () -> controller.leftBumper().getAsBoolean())); - drive.setDefaultCommand( - new DriveCommand( - drive, - () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.01), - () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.01), - () -> MathUtil.applyDeadband(-controller.getRightX(), 0.01))); - // TODO: Move deadband to constants file + if (drive != null) { + drive.setDefaultCommand( + new DriveCommand( + drive, + () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.01), + () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.01), + () -> MathUtil.applyDeadband(-controller.getRightX(), 0.01))); + // TODO: Move deadband to constants file - controller - .start() - .onTrue( - new InstantCommand(() -> drive.setFieldOrientedDrive(!drive.isFieldOrientedDrive()))); + controller + .start() + .onTrue( + new InstantCommand(() -> drive.setFieldOrientedDrive(!drive.isFieldOrientedDrive()))); + } } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index a7391cdf..c5eaa5e5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -8,8 +8,8 @@ public class IntakeIOSparkMax implements IntakeIO { private static final double GEAR_RATIO = 10.0; - // define the 2 SparkMax Controllers. A leader, and a follower - private final CANSparkMax leader = new CANSparkMax(2, MotorType.kBrushless); + // define the 1 SparkMax Controller + private final CANSparkMax leader = new CANSparkMax(3, MotorType.kBrushless); // Gets the NEO encoder private final RelativeEncoder encoder = leader.getEncoder(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonSRX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonSRX.java new file mode 100644 index 00000000..34f0f83b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonSRX.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.RobotController; + +public class IntakeIOTalonSRX implements IntakeIO { + // private static final double GEAR_RATIO = 10.0; + + // define the 1 SparkMax Controller + private final TalonSRX leader = new TalonSRX(3); + + public IntakeIOTalonSRX() { + // leader motor is not inverted, and set follower motor to follow the leader + leader.setInverted(false); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + // Set velocityRadPerSec to the encoder velocity(rotationsPerMinute) divided by the gear ratio + // and converted into Radians Per Second + // inputs.velocityRadPerSec = + // Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); + // Get applied voltage from the leader motor + // inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); + } + + @Override + public void setVoltage(double volts) { + // Set the voltage output for the leader motor + leader.set(ControlMode.PercentOutput, volts / RobotController.getBatteryVoltage()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 541aa72d..53a9bc2c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -5,8 +5,11 @@ public interface ShooterIO { @AutoLog public class ShooterIOInputs { - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; + public double velocityRadPerSecTop = 0.0; + public double appliedVoltsTop = 0.0; + + public double velocityRadPerSecBottom = 0.0; + public double appliedVoltsBottom = 0.0; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index e5088d81..772aa8bf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -15,8 +15,8 @@ public void updateInputs(ShooterIOInputs inputs) { sim.update(0.02); // Update inputs - inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; + inputs.velocityRadPerSecTop = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltsTop = appliedVolts; } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 07871f32..cb619608 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -7,34 +7,41 @@ public class ShooterIOSparkMax implements ShooterIO { // Gear ratio for the shooter mechanism - private static final double GEAR_RATIO = 5.0; + private static final double GEAR_RATIO = 1.0; - // define the 2 SparkMax Controllers. A leader, and a follower - private final CANSparkMax leader = new CANSparkMax(0, MotorType.kBrushless); - private final CANSparkMax follower = new CANSparkMax(1, MotorType.kBrushless); + // 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); // Gets the NEO encoder - private final RelativeEncoder encoder = leader.getEncoder(); + private final RelativeEncoder topEncoder = top.getEncoder(); + private final RelativeEncoder bottomEncoder = top.getEncoder(); public ShooterIOSparkMax() { - // leader motor is not inverted, and set follower motor to follow the leader - leader.setInverted(false); - follower.follow(leader, false); + // top motor is not inverted, and set bottom motor to follow the top + top.setInverted(false); + bottom.setInverted(false); } @Override public void updateInputs(ShooterIOInputs inputs) { // Set velocityRadPerSec to the encoder velocity(rotationsPerMinute) divided by the gear ratio // and converted into Radians Per Second - inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); - // Get applied voltage from the leader motor - inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); + inputs.velocityRadPerSecTop = + Units.rotationsPerMinuteToRadiansPerSecond(topEncoder.getVelocity() / GEAR_RATIO); + // Get applied voltage from the top motor + inputs.appliedVoltsTop = top.getAppliedOutput() * top.getBusVoltage(); + + inputs.velocityRadPerSecBottom = + Units.rotationsPerMinuteToRadiansPerSecond(bottomEncoder.getVelocity() / GEAR_RATIO); + // Get applied voltage from the top motor + inputs.appliedVoltsBottom = bottom.getAppliedOutput() * bottom.getBusVoltage(); } @Override public void setVoltage(double volts) { - // Set the voltage output for the leader motor - leader.setVoltage(volts); + // Set the voltage output for the top motor + top.setVoltage(volts); + bottom.setVoltage(volts); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 7afa4184..7aa36fb3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -35,12 +35,12 @@ public void setVoltage(double volts) { @Override public double getVoltage() { - return inputs.appliedVolts; + return inputs.appliedVoltsTop; } @Override public double getCurrentSpeed() { - return inputs.velocityRadPerSec; + return inputs.velocityRadPerSecTop; } @Override From 9160c53403a0a7cd8090787353520a999580715a Mon Sep 17 00:00:00 2001 From: Maciej Wiczynski Date: Tue, 30 Jan 2024 18:46:34 -0500 Subject: [PATCH 2/3] shooter pid work in progress --- gradlew | 0 src/main/java/frc/robot/RobotContainer.java | 13 +- .../frc/robot/commands/ShooterEnable.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 4 +- .../robot/subsystems/shooter/ShooterIO.java | 4 + .../subsystems/shooter/ShooterIOSparkMax.java | 153 +++++++++++++++++- .../subsystems/shooter/ShooterSubsystem.java | 25 ++- 7 files changed, 192 insertions(+), 9 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f768418..d0dd5943 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.shooter.ShooterIOSparkMax; import frc.robot.subsystems.shooter.ShooterSubsystem; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; public class RobotContainer { public final CommandXboxController controller; @@ -26,6 +27,9 @@ public class RobotContainer { public final IntakeBase intake; public final DriveSwerveYAGSL drive; + private final LoggedDashboardNumber shooterSpeedInput = + new LoggedDashboardNumber("Shooter Speed", 300.0); + public RobotContainer() { controller = new CommandXboxController(0); @@ -55,9 +59,16 @@ public RobotContainer() { } private void configureBindings() { - shooter.setDefaultCommand(new InstantCommand(() -> shooter.disable(), shooter)); + // shooter.setDefaultCommand(new InstantCommand(() -> shooter.disable(), shooter)); controller.rightTrigger().whileTrue(new ShooterEnable(shooter)); + + controller + .a() + .whileTrue( + Commands.startEnd( + () -> shooter.runVelocity(shooterSpeedInput.get()), shooter::disable, shooter)); + intake.setDefaultCommand( new IntakeBaseCommand( intake, diff --git a/src/main/java/frc/robot/commands/ShooterEnable.java b/src/main/java/frc/robot/commands/ShooterEnable.java index 2495b757..e9c6169a 100644 --- a/src/main/java/frc/robot/commands/ShooterEnable.java +++ b/src/main/java/frc/robot/commands/ShooterEnable.java @@ -37,7 +37,7 @@ public void initialize() {} @Override public void execute() { // Checks the volt Entry for the volt and sets the voltage of motors - shooter.setVoltage(voltsEntry.getDouble(0.0)); + shooter.runVoltage(voltsEntry.getDouble(0.0)); // Enable motors, It has to be called regularly for voltage compensation to work properly shooter.enable(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index def60b06..15c42a1c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -12,7 +12,9 @@ public default void enable() {} * * @param volts The volts to set. Value should be between -12.0 and 12.0. */ - public default void setVoltage(double volts) {} + public default void runVoltage(double volts) {} + + public default void runVelocity(double setPoint) {} 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 53a9bc2c..3f94b058 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -17,4 +17,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) {} + + 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 cb619608..da892d8d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -3,7 +3,9 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ShooterIOSparkMax implements ShooterIO { // Gear ratio for the shooter mechanism @@ -12,15 +14,79 @@ 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; + public double bkP, bkI, bkD, bkIz, bkFF, bkMaxOutput, bkMinOutput, bmaxRPS; // Gets the NEO encoder private final RelativeEncoder topEncoder = top.getEncoder(); private final RelativeEncoder bottomEncoder = top.getEncoder(); public ShooterIOSparkMax() { - // top motor is not inverted, and set bottom motor to follow the top - top.setInverted(false); + bottom.setInverted(false); + + top.setInverted(false); + + top.enableVoltageCompensation(12.0); + top.setSmartCurrentLimit(30); + + top.burnFlash(); + + tkP = 6e-5; + tkI = 0; + tkD = 0; + tkIz = 0; + tkFF = 0.000015; + tkMaxOutput = 1; + tkMinOutput = -1; + tmaxRPS = 300; + + bkP = 6e-5; + bkI = 0; + bkD = 0; + bkIz = 0; + bkFF = 0.000015; + bkMaxOutput = 1; + bkMinOutput = 0; + bmaxRPS = 300; + + topPid.setP(tkP); + topPid.setI(tkI); + topPid.setD(tkD); + topPid.setIZone(tkIz); + topPid.setFF(tkFF); + topPid.setOutputRange(tkMinOutput, tkMaxOutput); + + bottomPid.setP(bkP); + bottomPid.setI(bkI); + bottomPid.setD(bkD); + bottomPid.setIZone(bkIz); + bottomPid.setFF(bkFF); + bottomPid.setOutputRange(bkMinOutput, bkMaxOutput); + + SmartDashboard.putNumber("Shooter/top/P Gain", tkP); + SmartDashboard.putNumber("Shooter/top/I Gain", tkI); + SmartDashboard.putNumber("Shooter/top/D Gain", tkD); + SmartDashboard.putNumber("Shooter/top/I Zone", tkIz); + SmartDashboard.putNumber("Shooter/top/Feed Forward", tkFF); + SmartDashboard.putNumber("Shooter/top/Max Output", tkMaxOutput); + SmartDashboard.putNumber("Shooter/top/Min Output", tkMinOutput); + + // SmartDashboard.putNumber("Shooter/bot/P Gain", bkP); + // SmartDashboard.putNumber("Shooter/bot/I Gain", bkI); + // SmartDashboard.putNumber("Shooter/bot/D Gain", bkD); + // SmartDashboard.putNumber("Shooter/bot/I Zone", bkIz); + // SmartDashboard.putNumber("Shooter/bot/Feed Forward", bkFF); + // SmartDashboard.putNumber("Shooter/bot/Max Output", bkMaxOutput); + // SmartDashboard.putNumber("Shooter/bot/Min Output", bkMinOutput); + } + + @Override + public void stop() { + top.stopMotor(); + bottom.stopMotor(); } @Override @@ -36,6 +102,89 @@ public void updateInputs(ShooterIOInputs inputs) { Units.rotationsPerMinuteToRadiansPerSecond(bottomEncoder.getVelocity() / GEAR_RATIO); // Get applied voltage from the top motor inputs.appliedVoltsBottom = bottom.getAppliedOutput() * bottom.getBusVoltage(); + + double tp = SmartDashboard.getNumber("Shooter/top/P Gain", 0); + double ti = SmartDashboard.getNumber("Shooter/top/I Gain", 0); + double td = SmartDashboard.getNumber("Shooter/top/D Gain", 0); + double tiz = SmartDashboard.getNumber("Shooter/top/I Zone", 0); + double tff = SmartDashboard.getNumber("Shooter/top/Feed Forward", 0); + double tmax = SmartDashboard.getNumber("Shooter/top/Max Output", 0); + double tmin = SmartDashboard.getNumber("Shooter/top/Min Output", 0); + + // 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); + // double biz = SmartDashboard.getNumber("Shooter/bot/I Zone", 0); + // double bff = SmartDashboard.getNumber("Shooter/bot/Feed Forward", 0); + // double bmax = SmartDashboard.getNumber("Shooter/bot/Max Output", 0); + // double bmin = SmartDashboard.getNumber("Shooter/bot/Min Output", 0); + + if ((tp != tkP)) { + topPid.setP(tp); + tkP = tp; + } + if ((ti != tkI)) { + topPid.setI(ti); + tkI = ti; + } + if ((td != tkD)) { + topPid.setD(td); + tkD = td; + } + if ((tiz != tkIz)) { + topPid.setIZone(tiz); + tkIz = tiz; + } + if ((tff != tkFF)) { + topPid.setFF(tff); + tkFF = tff; + } + if ((tmax != tkMaxOutput) || (tmin != tkMinOutput)) { + topPid.setOutputRange(tmin, tmax); + tkMinOutput = tmin; + tkMaxOutput = tmax; + } + + // if ((bp != bkP)) { + // bottomPid.setP(bp); + // bkP = bp; + // } + // if ((bi != bkI)) { + // bottomPid.setI(bi); + // bkI = bi; + // } + // if ((bd != bkD)) { + // bottomPid.setD(bd); + // bkD = bd; + // } + // if ((biz != bkIz)) { + // bottomPid.setIZone(biz); + // bkIz = biz; + // } + // if ((bff != bkFF)) { + // bottomPid.setFF(bff); + // bkFF = bff; + // } + // if ((bmax != bkMaxOutput) || (bmin != bkMinOutput)) { + // bottomPid.setOutputRange(bmin, bmax); + // bkMinOutput = bmin; + // bkMaxOutput = bmax; + // } + } + + @Override + public void setVelocity(double setPoint, double ff) { + // topPid.setReference(setPoint, CANSparkMax.ControlType.kVelocity); + topPid.setReference( + Units.radiansPerSecondToRotationsPerMinute(setPoint) * GEAR_RATIO, + CANSparkMax.ControlType.kVelocity); + SmartDashboard.putNumber("Shooter/top/SetPoint", setPoint); + SmartDashboard.putNumber("Shooter/top/ProcessVariable", topEncoder.getVelocity()); + + // bottomPid.setReference(setPoint, CANSparkMax.ControlType.kVelocity); + + // SmartDashboard.putNumber("Shooter/bot/SetPoint", setPoint); + // SmartDashboard.putNumber("Shooter/bot/ProcessVariable", bottomEncoder.getVelocity()); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 7aa36fb3..b4a46dcd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -1,38 +1,53 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class ShooterSubsystem extends SubsystemBase implements Shooter { ShooterIO io; + private final SimpleMotorFeedforward ffModel; private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); @AutoLogOutput private double voltage; + @AutoLogOutput private double setPoint; public ShooterSubsystem(ShooterIO io) { this.io = io; + // TODO: need to run sysid on shooter and get these values. + ffModel = new SimpleMotorFeedforward(0.1, 0.05); voltage = 0; + setPoint = 0; } @Override // Disable the shooter public void disable() { - voltage = 0; - io.setVoltage(voltage); + io.stop(); } @Override // Enable the shooter public void enable() { - io.setVoltage(voltage); + // io.setVoltage(voltage); } @Override // Sets the voltage to volts. the volts value is -12 to 12 - public void setVoltage(double volts) { + public void runVoltage(double volts) { voltage = volts; } + @Override + public void runVelocity(double setPoint) { + this.setPoint = setPoint; + var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(setPoint); + + io.setVelocity(setPoint, ffModel.calculate(velocityRadPerSec)); + } + @Override public double getVoltage() { return inputs.appliedVoltsTop; @@ -48,5 +63,7 @@ public void periodic() { // Updates the inputs io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); + SmartDashboard.putNumber("Shooter/TopRPM", inputs.velocityRadPerSecTop); + SmartDashboard.putNumber("Shooter/BottomRPM", inputs.velocityRadPerSecBottom); } } From 95801012aaa64c4689f6a598595c0099e698e30d Mon Sep 17 00:00:00 2001 From: Maciej Wiczynski Date: Tue, 30 Jan 2024 21:34:37 -0500 Subject: [PATCH 3/3] 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