From 98a55c5164ca20a7bb6add1644d9fed0688797cd Mon Sep 17 00:00:00 2001 From: suryatho Date: Sat, 20 Jan 2024 18:38:46 -0500 Subject: [PATCH] Add Prototype shooter test base --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 47 +++--- .../frc/robot/subsystems/drive/Drive.java | 1 - .../subsystems/drive/ModuleIOSparkMax.java | 67 ++++---- .../frc/robot/subsystems/shooter/Shooter.java | 105 ++++++++++++ .../subsystems/shooter/ShooterConstants.java | 23 +++ .../robot/subsystems/shooter/ShooterIO.java | 57 +++++++ .../subsystems/shooter/ShooterIOSim.java | 109 ++++++++++++ .../subsystems/shooter/ShooterIOSparkMax.java | 158 ++++++++++++++++++ .../frc/robot/util/LoggedTunableNumber.java | 1 + 10 files changed, 514 insertions(+), 60 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1233ebde..c373cda4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,9 +27,9 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.SIMBOT; - public static final boolean tuningMode = false; - public static final boolean characterizationMode = false; + private static RobotType robotType = RobotType.RAINBOWT; + public static final boolean tuningMode = true; + public static final boolean characterizationMode = true; private static boolean invalidRobotAlertSent = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 448896b7..1c145e14 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,10 @@ import frc.robot.commands.FeedForwardCharacterization; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.kitbotshooter.*; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterIO; +import frc.robot.subsystems.shooter.ShooterIOSim; +import frc.robot.subsystems.shooter.ShooterIOSparkMax; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; @@ -34,7 +38,6 @@ import java.util.Optional; import java.util.function.Function; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -48,15 +51,13 @@ public class RobotContainer { // Subsystems private final Drive drive; - private final KitbotShooter shooter; + private final Shooter shooter; // Controller private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs private final LoggedDashboardChooser autoChooser; - private final LoggedDashboardNumber flywheelSpeedInput = - new LoggedDashboardNumber("Flywheel Speed", 1500.0); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -72,7 +73,7 @@ public RobotContainer() { new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); - shooter = new KitbotShooter(new KitbotFlywheelIO() {}, new KitbotFeederIO() {}); + shooter = new Shooter(new ShooterIOSparkMax()); } } } @@ -85,7 +86,7 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.moduleConfigs[1]), new ModuleIOSim(DriveConstants.moduleConfigs[2]), new ModuleIOSim(DriveConstants.moduleConfigs[3])); - shooter = new KitbotShooter(new KitbotFlywheelIOSim(), new KitbotFeederIOSim()); + shooter = new Shooter(new ShooterIOSim()); } default -> { // Replayed robot, disable IO implementations @@ -96,7 +97,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - shooter = new KitbotShooter(new KitbotFlywheelIO() {}, new KitbotFeederIO() {}); + shooter = new Shooter(new ShooterIO() {}); } } @@ -108,13 +109,21 @@ public RobotContainer() { new FeedForwardCharacterization( drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity)); autoChooser.addOption( - "Flywheel FF Characterization", - Commands.sequence( - Commands.runOnce( - () -> shooter.setCurrentMode(KitbotShooter.Mode.CHARACTERIZING), shooter), - new FeedForwardCharacterization( - shooter, shooter::runFlywheelVolts, shooter::getFlywheelVelocityRadPerSec), - Commands.runOnce(() -> shooter.setCurrentMode(null)))); + "Left Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runLeftCharacterizationVolts, + shooter::getLeftCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + autoChooser.addOption( + "Right Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runRightCharacterizationVolts, + shooter::getRightCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); // Testing autos paths Function> trajectoryCommandFactory = @@ -143,7 +152,8 @@ public RobotContainer() { } // Configure the button bindings - configureButtonBindings(); + // TODO: uncomment (done to disable drive movement) + // configureButtonBindings(); } /** @@ -175,17 +185,12 @@ private void configureButtonBindings() { drive) .ignoringDisable(true)); controller - .button(8) + .y() .onTrue( Commands.runOnce( () -> RobotState.getInstance() .resetPose(new Pose2d(), drive.getWheelPositions(), drive.getGyroYaw()))); - controller - .a() - .whileTrue( - Commands.run(() -> shooter.runFlywheelVelocity(flywheelSpeedInput.get()), shooter)) - .whileFalse(Commands.run(() -> shooter.runFlywheelVelocity(0.0), shooter)); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index dd61ae77..2f625660 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,7 +13,6 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.*; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index e093ce4e..34ae5562 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -16,7 +16,6 @@ import static frc.robot.subsystems.drive.DriveConstants.*; import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; @@ -34,8 +33,8 @@ public class ModuleIOSparkMax implements ModuleIO { // Every 100 times through periodic (2 secs) we reset relative encoder to absolute encoder private static final int reSeedIterations = 100; - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; + private final CANSparkMax driveMotor; + private final CANSparkMax turnMotor; private final RelativeEncoder driveEncoder; private final RelativeEncoder turnRelativeEncoder; private final AnalogInput turnAbsoluteEncoder; @@ -56,24 +55,24 @@ public class ModuleIOSparkMax implements ModuleIO { public ModuleIOSparkMax(ModuleConfig config) { // Init motor & encoder objects - driveSparkMax = new CANSparkMax(config.driveID(), CANSparkLowLevel.MotorType.kBrushless); - turnSparkMax = new CANSparkMax(config.turnID(), CANSparkLowLevel.MotorType.kBrushless); + driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); + turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); absoluteEncoderOffset = config.absoluteEncoderOffset(); - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); + driveEncoder = driveMotor.getEncoder(); + turnRelativeEncoder = turnMotor.getEncoder(); - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); + driveMotor.restoreFactoryDefaults(); + turnMotor.restoreFactoryDefaults(); + driveMotor.setCANTimeout(250); + turnMotor.setCANTimeout(250); for (int i = 0; i < 4; i++) { - turnSparkMax.setInverted(config.turnMotorInverted()); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); + turnMotor.setInverted(config.turnMotorInverted()); + driveMotor.setSmartCurrentLimit(40); + turnMotor.setSmartCurrentLimit(30); + driveMotor.enableVoltageCompensation(12.0); + turnMotor.enableVoltageCompensation(12.0); driveEncoder.setPosition(0.0); driveEncoder.setMeasurementPeriod(10); @@ -90,16 +89,14 @@ public ModuleIOSparkMax(ModuleConfig config) { turnRelativeEncoder.setVelocityConversionFactor( 2 * Math.PI / (60.0 * moduleConstants.turnReduction())); - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); + driveMotor.setCANTimeout(0); + turnMotor.setCANTimeout(0); - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - if (driveSparkMax.burnFlash().equals(REVLibError.kOk) - && turnSparkMax.burnFlash().equals(REVLibError.kOk)) break; + if (driveMotor.burnFlash().equals(REVLibError.kOk) + && turnMotor.burnFlash().equals(REVLibError.kOk)) break; } absoluteEncoderValue = @@ -127,14 +124,14 @@ public ModuleIOSparkMax(ModuleConfig config) { public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = driveEncoder.getPosition(); inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; + inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); + inputs.driveCurrentAmps = new double[] {driveMotor.getOutputCurrent()}; inputs.turnAbsolutePosition = absoluteEncoderValue.get(); inputs.turnPosition = Rotation2d.fromRadians(turnRelativeEncoder.getPosition()); inputs.turnVelocityRadPerSec = turnRelativeEncoder.getVelocity(); - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnMotor.getOutputCurrent()}; inputs.odometryDrivePositionsMeters = drivePositionQueue.stream().mapToDouble(rads -> rads * wheelRadius).toArray(); @@ -158,7 +155,7 @@ public void periodic() { // Run closed loop turn control if (angleSetpoint != null) { - turnSparkMax.setVoltage( + turnMotor.setVoltage( turnFeedback.calculate( absoluteEncoderValue.get().getRadians(), angleSetpoint.getRadians())); // Run closed loop drive control @@ -166,7 +163,7 @@ public void periodic() { // Scale velocity based on turn error double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); double velocityRadPerSec = adjustSpeedSetpoint / wheelRadius; - driveSparkMax.setVoltage( + driveMotor.setVoltage( driveFeedforward.calculate(velocityRadPerSec) + driveFeedback.calculate(driveEncoder.getVelocity(), velocityRadPerSec)); } @@ -184,23 +181,23 @@ public void runCharacterization(double volts) { angleSetpoint = new Rotation2d(); speedSetpoint = null; - driveSparkMax.setVoltage(volts); + driveMotor.setVoltage(volts); } @Override public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); } @Override public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); } @Override public void stop() { - driveSparkMax.setVoltage(0.0); - turnSparkMax.setVoltage(0.0); + driveMotor.setVoltage(0.0); + turnMotor.setVoltage(0.0); speedSetpoint = null; angleSetpoint = null; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 00000000..e0577aa0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,105 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.LoggedTunableNumber; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +public class Shooter extends SubsystemBase { + private static LoggedTunableNumber feedVolts = new LoggedTunableNumber("Shooter/FeedVolts", 6.0); + private static LoggedTunableNumber kP = new LoggedTunableNumber("Shooter/kP"); + private static LoggedTunableNumber kI = new LoggedTunableNumber("Shooter/kI"); + private static LoggedTunableNumber kD = new LoggedTunableNumber("Shooter/kD"); + private static LoggedTunableNumber kS = new LoggedTunableNumber("Shooter/kS"); + private static LoggedTunableNumber kV = new LoggedTunableNumber("Shooter/kV"); + private static LoggedTunableNumber kA = new LoggedTunableNumber("Shooter/kA"); + private static LoggedTunableNumber shooterTolerance = + new LoggedTunableNumber("Shooter/ToleranceRPM"); + + private final LoggedDashboardNumber leftSpeedRpm = + new LoggedDashboardNumber("Left Speed RPM", 1500.0); + private final LoggedDashboardNumber rightSpeedRpm = + new LoggedDashboardNumber("Right Speed RPM", 1500.0); + + static { + kP.initDefault(ShooterConstants.kP); + kI.initDefault(ShooterConstants.kI); + kD.initDefault(ShooterConstants.kD); + kS.initDefault(ShooterConstants.kS); + kV.initDefault(ShooterConstants.kV); + kA.initDefault(ShooterConstants.kA); + shooterTolerance.initDefault(ShooterConstants.shooterToleranceRPM); + } + + private ShooterIO shooterIO; + private ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); + private double feederSetpointVolts = 0.0; + + private boolean characterizing = false; + + public Shooter(ShooterIO io) { + shooterIO = io; + shooterIO.setPID(kP.get(), kI.get(), kD.get()); + shooterIO.setFF(kS.get(), kV.get(), kA.get()); + } + + @Override + public void periodic() { + // check controllers + if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) + shooterIO.setPID(kP.get(), kI.get(), kD.get()); + if (kS.hasChanged(hashCode()) || kV.hasChanged(hashCode()) || kA.hasChanged(hashCode())) + shooterIO.setFF(kS.get(), kV.get(), kA.get()); + + shooterIO.updateInputs(shooterInputs); + Logger.processInputs("Shooter", shooterInputs); + + if (DriverStation.isDisabled()) { + shooterIO.stop(); + } else { + if (!characterizing) { + shooterIO.setRPM(leftSpeedRpm.get(), rightSpeedRpm.get()); + if (leftSpeedRpm.get() > 0 && rightSpeedRpm.get() > 0 && atSetpoint()) { + feederSetpointVolts = feedVolts.get(); + } else { + feederSetpointVolts = 0; + } + shooterIO.setFeederVoltage(feederSetpointVolts); + } + } + + Logger.recordOutput("Shooter/LeftRPM", shooterInputs.leftFlywheelVelocityRPM); + Logger.recordOutput("Shooter/RightRPM", shooterInputs.rightFlywheelVelocityRPM); + Logger.recordOutput("Shooter/FeederRPM", shooterInputs.feederVelocityRPM); + } + + public void runLeftCharacterizationVolts(double volts) { + shooterIO.setLeftCharacterizationVoltage(volts); + } + + public void runRightCharacterizationVolts(double volts) { + shooterIO.setRightCharacterizationVoltage(volts); + } + + public double getLeftCharacterizationVelocity() { + return shooterInputs.leftFlywheelVelocityRPM; + } + + public void setCharacterizing(boolean characterizing) { + this.characterizing = characterizing; + } + + public double getRightCharacterizationVelocity() { + return shooterInputs.rightFlywheelVelocityRPM; + } + + @AutoLogOutput(key = "Shooter/AtSetpoint") + public boolean atSetpoint() { + return Math.abs(shooterInputs.leftFlywheelVelocityRPM - leftSpeedRpm.get()) + <= shooterTolerance.get() + && Math.abs(shooterInputs.rightFlywheelVelocityRPM - rightSpeedRpm.get()) + <= shooterTolerance.get(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 00000000..48190340 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.shooter; + +public class ShooterConstants { + // encoder / flywheelReduction = flywheel + public static double flywheelReduction = (1.0 / 2.0); + + public static int leftMotorId = 2; + public static int rightMotorId = 1; + public static int feederMotorId = 3; + + public static boolean leftMotorInverted = false; + public static boolean rightMotorInverted = false; + public static boolean feederMotorInverted = false; + + public static double kP = 0.0; + public static double kI = 0.0; + public static double kD = 0.0; + public static double kS = 0.33329; + public static double kV = 0.00083; + public static double kA = 0.0; + + public static double shooterToleranceRPM = 50.0; +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 00000000..b0743519 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterIO { + @AutoLog + class ShooterIOInputs { + public double leftFlywheelPositionRotations = 0.0; + public double leftFlywheelVelocityRPM = 0.0; + public double leftFlywheelAppliedVolts = 0.0; + public double leftFlywheelOutputCurrent = 0.0; + + public double rightFlywheelPositionRotations = 0.0; + public double rightFlywheelVelocityRPM = 0.0; + public double rightFlywheelAppliedVolts = 0.0; + public double rightFlywheelOutputCurrent = 0.0; + + public double feederVelocityRPM = 0.0; + public double feederAppliedVolts = 0.0; + public double feederOutputCurrent = 0.0; + } + + /** Update inputs */ + default void updateInputs(ShooterIOInputs inputs) {} + + default void setLeftRPM(double rpm) {} + + default void setRightRPM(double rpm) {} + + default void setRPM(double leftRpm, double rightRpm) { + setLeftRPM(leftRpm); + setRightRPM(rightRpm); + } + + default void setFeederVoltage(double volts) {} + + default void setLeftBrakeMode(boolean enabled) {} + + default void setRightBrakeMode(boolean enabled) {} + + default void setShooterBrakeMode(boolean enabled) { + setLeftBrakeMode(enabled); + setRightBrakeMode(enabled); + } + + default void setFeederBrakeMode(boolean enabled) {} + + default void setLeftCharacterizationVoltage(double volts) {} + + default void setRightCharacterizationVoltage(double volts) {} + + default void setPID(double p, double i, double d) {} + + default void setFF(double s, double v, double a) {} + + default void stop() {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java new file mode 100644 index 00000000..2fec4b5b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -0,0 +1,109 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class ShooterIOSim implements ShooterIO { + private FlywheelSim leftSim = + new FlywheelSim(DCMotor.getNeoVortex(1), ShooterConstants.flywheelReduction, 0.1); + private FlywheelSim rightSim = + new FlywheelSim(DCMotor.getNeoVortex(1), ShooterConstants.flywheelReduction, 0.1); + private FlywheelSim feederSim = new FlywheelSim(DCMotor.getAndymarkRs775_125(1), 1.0, 0.01); + + private PIDController leftController = new PIDController(0.0, 0.0, 0.0); + private PIDController rightController = new PIDController(0.0, 0.0, 0.0); + private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.0, 0.0, 0.0); + + private double leftAppliedVolts = 0.0; + private double rightAppliedVolts = 0.0; + private double feederAppliedVolts = 0.0; + + private Double leftSetpointRPM = null; + private Double rightSetpointRPM = null; + + @Override + public void updateInputs(ShooterIOInputs inputs) { + leftSim.update(0.02); + rightSim.update(0.02); + feederSim.update(0.02); + // control to setpoint + if (leftSetpointRPM != null) { + leftAppliedVolts = + leftController.calculate(leftSim.getAngularVelocityRPM(), leftSetpointRPM) + + ff.calculate(leftSetpointRPM); + leftSim.setInputVoltage(MathUtil.clamp(leftAppliedVolts, -12.0, 12.0)); + } + if (rightSetpointRPM != null) { + rightAppliedVolts = + rightController.calculate(rightSim.getAngularVelocityRPM(), rightSetpointRPM) + + ff.calculate(rightSetpointRPM); + rightSim.setInputVoltage(MathUtil.clamp(rightAppliedVolts, -12.0, 12.0)); + } + + inputs.leftFlywheelPositionRotations += + Units.radiansToRotations(leftSim.getAngularVelocityRadPerSec() * 0.02); + inputs.leftFlywheelVelocityRPM = leftSim.getAngularVelocityRPM(); + inputs.leftFlywheelAppliedVolts = leftAppliedVolts; + inputs.leftFlywheelOutputCurrent = leftSim.getCurrentDrawAmps(); + + inputs.rightFlywheelPositionRotations += + Units.radiansToRotations(rightSim.getAngularVelocityRadPerSec() * 0.02); + inputs.rightFlywheelVelocityRPM = rightSim.getAngularVelocityRPM(); + inputs.rightFlywheelAppliedVolts = rightAppliedVolts; + inputs.rightFlywheelOutputCurrent = rightSim.getCurrentDrawAmps(); + + inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); + inputs.feederAppliedVolts = feederAppliedVolts; + inputs.feederOutputCurrent = feederSim.getCurrentDrawAmps(); + } + + @Override + public void setLeftRPM(double rpm) { + leftSetpointRPM = rpm; + } + + @Override + public void setRightRPM(double rpm) { + rightSetpointRPM = rpm; + } + + @Override + public void setFeederVoltage(double volts) { + feederAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + } + + @Override + public void setLeftCharacterizationVoltage(double volts) { + leftSetpointRPM = null; + leftAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + leftSim.setInputVoltage(leftAppliedVolts); + } + + @Override + public void setRightCharacterizationVoltage(double volts) { + rightSetpointRPM = null; + rightAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + rightSim.setInputVoltage(rightAppliedVolts); + } + + @Override + public void setPID(double p, double i, double d) { + leftController.setPID(p, i, d); + rightController.setPID(p, i, d); + } + + @Override + public void setFF(double s, double v, double a) { + ff = new SimpleMotorFeedforward(s, v, a); + } + + @Override + public void stop() { + setRPM(0.0, 0.0); + setFeederVoltage(0.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java new file mode 100644 index 00000000..4f89e9fe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -0,0 +1,158 @@ +package frc.robot.subsystems.shooter; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + +public class ShooterIOSparkMax implements ShooterIO { + private CANSparkFlex leftMotor; + private CANSparkFlex rightMotor; + private CANSparkFlex feederMotor; + + private RelativeEncoder leftEncoder; + private RelativeEncoder rightEncoder; + + private SparkPIDController leftController; + private SparkPIDController rightController; + private SimpleMotorFeedforward flywheelFF = new SimpleMotorFeedforward(0.0, 0.0, 0.0); + + public ShooterIOSparkMax() { + leftMotor = new CANSparkFlex(ShooterConstants.leftMotorId, CANSparkFlex.MotorType.kBrushless); + rightMotor = new CANSparkFlex(ShooterConstants.rightMotorId, CANSparkFlex.MotorType.kBrushless); + feederMotor = + new CANSparkFlex(ShooterConstants.feederMotorId, CANSparkFlex.MotorType.kBrushless); + + leftEncoder = leftMotor.getEncoder(); + rightEncoder = rightMotor.getEncoder(); + + leftMotor.restoreFactoryDefaults(); + rightMotor.restoreFactoryDefaults(); + feederMotor.restoreFactoryDefaults(); + + leftMotor.setInverted(ShooterConstants.leftMotorInverted); + rightMotor.setInverted(ShooterConstants.rightMotorInverted); + leftMotor.setSmartCurrentLimit(60); + rightMotor.setSmartCurrentLimit(60); + leftMotor.enableVoltageCompensation(12.0); + rightMotor.enableVoltageCompensation(12.0); + + feederMotor.setInverted(ShooterConstants.feederMotorInverted); + feederMotor.setSmartCurrentLimit(100); + feederMotor.enableVoltageCompensation(12.0); + + setShooterBrakeMode(false); + setFeederBrakeMode(false); + + leftEncoder.setPosition(0.0); + rightEncoder.setPosition(0.0); + // leftEncoder.setMeasurementPeriod(10); + // rightEncoder.setMeasurementPeriod(10); + // leftEncoder.setAverageDepth(2); + // rightEncoder.setAverageDepth(2); + + // rotations, rps + leftEncoder.setPositionConversionFactor(1.0 / ShooterConstants.flywheelReduction); + rightEncoder.setPositionConversionFactor(1.0 / ShooterConstants.flywheelReduction); + leftEncoder.setVelocityConversionFactor(1.0 / ShooterConstants.flywheelReduction); + rightEncoder.setVelocityConversionFactor(1.0 / ShooterConstants.flywheelReduction); + + leftController = leftMotor.getPIDController(); + rightController = rightMotor.getPIDController(); + leftController.setFeedbackDevice(leftEncoder); + rightController.setFeedbackDevice(rightEncoder); + + leftMotor.burnFlash(); + rightMotor.burnFlash(); + feederMotor.burnFlash(); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.leftFlywheelPositionRotations = leftEncoder.getPosition(); + inputs.leftFlywheelVelocityRPM = leftEncoder.getVelocity(); + inputs.leftFlywheelAppliedVolts = leftMotor.getAppliedOutput(); + inputs.leftFlywheelOutputCurrent = leftMotor.getOutputCurrent(); + + inputs.rightFlywheelPositionRotations = rightEncoder.getPosition(); + inputs.rightFlywheelVelocityRPM = rightEncoder.getVelocity(); + inputs.rightFlywheelAppliedVolts = rightMotor.getAppliedOutput(); + inputs.rightFlywheelOutputCurrent = rightMotor.getOutputCurrent(); + + inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity(); + inputs.feederAppliedVolts = feederMotor.getAppliedOutput(); + inputs.feederOutputCurrent = feederMotor.getOutputCurrent(); + } + + @Override + public void setLeftRPM(double rpm) { + leftController.setReference( + rpm, + CANSparkBase.ControlType.kVelocity, + 0, + flywheelFF.calculate(rpm), + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setRightRPM(double rpm) { + rightController.setReference( + rpm, + CANSparkBase.ControlType.kVelocity, + 0, + flywheelFF.calculate(rpm), + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setFeederVoltage(double volts) { + feederMotor.setVoltage(volts); + } + + @Override + public void setLeftCharacterizationVoltage(double volts) { + leftMotor.setVoltage(volts); + } + + @Override + public void setRightCharacterizationVoltage(double volts) { + rightMotor.setVoltage(volts); + } + + @Override + public void setLeftBrakeMode(boolean enabled) { + leftMotor.setIdleMode(enabled ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); + } + + @Override + public void setRightBrakeMode(boolean enabled) { + rightMotor.setIdleMode(enabled ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); + } + + @Override + public void setFeederBrakeMode(boolean enabled) { + feederMotor.setIdleMode(enabled ? CANSparkBase.IdleMode.kBrake : CANSparkBase.IdleMode.kCoast); + } + + @Override + public void setPID(double p, double i, double d) { + leftController.setP(p); + leftController.setI(i); + leftController.setD(d); + rightController.setP(p); + rightController.setI(i); + rightController.setD(d); + } + + @Override + public void setFF(double kS, double kV, double kA) { + flywheelFF = new SimpleMotorFeedforward(kS, kV, kA); + } + + @Override + public void stop() { + setRPM(0.0, 0.0); + setFeederVoltage(0.0); + } +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java index 20958122..07d6832c 100644 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -75,6 +75,7 @@ public double get() { * otherwise. */ public boolean hasChanged(int id) { + if (!Constants.tuningMode) return false; double currentValue = get(); Double lastValue = lastHasChangedValues.get(id); if (lastValue == null || currentValue != lastValue) {