From ce05966aed74256175386ef45a55e1c94a7fdc12 Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 9 Feb 2024 17:41:36 -0500 Subject: [PATCH] Split feeder and flywheels --- .../frc2024/RobotContainer.java | 34 ++-- .../frc2024/subsystems/drive/Module.java | 9 +- .../subsystems/drive/ModuleIOKrakenFOC.java | 3 +- .../subsystems/drive/ModuleIOSparkMax.java | 3 +- .../trajectory/GenerateTrajectories.java | 3 +- .../superstructure/DevBotSuperstructure.java | 44 +---- .../SuperstructureConstants.java | 77 +++----- .../subsystems/superstructure/arm/Arm.java | 84 +++------ .../subsystems/superstructure/arm/ArmIO.java | 10 +- .../superstructure/arm/ArmIOKrakenFOC.java | 15 +- .../superstructure/arm/ArmIOSim.java | 10 +- .../superstructure/feeder/FeederIO.java | 21 +++ .../superstructure/feeder/FeederIOSim.java | 28 +++ .../superstructure/flywheels/Flywheels.java | 93 ++++++++++ .../superstructure/flywheels/FlywheelsIO.java | 44 +++++ .../flywheels/FlywheelsIOSim.java | 100 +++++++++++ .../flywheels/FlywheelsIOSparkFlex.java | 131 ++++++++++++++ .../superstructure/shooter/Shooter.java | 160 ----------------- .../superstructure/shooter/ShooterIO.java | 61 ------- .../superstructure/shooter/ShooterIOSim.java | 129 ------------- .../shooter/ShooterIOSparkMax.java | 169 ------------------ 21 files changed, 526 insertions(+), 702 deletions(-) create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/Shooter.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIO.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSim.java delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSparkMax.java diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 43462ad4..3c4828f9 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -35,13 +35,13 @@ import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim; +import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; +import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIO; +import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSim; +import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.superstructure.intake.Intake; import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIO; import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIOSim; -import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.Shooter; -import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.ShooterIO; -import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.ShooterIOSim; -import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.ShooterIOSparkMax; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.frc2024.util.trajectory.ChoreoTrajectoryDeserializer; import org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory; @@ -60,7 +60,7 @@ public class RobotContainer { // Subsystems private Drive drive; private AprilTagVision aprilTagVision; - private Shooter shooter; + private Flywheels flywheels; private Intake intake; private Arm arm; private DevBotSuperstructure superstructure; @@ -83,9 +83,9 @@ public RobotContainer() { new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); arm = new Arm(new ArmIOKrakenFOC()); - shooter = new Shooter(new ShooterIOSparkMax()); + flywheels = new Flywheels(new FlywheelsIOSparkFlex()); // intake = new Intake(new IntakeIOSparkMax()); - superstructure = new DevBotSuperstructure(arm, shooter); + superstructure = new DevBotSuperstructure(arm, flywheels); } case SIMBOT -> { drive = @@ -96,9 +96,9 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.moduleConfigs[2]), new ModuleIOSim(DriveConstants.moduleConfigs[3])); arm = new Arm(new ArmIOSim()); - shooter = new Shooter(new ShooterIOSim()); + flywheels = new Flywheels(new FlywheelsIOSim()); intake = new Intake(new IntakeIOSim()); - superstructure = new DevBotSuperstructure(arm, shooter); + superstructure = new DevBotSuperstructure(arm, flywheels); } case COMPBOT -> { // No impl yet @@ -119,8 +119,8 @@ public RobotContainer() { aprilTagVision = new AprilTagVision(); } - if (shooter == null) { - shooter = new Shooter(new ShooterIO() {}); + if (flywheels == null) { + flywheels = new Flywheels(new FlywheelsIO() {}); } if (intake == null) { @@ -141,15 +141,15 @@ public RobotContainer() { autoChooser.addOption( "Left Flywheels FF Characterization", new FeedForwardCharacterization( - shooter, - shooter::runLeftCharacterizationVolts, - shooter::getLeftCharacterizationVelocity)); + flywheels, + flywheels::runLeftCharacterizationVolts, + flywheels::getLeftCharacterizationVelocity)); autoChooser.addOption( "Right Flywheels FF Characterization", new FeedForwardCharacterization( - shooter, - shooter::runRightCharacterizationVolts, - shooter::getRightCharacterizationVelocity)); + flywheels, + flywheels::runRightCharacterizationVolts, + flywheels::getRightCharacterizationVelocity)); autoChooser.addOption("Arm get static current", arm.getStaticCurrent()); // Testing autos paths diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java index 8ab62ce0..eba0b484 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java @@ -1,5 +1,8 @@ package org.littletonrobotics.frc2024.subsystems.drive; +import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.driveConfig; +import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.moduleConstants; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -8,9 +11,6 @@ import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; -import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.moduleConstants; -import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.driveConfig; - public class Module { private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("Drive/Module/DrivekP", moduleConstants.drivekP()); @@ -29,8 +29,7 @@ public class Module { private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private SimpleMotorFeedforward driveFF = - new SimpleMotorFeedforward( - moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0); + new SimpleMotorFeedforward(moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0); @Getter private SwerveModuleState setpointState = new SwerveModuleState(); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java index 7407e42e..543575c3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java @@ -171,7 +171,8 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryDrivePositionsMeters = drivePositionQueue.stream() - .mapToDouble(signalValue -> Units.rotationsToRadians(signalValue) * driveConfig.wheelRadius()) + .mapToDouble( + signalValue -> Units.rotationsToRadians(signalValue) * driveConfig.wheelRadius()) .toArray(); inputs.odometryTurnPositions = turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java index 7cabef36..ec420b86 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java @@ -94,8 +94,7 @@ public ModuleIOSparkMax(ModuleConfig config) { drivePositionQueue = SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal(absoluteEncoderValue.get()::getRadians); + SparkMaxOdometryThread.getInstance().registerSignal(absoluteEncoderValue.get()::getRadians); // Init Controllers driveController = new PIDController(moduleConstants.drivekP(), 0.0, moduleConstants.drivekD()); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java index 637654a6..11354e71 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java @@ -24,7 +24,8 @@ public static void main(String[] args) { .setWheelRadius(DriveConstants.driveConfig.wheelRadius()) .setMaxWheelTorque(2) .setMaxWheelOmega( - DriveConstants.moduleLimits.maxDriveVelocity() / DriveConstants.driveConfig.wheelRadius()) + DriveConstants.moduleLimits.maxDriveVelocity() + / DriveConstants.driveConfig.wheelRadius()) .build(); PathRequest request = diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java index 3a7ac52b..f908f422 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java @@ -1,13 +1,10 @@ package org.littletonrobotics.frc2024.subsystems.superstructure; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; -import org.littletonrobotics.frc2024.FieldConstants; -import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; -import org.littletonrobotics.frc2024.subsystems.superstructure.shooter.Shooter; +import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -39,46 +36,23 @@ public enum State { @Getter private State currentState = State.IDLE; private final Arm arm; - private final Shooter shooter; + private final Flywheels flywheels; private final Timer followThroughTimer = new Timer(); private double followThroughArmAngle = 0.0; - public DevBotSuperstructure(Arm arm, Shooter shooter) { + public DevBotSuperstructure(Arm arm, Flywheels flywheels) { this.arm = arm; - this.shooter = shooter; + this.flywheels = flywheels; } @Override public void periodic() { switch (currentState) { - case IDLE -> { - arm.setSetpoint(Units.degreesToRadians(armIdleSetpointDegrees.get())); - shooter.requestRPM(idleLeftRPM.get(), idleRightRPM.get()); - } - case INTAKE -> { - arm.setSetpoint(Units.degreesToRadians(armIntakeSetpointDegrees.get())); - if (arm.atSetpoint()) { - shooter.requestIntake(); - } - } - case PREPARE_SHOOT -> { - var aimingParameters = RobotState.getInstance().getAimingParameters(); - double y = (FieldConstants.Speaker.centerSpeakerOpening.getY()) / 2.0; - y += Units.inchesToMeters(yCompensation.get()); - double x = aimingParameters.effectiveDistance(); - arm.setSetpoint(Math.atan(y / x)); - shooter.requestRPM(shootingLeftRPM.get(), shootingRightRPM.get()); - } - case SHOOT -> { - if (!atShootingSetpoint()) { - currentState = State.PREPARE_SHOOT; - } else { - shooter.requestFeed(); - followThroughArmAngle = arm.getAngle().getRadians(); - followThroughTimer.restart(); - } - } + case IDLE -> {} + case INTAKE -> {} + case PREPARE_SHOOT -> {} + case SHOOT -> {} } Logger.recordOutput("DevBotSuperstructure/currentState", currentState.toString()); @@ -88,7 +62,7 @@ public void periodic() { public boolean atShootingSetpoint() { return (currentState == State.PREPARE_SHOOT || currentState == State.SHOOT) && arm.atSetpoint() - && shooter.atSetpoint(); + && flywheels.atSetpoint(); } public void setDesiredState(State desiredState) { diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java index 9bd33842..3c7f7768 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java @@ -7,45 +7,34 @@ public class SuperstructureConstants { - public static class ShooterConstants { - // encoder / flywheelReduction = flywheel - public static double flywheelReduction = (1.0 / 2.0); - public static double shooterToleranceRPM = 50.0; + public static class FeederConstants { + public static double reduction = (1.0 / 1.0); - public static FlywheelConstants leftFlywheelConstants = + public static int id = switch (Constants.getRobot()) { - default -> new FlywheelConstants( - 5, - false, - 0.0, - 0.0, - 0.0, - 0.33329, - 0.00083, - 0.0); + default -> 6; }; - - public static FlywheelConstants rightFlywheelConstants = + public static boolean inverted = switch (Constants.getRobot()) { - default -> new FlywheelConstants(4, false, 0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0); + default -> false; }; + } - public static FeederConstants feederConstants = + public static class FlywheelConstants { + // encoder / flywheelReduction = flywheel + public static double reduction = (1.0 / 2.0); + public static double shooterToleranceRPM = 50.0; + public static int leftID = 5; + public static int rightID = 6; + + public static Gains gains = switch (Constants.getRobot()) { - default -> new FeederConstants(6, false); + case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0); + case DEVBOT -> new Gains(0.0, 0.0, 0.0, 0.33329, 0.00083, 0.0); + case COMPBOT -> null; }; - public record FlywheelConstants( - int id, - boolean inverted, - double kP, - double kI, - double kD, - double kS, - double kV, - double kA) {} - - public record FeederConstants(int id, boolean inverted) {} + public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} } public static class IntakeConstants { @@ -64,29 +53,20 @@ public static class ArmConstants { // reduction is 12:62 18:60 12:65 public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); - public static Translation2d armOrigin2d = - new Translation2d(-Units.inchesToMeters(0), Units.inchesToMeters(11.75)); - - public static Rotation2d minAngle = Rotation2d.fromDegrees(0.0); + public static Translation2d armOrigin = + new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); + public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); public static int leaderID = 25; public static int followerID = 26; public static int armEncoderID = 1; - public static boolean leaderInverted = - switch (Constants.getRobot()) { - case DEVBOT -> false; - case COMPBOT -> false; - case SIMBOT -> false; - }; + public static boolean leaderInverted = false; + public static boolean followerInverted = false; - public static boolean followerInverted = - switch (Constants.getRobot()) { - case DEVBOT -> false; - case COMPBOT -> false; - case SIMBOT -> false; - }; + /** The offset of the arm encoder in rotations. */ + public static double armEncoderOffsetRotations = 0.0; public static double armLength = switch (Constants.getRobot()) { @@ -101,10 +81,7 @@ public static class ArmConstants { case COMPBOT -> new ControllerConstants(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); }; - public static ProfileConstraints profileConstraints = - switch (Constants.getRobot()) { - default -> new ProfileConstraints(2.0 * Math.PI, 10); - }; + public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10); public record ProfileConstraints( double cruiseVelocityRadPerSec, double accelerationRadPerSec2) {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 0acd18c8..6b223463 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -2,7 +2,6 @@ import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ArmConstants.*; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -14,8 +13,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.BooleanSupplier; -import org.littletonrobotics.frc2024.util.EqualsUtil; +import lombok.Setter; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -39,42 +37,38 @@ public class Arm extends SubsystemBase { new LoggedTunableNumber("Arm/Velocity", profileConstraints.cruiseVelocityRadPerSec()); private static final LoggedTunableNumber armAcceleration = new LoggedTunableNumber("Arm/Acceleration", profileConstraints.accelerationRadPerSec2()); - private static final LoggedTunableNumber armTolerance = - new LoggedTunableNumber("Arm/Tolerance", positionTolerance.getRadians()); + private static final LoggedTunableNumber armToleranceDegreees = + new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees()); private static final LoggedTunableNumber armLowerLimit = new LoggedTunableNumber("Arm/LowerLimitDegrees", 7.0); private static final LoggedTunableNumber armUpperLimit = new LoggedTunableNumber("Arm/UpperLimitDegrees", 90.0); - private final ArmIO armIO; + private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private BooleanSupplier disableSupplier = () -> false; - private BooleanSupplier coastSupplier = () -> false; private final Mechanism2d armMechanism; private final MechanismRoot2d armRoot; private final MechanismLigament2d armMeasured; - private final Timer homingTimer = new Timer(); - private boolean homed = false; - private double setpoint = armLowerLimit.get(); + @Setter private Rotation2d setpoint = null; // private final MechanismLigament2d armSetpoint; public Arm(ArmIO io) { System.out.println("[Init] Creating Arm"); - this.armIO = io; + this.io = io; io.setBrakeMode(true); // Create a mechanism armMechanism = new Mechanism2d(2, 3, new Color8Bit(Color.kAntiqueWhite)); - armRoot = armMechanism.getRoot("Arm Joint", armOrigin2d.getX(), armOrigin2d.getY()); + armRoot = armMechanism.getRoot("Arm Joint", armOrigin.getX(), armOrigin.getY()); armMeasured = new MechanismLigament2d( "Arm Measured", armLength, - Units.radiansToDegrees(inputs.armAnglePositionRads), + Units.radiansToDegrees(inputs.armPositionRads), 2.0, new Color8Bit(Color.kBlack)); armRoot.append(armMeasured); @@ -83,74 +77,49 @@ public Arm(ArmIO io) { @Override public void periodic() { // Process inputs - armIO.updateInputs(inputs); + io.updateInputs(inputs); Logger.processInputs("Arm", inputs); // Update controllers LoggedTunableNumber.ifChanged( - hashCode(), () -> armIO.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); + hashCode(), () -> io.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); LoggedTunableNumber.ifChanged( - hashCode(), () -> armIO.setFF(kS.get(), kV.get(), kA.get(), kG.get()), kS, kV, kA, kG); + hashCode(), () -> io.setFF(kS.get(), kV.get(), kA.get(), kG.get()), kS, kV, kA, kG); LoggedTunableNumber.ifChanged( hashCode(), - constraints -> armIO.setProfileConstraints(constraints[0], constraints[1]), + constraints -> io.setProfileConstraints(constraints[0], constraints[1]), armVelocity, armAcceleration); - // Home if not already homed - if (!homed && DriverStation.isEnabled()) { - armIO.setVoltage(-1.0); - if (EqualsUtil.epsilonEquals( - inputs.armVelocityRadsPerSec, 0.0, Units.degreesToRadians(1.0))) { - homingTimer.start(); - } else { - homingTimer.reset(); - } - - if (homingTimer.hasElapsed(0.5)) { - armIO.setPosition(Units.degreesToRadians(armLowerLimit.get())); - homed = true; - } - } else { - setpoint = - MathUtil.clamp( - setpoint, - Units.degreesToRadians(armLowerLimit.get()), - Units.degreesToRadians(armUpperLimit.get())); - armIO.setSetpoint(setpoint); + if (setpoint != null) { + io.setSetpoint(setpoint.getRadians()); } if (DriverStation.isDisabled()) { - armIO.stop(); + io.stop(); } - if (coastSupplier.getAsBoolean()) armIO.setBrakeMode(false); - // Logs - armMeasured.setAngle(Units.radiansToDegrees(inputs.armAnglePositionRads)); + armMeasured.setAngle(Units.radiansToDegrees(inputs.armPositionRads)); Logger.recordOutput("Arm/Mechanism", armMechanism); } - public void setVoltage(double volts) { - armIO.setVoltage(volts); - } - - public void setSetpoint(double setpointRads) { - if (disableSupplier.getAsBoolean() || !homed) return; - setpoint = setpointRads; + public void runVolts(double volts) { + setpoint = null; + io.runVolts(volts); } public void stop() { - armIO.stop(); + io.stop(); } public Rotation2d getAngle() { - return Rotation2d.fromRadians(inputs.armAnglePositionRads); + return Rotation2d.fromRadians(inputs.armAbsolutePositionRads); } @AutoLogOutput(key = "Arm/SetpointAngle") public Rotation2d getSetpoint() { - return Rotation2d.fromRadians(setpoint); + return setpoint != null ? setpoint : new Rotation2d(); } @AutoLogOutput(key = "Arm/Homed") @@ -160,15 +129,18 @@ public boolean homed() { @AutoLogOutput(key = "Arm/AtSetpoint") public boolean atSetpoint() { - return Math.abs(inputs.armAnglePositionRads - setpoint) <= armTolerance.get(); + return setpoint != null + && Math.abs( + Rotation2d.fromRadians(inputs.armAbsolutePositionRads).minus(setpoint).getDegrees()) + <= armToleranceDegreees.get(); } public Command getStaticCurrent() { Timer timer = new Timer(); - return run(() -> armIO.setCurrent(0.5 * timer.get())) + return run(() -> io.runCurrent(0.5 * timer.get())) .beforeStarting(timer::restart) .until(() -> Math.abs(inputs.armVelocityRadsPerSec) >= Units.degreesToRadians(10)) .andThen(() -> Logger.recordOutput("Arm/staticCurrent", inputs.armTorqueCurrentAmps[0])) - .andThen(armIO::stop); + .andThen(io::stop); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java index 7ddc60e5..4626caf1 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java @@ -7,7 +7,8 @@ public interface ArmIO { class ArmIOInputs { public boolean hasFoc = false; public boolean hasAbsoluteSensor = false; - public double armAnglePositionRads = 0.0; + public double armPositionRads = 0.0; + public double armAbsolutePositionRads = 0.0; public double armTrajectorySetpointRads = 0.0; public double armVelocityRadsPerSec = 0.0; public double[] armAppliedVolts = new double[] {}; @@ -21,11 +22,11 @@ default void updateInputs(ArmIOInputs inputs) {} /** Run to setpoint angle in radians */ default void setSetpoint(double setpointRads) {} - /** Run motors at voltage */ - default void setVoltage(double volts) {} + /** Run motors at volts */ + default void runVolts(double volts) {} /** Run motors at current */ - default void setCurrent(double amps) {} + default void runCurrent(double amps) {} /** Set brake mode enabled */ default void setBrakeMode(boolean enabled) {} @@ -40,6 +41,7 @@ default void setPID(double p, double i, double d) {} default void setProfileConstraints( double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) {} + /** Sets position of internal encoder */ default void setPosition(double positionRads) {} /** Stops motors */ diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java index 8d3c424c..7d3871ea 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java @@ -21,7 +21,7 @@ public class ArmIOKrakenFOC implements ArmIO { private final CANcoder armEncoder; private final StatusSignal armPositionRotations; - private final StatusSignal absoluteEncoderPositionRotations; + private final StatusSignal armAbsolutePositionRotations; private final StatusSignal armTrajectorySetpointPositionRotations; private final StatusSignal armVelocityRPS; private final List> armAppliedVoltage; @@ -42,8 +42,9 @@ public ArmIOKrakenFOC() { armEncoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; armEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - armEncoderConfig.MagnetSensor.MagnetOffset = 0.0; + armEncoderConfig.MagnetSensor.MagnetOffset = armEncoderOffsetRotations; armEncoder.getConfigurator().apply(armEncoderConfig); + // Leader motor configs TalonFXConfiguration leaderConfig = new TalonFXConfiguration(); leaderConfig.CurrentLimits.SupplyCurrentLimit = 40.0; @@ -82,7 +83,7 @@ public ArmIOKrakenFOC() { // Status signals armPositionRotations = leaderMotor.getPosition(); - absoluteEncoderPositionRotations = armEncoder.getPosition(); + armAbsolutePositionRotations = armEncoder.getPosition(); armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference(); armVelocityRPS = leaderMotor.getVelocity(); armAppliedVoltage = List.of(leaderMotor.getMotorVoltage(), followerMotor.getMotorVoltage()); @@ -93,7 +94,7 @@ public ArmIOKrakenFOC() { BaseStatusSignal.setUpdateFrequencyForAll( 100, armPositionRotations, - absoluteEncoderPositionRotations, + armAbsolutePositionRotations, armTrajectorySetpointPositionRotations, armVelocityRPS, armAppliedVoltage.get(0), @@ -122,7 +123,7 @@ public void updateInputs(ArmIOInputs inputs) { armTempCelsius.get(0), armTempCelsius.get(1)); - inputs.armAnglePositionRads = Units.rotationsToRadians(armPositionRotations.getValue()); + inputs.armPositionRads = Units.rotationsToRadians(armPositionRotations.getValue()); inputs.armTrajectorySetpointRads = Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue()); inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRPS.getValue()); @@ -142,12 +143,12 @@ public void setSetpoint(double setpointRads) { } @Override - public void setVoltage(double volts) { + public void runVolts(double volts) { leaderMotor.setControl(new VoltageOut(volts).withEnableFOC(true)); } @Override - public void setCurrent(double amps) { + public void runCurrent(double amps) { leaderMotor.setControl(new TorqueCurrentFOC(amps)); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java index 019b4ceb..33b9e84a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java @@ -14,12 +14,12 @@ public class ArmIOSim implements ArmIO { private final SingleJointedArmSim sim = new SingleJointedArmSim( - DCMotor.getKrakenX60(2), + DCMotor.getKrakenX60Foc(2), reduction, 1.06328, armLength, - 0.0, - Units.degreesToRadians(110.0), + minAngle.getRadians(), + maxAngle.getRadians(), true, Units.degreesToRadians(0.0)); @@ -74,7 +74,7 @@ public void updateInputs(ArmIOInputs inputs) { sim.update(0.001); } - inputs.armAnglePositionRads = sim.getAngleRads() + positionOffset; + inputs.armPositionRads = sim.getAngleRads() + positionOffset; inputs.armTrajectorySetpointRads = profiledController.getSetpoint().position; inputs.armVelocityRadsPerSec = sim.getVelocityRadPerSec(); inputs.armAppliedVolts = new double[] {appliedVoltage}; @@ -93,7 +93,7 @@ public void setSetpoint(double setpointRads) { } @Override - public void setVoltage(double volts) { + public void runVolts(double volts) { closedLoop = false; appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); sim.setInputVoltage(appliedVoltage); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java new file mode 100644 index 00000000..d5095e1c --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIO.java @@ -0,0 +1,21 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; + +import org.littletonrobotics.junction.AutoLog; + +public interface FeederIO { + @AutoLog + class FeederIOInputs { + public double positionRads = 0.0; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double outputCurrent = 0.0; + } + + default void updateInputs(FeederIOInputs inputs) {} + + /** Run feeder at volts */ + default void runVolts(double volts) {} + + /** Stop feeder */ + default void stop() {} +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java new file mode 100644 index 00000000..bfeb9993 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java @@ -0,0 +1,28 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; + +import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class FeederIOSim implements FeederIO { + private final FlywheelSim sim = new FlywheelSim( + DCMotor.getKrakenX60Foc(1), + reduction, + 0.01); + + @Override + public void updateInputs(FeederIOInputs inputs) { + FeederIO.super.updateInputs(inputs); + } + + @Override + public void runVolts(double volts) { + FeederIO.super.runVolts(volts); + } + + @Override + public void stop() { + FeederIO.super.stop(); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java new file mode 100644 index 00000000..b333ee12 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java @@ -0,0 +1,93 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; + +import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FlywheelConstants.*; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.frc2024.util.LoggedTunableNumber; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Flywheels extends SubsystemBase { + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheels/kP", gains.kP()); + private static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheels/kI", gains.kI()); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheels/kD", gains.kD()); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS()); + private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV()); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA()); + private static final LoggedTunableNumber shooterTolerance = + new LoggedTunableNumber("Flywheels/ToleranceRPM", shooterToleranceRPM); + + private final FlywheelsIO io; + private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); + + private Double leftSetpointRpm = null; + private Double rightSetpointRPM = null; + + public Flywheels(FlywheelsIO io) { + System.out.println("[Init] Creating Shooter"); + this.io = io; + } + + @Override + public void periodic() { + // check controllers + LoggedTunableNumber.ifChanged(hashCode(), pid -> io.setPID(pid[0], pid[1], pid[2]), kP, kI, kD); + LoggedTunableNumber.ifChanged( + hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA); + + io.updateInputs(inputs); + Logger.processInputs("Flywheels", inputs); + + if (DriverStation.isDisabled()) { + io.stop(); + leftSetpointRpm = null; + rightSetpointRPM = null; + } else if (leftSetpointRpm != null && rightSetpointRPM != null) { + // Run to setpoint + io.runVelocity(leftSetpointRpm, rightSetpointRPM); + } + + Logger.recordOutput( + "Flywheels/LeftSetpointRPM", leftSetpointRpm != null ? leftSetpointRpm : 0.0); + Logger.recordOutput( + "Flywheels/RightSetpointRPM", rightSetpointRPM != null ? rightSetpointRPM : 0.0); + Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm); + Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm); + } + + public void setSetpointRpm(double leftRpm, double rightRpm) { + leftSetpointRpm = leftRpm; + rightSetpointRPM = rightRpm; + } + + public void runVolts(double leftVolts, double rightVolts) { + leftSetpointRpm = null; + rightSetpointRPM = null; + io.runVolts(leftVolts, rightVolts); + } + + public void runLeftCharacterizationVolts(double volts) { + io.runCharacterizationLeftVolts(volts); + } + + public void runRightCharacterizationVolts(double volts) { + io.runCharacterizationRightVolts(volts); + } + + public double getLeftCharacterizationVelocity() { + return inputs.leftVelocityRpm; + } + + public double getRightCharacterizationVelocity() { + return inputs.rightVelocityRpm; + } + + @AutoLogOutput(key = "Shooter/AtSetpoint") + public boolean atSetpoint() { + return leftSetpointRpm != null + && rightSetpointRPM != null + && Math.abs(inputs.leftVelocityRpm - leftSetpointRpm) <= shooterTolerance.get() + && Math.abs(inputs.rightVelocityRpm - rightSetpointRPM) <= shooterTolerance.get(); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java new file mode 100644 index 00000000..a9ee2783 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java @@ -0,0 +1,44 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelsIO { + @AutoLog + class FlywheelsIOInputs { + public double leftPositionRads = 0.0; + public double leftVelocityRpm = 0.0; + public double leftAppliedVolts = 0.0; + public double leftOutputCurrent = 0.0; + public double leftTempCelsius = 0.0; + + public double rightPositionRads = 0.0; + public double rightVelocityRpm = 0.0; + public double rightAppliedVolts = 0.0; + public double rightOutputCurrent = 0.0; + public double rightTempCelsius = 0.0; + } + + /** Update inputs */ + default void updateInputs(FlywheelsIOInputs inputs) {} + + /** Run both motors at voltage */ + default void runVolts(double leftVolts, double rightVolts) {} + + /** Stop both flywheels */ + default void stop() {} + + /** Run left and right flywheels at velocity in rpm */ + default void runVelocity(double leftRpm, double rightRpm) {} + + /** Config PID values for both motors */ + default void setPID(double kP, double kI, double kD) {} + + /** Config FF values for both motors */ + default void setFF(double kS, double kV, double kA) {} + + /** Run left flywheels at voltage */ + default void runCharacterizationLeftVolts(double volts) {} + + /** Run right flywheels at voltage */ + default void runCharacterizationRightVolts(double volts) {} +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java new file mode 100644 index 00000000..741487e3 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java @@ -0,0 +1,100 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; + +import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FlywheelConstants.*; + +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 FlywheelsIOSim implements FlywheelsIO { + private final FlywheelSim leftSim = + new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); + private final FlywheelSim rightSim = + new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); + + private final PIDController leftController = + new PIDController(gains.kP(), gains.kI(), gains.kD()); + private final PIDController rightController = + new PIDController(gains.kP(), gains.kI(), gains.kD()); + private SimpleMotorFeedforward ff = + new SimpleMotorFeedforward(gains.kS(), gains.kV(), gains.kA()); + + private double leftAppliedVolts = 0.0; + private double rightAppliedVolts = 0.0; + + private Double leftSetpointRPM = null; + private Double rightSetpointRPM = null; + + @Override + public void updateInputs(FlywheelsIOInputs inputs) { + leftSim.update(0.02); + rightSim.update(0.02); + // control to setpoint + if (leftSetpointRPM != null && rightSetpointRPM != null) { + runVolts( + leftController.calculate(leftSim.getAngularVelocityRPM(), leftSetpointRPM) + + ff.calculate(leftSetpointRPM), + rightController.calculate(rightSim.getAngularVelocityRPM(), rightSetpointRPM) + + ff.calculate(rightSetpointRPM)); + } + + inputs.leftPositionRads += + Units.radiansToRotations(leftSim.getAngularVelocityRadPerSec() * 0.02); + inputs.leftVelocityRpm = leftSim.getAngularVelocityRPM(); + inputs.leftAppliedVolts = leftAppliedVolts; + inputs.leftOutputCurrent = leftSim.getCurrentDrawAmps(); + + inputs.rightPositionRads += + Units.radiansToRotations(rightSim.getAngularVelocityRadPerSec() * 0.02); + inputs.rightVelocityRpm = rightSim.getAngularVelocityRPM(); + inputs.rightAppliedVolts = rightAppliedVolts; + inputs.rightOutputCurrent = rightSim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double leftVolts, double rightVolts) { + leftAppliedVolts = MathUtil.clamp(leftVolts, -12.0, 12.0); + rightAppliedVolts = MathUtil.clamp(rightVolts, -12.0, 12.0); + leftSim.setInputVoltage(leftAppliedVolts); + rightSim.setInputVoltage(rightAppliedVolts); + } + + @Override + public void runVelocity(double leftRpm, double rightRpm) { + leftSetpointRPM = leftRpm; + rightSetpointRPM = rightRpm; + } + + @Override + public void setPID(double kP, double kI, double kD) { + leftController.setPID(kP, kI, kD); + rightController.setPID(kP, kI, kD); + } + + @Override + public void setFF(double kS, double kV, double kA) { + ff = new SimpleMotorFeedforward(kS, kV, kA); + } + + @Override + public void stop() { + runVelocity(0.0, 0.0); + } + + @Override + public void runCharacterizationLeftVolts(double volts) { + leftSetpointRPM = null; + rightSetpointRPM = null; + runVolts(volts, 0.0); + } + + @Override + public void runCharacterizationRightVolts(double volts) { + leftSetpointRPM = null; + rightSetpointRPM = null; + runVolts(0.0, volts); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java new file mode 100644 index 00000000..ba4235be --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java @@ -0,0 +1,131 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; + +import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FlywheelConstants.*; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; + +public class FlywheelsIOSparkFlex implements FlywheelsIO { + // Hardware + private CANSparkFlex leftMotor; + private CANSparkFlex rightMotor; + private RelativeEncoder leftEncoder; + private RelativeEncoder rightEncoder; + + // Controllers + private SparkPIDController leftController; + private SparkPIDController rightController; + // Open loop + private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.0, 0.0, 0.0); + + public FlywheelsIOSparkFlex() { + // Init Hardware + leftMotor = new CANSparkFlex(leftID, CANSparkFlex.MotorType.kBrushless); + rightMotor = new CANSparkFlex(rightID, CANSparkFlex.MotorType.kBrushless); + leftEncoder = leftMotor.getEncoder(); + rightEncoder = rightMotor.getEncoder(); + + // Config Hardware + // Default + leftMotor.restoreFactoryDefaults(); + rightMotor.restoreFactoryDefaults(); + + // Limits + leftMotor.setSmartCurrentLimit(60); + rightMotor.setSmartCurrentLimit(60); + leftMotor.enableVoltageCompensation(12.0); + rightMotor.enableVoltageCompensation(12.0); + + // Reset encoders + leftEncoder.setPosition(0.0); + rightEncoder.setPosition(0.0); + leftEncoder.setMeasurementPeriod(10); + rightEncoder.setMeasurementPeriod(10); + leftEncoder.setAverageDepth(2); + rightEncoder.setAverageDepth(2); + + // Get controllers + leftController = leftMotor.getPIDController(); + rightController = rightMotor.getPIDController(); + setPID(gains.kP(), gains.kI(), gains.kD()); + setFF(gains.kS(), gains.kV(), gains.kA()); + + // Disable brake mode + leftMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + rightMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + + leftMotor.burnFlash(); + rightMotor.burnFlash(); + } + + @Override + public void updateInputs(FlywheelsIOInputs inputs) { + inputs.leftPositionRads = Units.rotationsToRadians(leftEncoder.getPosition()) / reduction; + inputs.leftVelocityRpm = leftEncoder.getVelocity() / reduction; + inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); + inputs.leftOutputCurrent = leftMotor.getOutputCurrent(); + inputs.leftTempCelsius = leftMotor.getMotorTemperature(); + + inputs.rightPositionRads = Units.rotationsToRadians(rightEncoder.getPosition()) / reduction; + inputs.rightVelocityRpm = rightEncoder.getVelocity() / reduction; + inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); + inputs.rightOutputCurrent = rightMotor.getOutputCurrent(); + inputs.rightTempCelsius = rightMotor.getMotorTemperature(); + } + + @Override + public void runVolts(double leftVolts, double rightVolts) { + leftMotor.setVoltage(leftVolts); + rightMotor.setVoltage(rightVolts); + } + + @Override + public void runVelocity(double leftRpm, double rightRpm) { + leftController.setReference( + leftRpm, + CANSparkBase.ControlType.kVelocity, + 0, + ff.calculate(leftRpm), + SparkPIDController.ArbFFUnits.kVoltage); + rightController.setReference( + rightRpm, + CANSparkBase.ControlType.kVelocity, + 0, + ff.calculate(rightRpm), + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setPID(double kP, double kI, double kD) { + leftController.setP(kP); + leftController.setI(kI); + leftController.setD(kD); + rightController.setP(kP); + rightController.setI(kI); + rightController.setD(kD); + } + + @Override + public void setFF(double kS, double kV, double kA) { + ff = new SimpleMotorFeedforward(kS, kV, kA); + } + + @Override + public void runCharacterizationLeftVolts(double volts) { + leftMotor.setVoltage(volts); + } + + @Override + public void runCharacterizationRightVolts(double volts) { + rightMotor.setVoltage(volts); + } + + @Override + public void stop() { + runVelocity(0.0, 0.0); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/Shooter.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/Shooter.java deleted file mode 100644 index cb526813..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/Shooter.java +++ /dev/null @@ -1,160 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.shooter; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ShooterConstants.*; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.frc2024.util.LoggedTunableNumber; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Shooter extends SubsystemBase { - private static final LoggedTunableNumber feedVolts = - new LoggedTunableNumber("Shooter/FeedVolts", 6.0); - private static final LoggedTunableNumber leftkP = - new LoggedTunableNumber("Shooter/leftkP", leftFlywheelConstants.kP()); - private static final LoggedTunableNumber leftkI = - new LoggedTunableNumber("Shooter/leftkI", leftFlywheelConstants.kI()); - private static final LoggedTunableNumber leftkD = - new LoggedTunableNumber("Shooter/leftkD", leftFlywheelConstants.kD()); - private static final LoggedTunableNumber leftkS = - new LoggedTunableNumber("Shooter/leftkS", leftFlywheelConstants.kS()); - private static final LoggedTunableNumber leftkV = - new LoggedTunableNumber("Shooter/leftkV", leftFlywheelConstants.kV()); - private static final LoggedTunableNumber leftkA = - new LoggedTunableNumber("Shooter/leftkA", leftFlywheelConstants.kA()); - private static final LoggedTunableNumber rightkP = - new LoggedTunableNumber("Shooter/rightkP", rightFlywheelConstants.kP()); - private static final LoggedTunableNumber rightkI = - new LoggedTunableNumber("Shooter/rightkI", rightFlywheelConstants.kI()); - private static final LoggedTunableNumber rightkD = - new LoggedTunableNumber("Shooter/rightkD", rightFlywheelConstants.kD()); - private static final LoggedTunableNumber rightkS = - new LoggedTunableNumber("Shooter/rightkS", rightFlywheelConstants.kS()); - private static final LoggedTunableNumber rightkV = - new LoggedTunableNumber("Shooter/rightkV", rightFlywheelConstants.kV()); - private static final LoggedTunableNumber rightkA = - new LoggedTunableNumber("Shooter/rightkA", rightFlywheelConstants.kA()); - private static final LoggedTunableNumber shooterTolerance = - new LoggedTunableNumber("Shooter/ToleranceRPM", shooterToleranceRPM); - private static final LoggedTunableNumber intakeVolts = - new LoggedTunableNumber("Shooter/IntakeVolts", -5.0); - - private final ShooterIO io; - private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - - private boolean spinning = false; - private boolean intaking = false; - - private boolean feeding = false; - private double leftRPM = 0.0; - private double rightRPM = 0.0; - - public Shooter(ShooterIO io) { - System.out.println("[Init] Creating Shooter"); - this.io = io; - this.io.setLeftPID(leftkP.get(), leftkI.get(), leftkD.get()); - this.io.setLeftFF(leftkS.get(), leftkV.get(), leftkA.get()); - this.io.setRightFF(rightkS.get(), rightkV.get(), rightkA.get()); - this.io.setRightPID(rightkP.get(), rightkI.get(), rightkD.get()); - } - - @Override - public void periodic() { - // check controllers - LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setLeftPID(leftkP.get(), leftkI.get(), leftkD.get()), - leftkP, - leftkI, - leftkD); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setLeftFF(leftkS.get(), leftkV.get(), leftkA.get()), - leftkS, - leftkV, - leftkA); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setRightPID(rightkP.get(), rightkI.get(), rightkD.get()), - rightkP, - rightkI, - rightkD); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setRightFF(rightkS.get(), rightkV.get(), rightkA.get()), - rightkS, - rightkV, - rightkA); - - io.updateInputs(inputs); - Logger.processInputs("Shooter", inputs); - - if (DriverStation.isDisabled()) { - io.stop(); - } else { - if (spinning) { - io.setRPM(leftRPM, rightRPM); - if (feeding) { - io.setFeederVoltage(feedVolts.get()); - } - } else if (intaking) { - io.setLeftVoltage(intakeVolts.get()); - io.setRightVoltage(intakeVolts.get()); - } - } - - Logger.recordOutput("Shooter/LeftRPM", inputs.leftFlywheelVelocityRPM); - Logger.recordOutput("Shooter/RightRPM", inputs.rightFlywheelVelocityRPM); - Logger.recordOutput("Shooter/FeederRPM", inputs.feederVelocityRPM); - } - - public void requestRPM(double leftRPM, double rightRPM) { - intaking = false; - spinning = true; - this.leftRPM = leftRPM; - this.rightRPM = rightRPM; - } - - public void requestFeed() { - intaking = false; - feeding = true; - } - - public void requestIntake() { - spinning = false; - intaking = true; - } - - public void stop() { - spinning = false; - intaking = false; - } - - public void runLeftCharacterizationVolts(double volts) { - spinning = false; - intaking = false; - io.setLeftVoltage(volts); - } - - public void runRightCharacterizationVolts(double volts) { - spinning = false; - intaking = false; - io.setRightVoltage(volts); - } - - public double getLeftCharacterizationVelocity() { - return inputs.leftFlywheelVelocityRPM; - } - - public double getRightCharacterizationVelocity() { - return inputs.rightFlywheelVelocityRPM; - } - - @AutoLogOutput(key = "Shooter/AtSetpoint") - public boolean atSetpoint() { - return Math.abs(inputs.leftFlywheelVelocityRPM - leftRPM) <= shooterTolerance.get() - && Math.abs(inputs.rightFlywheelVelocityRPM - rightRPM) <= shooterTolerance.get() - && spinning; - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIO.java deleted file mode 100644 index 8cd35ae0..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIO.java +++ /dev/null @@ -1,61 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.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 setLeftVoltage(double volts) {} - - default void setRightVoltage(double volts) {} - - default void setLeftPID(double p, double i, double d) {} - - default void setLeftFF(double s, double v, double a) {} - - default void setRightPID(double p, double i, double d) {} - - default void setRightFF(double s, double v, double a) {} - - default void stop() {} -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSim.java deleted file mode 100644 index 480359aa..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSim.java +++ /dev/null @@ -1,129 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.shooter; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ShooterConstants.*; - -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 final FlywheelSim leftSim = - new FlywheelSim(DCMotor.getNeoVortex(1), flywheelReduction, 0.01); - private final FlywheelSim rightSim = - new FlywheelSim(DCMotor.getNeoVortex(1), flywheelReduction, 0.01); - private final FlywheelSim feederSim = new FlywheelSim(DCMotor.getAndymarkRs775_125(1), 1.0, 0.01); - - private final PIDController leftController = - new PIDController( - leftFlywheelConstants.kP(), leftFlywheelConstants.kI(), leftFlywheelConstants.kD()); - private final PIDController rightController = - new PIDController( - rightFlywheelConstants.kP(), rightFlywheelConstants.kI(), rightFlywheelConstants.kD()); - private SimpleMotorFeedforward leftFF = - new SimpleMotorFeedforward( - leftFlywheelConstants.kS(), leftFlywheelConstants.kV(), leftFlywheelConstants.kA()); - private SimpleMotorFeedforward rightFF = - new SimpleMotorFeedforward( - rightFlywheelConstants.kS(), rightFlywheelConstants.kV(), rightFlywheelConstants.kA()); - - 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) - + leftFF.calculate(leftSetpointRPM); - leftSim.setInputVoltage(MathUtil.clamp(leftAppliedVolts, -12.0, 12.0)); - } - if (rightSetpointRPM != null) { - rightAppliedVolts = - rightController.calculate(rightSim.getAngularVelocityRPM(), rightSetpointRPM) - + rightFF.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 setLeftVoltage(double volts) { - leftSetpointRPM = null; - leftAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - leftSim.setInputVoltage(leftAppliedVolts); - } - - @Override - public void setRightVoltage(double volts) { - rightSetpointRPM = null; - rightAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - rightSim.setInputVoltage(rightAppliedVolts); - } - - @Override - public void setLeftPID(double p, double i, double d) { - leftController.setPID(p, i, d); - } - - @Override - public void setLeftFF(double s, double v, double a) { - leftFF = new SimpleMotorFeedforward(s, v, a); - } - - @Override - public void setRightPID(double p, double i, double d) { - rightController.setPID(p, i, d); - } - - @Override - public void setRightFF(double s, double v, double a) { - rightFF = new SimpleMotorFeedforward(s, v, a); - } - - @Override - public void stop() { - setRPM(0.0, 0.0); - setFeederVoltage(0.0); - } -} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSparkMax.java deleted file mode 100644 index 24b9906e..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/shooter/ShooterIOSparkMax.java +++ /dev/null @@ -1,169 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.shooter; - -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ShooterConstants.*; - -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 leftFF = new SimpleMotorFeedforward(0.0, 0.0, 0.0); - private SimpleMotorFeedforward rightFF = new SimpleMotorFeedforward(0.0, 0.0, 0.0); - - public ShooterIOSparkMax() { - leftMotor = new CANSparkFlex(leftFlywheelConstants.id(), CANSparkFlex.MotorType.kBrushless); - rightMotor = new CANSparkFlex(rightFlywheelConstants.id(), CANSparkFlex.MotorType.kBrushless); - feederMotor = new CANSparkFlex(feederConstants.id(), CANSparkFlex.MotorType.kBrushless); - - leftEncoder = leftMotor.getEncoder(); - rightEncoder = rightMotor.getEncoder(); - - leftMotor.restoreFactoryDefaults(); - rightMotor.restoreFactoryDefaults(); - feederMotor.restoreFactoryDefaults(); - - leftMotor.setInverted(leftFlywheelConstants.inverted()); - rightMotor.setInverted(rightFlywheelConstants.inverted()); - leftMotor.setSmartCurrentLimit(60); - rightMotor.setSmartCurrentLimit(60); - leftMotor.enableVoltageCompensation(12.0); - rightMotor.enableVoltageCompensation(12.0); - - feederMotor.setInverted(feederConstants.inverted()); - 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 / flywheelReduction); - rightEncoder.setPositionConversionFactor(1.0 / flywheelReduction); - leftEncoder.setVelocityConversionFactor(1.0 / flywheelReduction); - rightEncoder.setVelocityConversionFactor(1.0 / 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, - leftFF.calculate(rpm), - SparkPIDController.ArbFFUnits.kVoltage); - } - - @Override - public void setRightRPM(double rpm) { - rightController.setReference( - rpm, - CANSparkBase.ControlType.kVelocity, - 0, - rightFF.calculate(rpm), - SparkPIDController.ArbFFUnits.kVoltage); - } - - @Override - public void setFeederVoltage(double volts) { - feederMotor.setVoltage(volts); - } - - @Override - public void setLeftVoltage(double volts) { - leftMotor.setVoltage(volts); - } - - @Override - public void setRightVoltage(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 setLeftPID(double p, double i, double d) { - leftController.setP(p); - leftController.setI(i); - leftController.setD(d); - } - - @Override - public void setLeftFF(double kS, double kV, double kA) { - leftFF = new SimpleMotorFeedforward(kS, kV, kA); - } - - @Override - public void setRightPID(double p, double i, double d) { - rightController.setP(p); - rightController.setI(i); - rightController.setD(d); - } - - @Override - public void setRightFF(double s, double v, double a) { - rightFF = new SimpleMotorFeedforward(s, v, a); - } - - @Override - public void stop() { - setRPM(0.0, 0.0); - setFeederVoltage(0.0); - } -}