From cf86e763bfc1f2f0632e4a969037c3bb0494abe0 Mon Sep 17 00:00:00 2001 From: Ethan Li Date: Tue, 3 Dec 2024 16:59:51 -0800 Subject: [PATCH] Elevator subsystem copied --- src/main/java/frc/robot/RobotContainer.java | 10 ++ .../robot/subsystems/elevator/Elevator.java | 45 +++++++ .../elevator/ElevatorConstants.java | 23 ++++ .../robot/subsystems/elevator/ElevatorIO.java | 33 +++++ .../elevator/ElevatorIOTalonFX.java | 124 ++++++++++++++++++ 5 files changed, 235 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/elevator/Elevator.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java create mode 100644 src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e160785..727b501 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,7 +7,13 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.Mode; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorConstants; +import frc.robot.subsystems.elevator.ElevatorIO; +import frc.robot.subsystems.elevator.ElevatorIOTalonFX; +import frc.robot.subsystems.elevator.ElevatorIOTalonFX; import frc.robot.subsystems.flywheels.Flywheels; +import frc.robot.subsystems.flywheels.FlywheelsIO; import frc.robot.subsystems.flywheels.Flywheels.VelocityTarget; import frc.robot.subsystems.flywheels.FlywheelsIOTalonFX; import frc.robot.subsystems.rollers.Rollers; @@ -35,6 +41,7 @@ public class RobotContainer { private Drive swerve; // FIXME make final, implement other robot types private Rollers rollers; private Flywheels flywheels; + private Elevator elevator; public RobotContainer() { Intake intake = null; @@ -51,6 +58,7 @@ public RobotContainer() { new ModuleIOTalonFX(DriveConstants.MODULE_CONFIGS[3])); intake = new Intake(new IntakeIOTalonFX()); flywheels = new Flywheels(new FlywheelsIOTalonFX()); + elevator = new Elevator(new ElevatorIOTalonFX()); } case DEV -> { swerve = @@ -62,6 +70,7 @@ public RobotContainer() { new ModuleIOTalonFX(DriveConstants.MODULE_CONFIGS[3])); intake = new Intake(new IntakeIOTalonFX()); // FIXME flywheels = new Flywheels(new FlywheelsIOTalonFX()); + elevator = new Elevator(new ElevatorIOTalonFX()); } case SIM -> { swerve = @@ -73,6 +82,7 @@ public RobotContainer() { new ModuleIOTalonFX(DriveConstants.MODULE_CONFIGS[3])); intake = new Intake(new IntakeIOTalonFX()); // FIXME flywheels = new Flywheels(new FlywheelsIOTalonFX()); + elevator = new Elevator(new ElevatorIOTalonFX()); } } } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..d95e111 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.elevator; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Elevator extends SubsystemBase { + // RPM + public enum VelocityTarget { + IDLE(0, 0), + SHOOT(7500, 7500), + SLOW(1200, 1200); + private int topVelocity, bottomVelocity; + + private VelocityTarget(int topVelocity, int bottomVelocity) { + this.topVelocity = topVelocity; + this.bottomVelocity = bottomVelocity; + } + } + + private final ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + @AutoLogOutput(key = "Mechanism/Elevator/Target") + private VelocityTarget velocityTarget = VelocityTarget.IDLE; + + public Elevator(ElevatorIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Mechanism/Elevator", inputs); + + io.runVelocity(velocityTarget.topVelocity, velocityTarget.bottomVelocity); + + Logger.recordOutput("Mechanism/Elevator/TopTargetVelocity", velocityTarget.topVelocity); + Logger.recordOutput("Mechanism/Elevator/BottomTargetVelocity", velocityTarget.bottomVelocity); + } + + public void setVelocityTarget(VelocityTarget target) { + velocityTarget = target; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java new file mode 100644 index 0000000..0c79a40 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.elevator; + +import frc.robot.Constants; + +public class ElevatorConstants { + public static final ElevatorConfig ELEVATOR_CONFIG = + switch (Constants.getRobotType()) { + case COMP -> new ElevatorConfig(15, 16, 0.75); // FIXME + case DEV -> new ElevatorConfig(0, 0, 1); + case SIM -> new ElevatorConfig(0, 0, 1); + }; + + public static final PIDGains GAINS = + switch (Constants.getRobotType()) { + case COMP -> new PIDGains(0, 0, 0, 0, 0.09, 0); // FIXME + case DEV -> new PIDGains(0, 0, 0, 0, 0, 0); + case SIM -> new PIDGains(0, 0, 0, 0, 0, 0); + }; + + public record ElevatorConfig(int topID, int bottomID, double reduction) {} + + public record PIDGains(double kP, double kI, double kD, double kS, double kV, double kA) {} +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..7acbb74 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + @AutoLog + class ElevatorIOInputs { + public boolean topMotorConnected = true; + public boolean bottomMotorConnected = true; + + public double topPositionRads = 0; + public double topVelocityRPM = 0; + public double topAppliedVolts = 0; + public double topSupplyCurrent = 0; + public double topTempCelcius = 0; + + public double bottomPositionRads = 0; + public double bottomVelocityRPM = 0; + public double bottomAppliedVolts = 0; + public double bottomSupplyCurrent = 0; + public double bottomTempCelcius = 0; + } + + default void updateInputs(ElevatorIOInputs inputs) {} + + /* Run top and bottom flywheels at target velocities (RPM) */ + default void runVelocity(int topRPM, int bottomRPM) {} + + /* set slot0 (PID + ff) for both motors */ + default void setSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + + default void stop() {} +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java new file mode 100644 index 0000000..57d8e72 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -0,0 +1,124 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.subsystems.elevator.ElevatorConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; + +public class ElevatorIOTalonFX implements ElevatorIO { + private final TalonFX topTalon; + private final TalonFX bottomTalon; + + private final StatusSignal topPositionRads; + private final StatusSignal topVelocityRPM; + private final StatusSignal topAppliedVolts; + private final StatusSignal topSupplyCurrent; + private final StatusSignal topTemp; + private final StatusSignal bottomPositionRads; + private final StatusSignal bottomVelocityRPM; + private final StatusSignal bottomAppliedVolts; + private final StatusSignal bottomSupplyCurrent; + private final StatusSignal bottomTemp; + + private final Slot0Configs gainsConfig = new Slot0Configs(); + private final VelocityVoltage velocityOutput = new VelocityVoltage(0).withUpdateFreqHz(0); + private final NeutralOut neutralOutput = new NeutralOut(); + + public ElevatorIOTalonFX() { + topTalon = new TalonFX(ELEVATOR_CONFIG.topID()); + bottomTalon = new TalonFX(ELEVATOR_CONFIG.bottomID()); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.CurrentLimits.SupplyCurrentLimit = 60.0; // FIXME + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.Feedback.SensorToMechanismRatio = ELEVATOR_CONFIG.reduction(); + + gainsConfig.kP = GAINS.kP(); + gainsConfig.kI = GAINS.kI(); + gainsConfig.kD = GAINS.kD(); + gainsConfig.kS = GAINS.kS(); + gainsConfig.kV = GAINS.kV(); + gainsConfig.kA = GAINS.kA(); + + topTalon.getConfigurator().apply(config); + bottomTalon.getConfigurator().apply(config); + topTalon.getConfigurator().apply(gainsConfig); + bottomTalon.getConfigurator().apply(gainsConfig); + + topTalon.setInverted(true); + bottomTalon.setInverted(true); // FIXME + + topPositionRads = topTalon.getPosition(); + topVelocityRPM = topTalon.getVelocity(); + topAppliedVolts = topTalon.getMotorVoltage(); + topSupplyCurrent = topTalon.getSupplyCurrent(); + topTemp = topTalon.getDeviceTemp(); + + bottomPositionRads = bottomTalon.getPosition(); + bottomVelocityRPM = bottomTalon.getVelocity(); + bottomAppliedVolts = bottomTalon.getMotorVoltage(); + bottomSupplyCurrent = bottomTalon.getSupplyCurrent(); + bottomTemp = bottomTalon.getDeviceTemp(); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.topMotorConnected = + BaseStatusSignal.refreshAll( + topPositionRads, topVelocityRPM, topAppliedVolts, topSupplyCurrent, topTemp) + .isOK(); + inputs.bottomMotorConnected = + BaseStatusSignal.refreshAll( + bottomPositionRads, + bottomVelocityRPM, + bottomAppliedVolts, + bottomSupplyCurrent, + bottomTemp) + .isOK(); + + inputs.topPositionRads = Units.rotationsToRadians(topPositionRads.getValueAsDouble()); + inputs.topVelocityRPM = topVelocityRPM.getValueAsDouble() * 60.0; + inputs.topAppliedVolts = topAppliedVolts.getValueAsDouble(); + inputs.topSupplyCurrent = topSupplyCurrent.getValueAsDouble(); + inputs.topTempCelcius = topTemp.getValueAsDouble(); + + inputs.bottomPositionRads = Units.rotationsToRadians(bottomPositionRads.getValueAsDouble()); + inputs.bottomVelocityRPM = bottomVelocityRPM.getValueAsDouble() * 60.0; + inputs.bottomAppliedVolts = bottomAppliedVolts.getValueAsDouble(); + inputs.bottomSupplyCurrent = bottomSupplyCurrent.getValueAsDouble(); + inputs.bottomTempCelcius = bottomTemp.getValueAsDouble(); + } + + @Override + public void runVelocity(int topVelocity, int bottomVelocity) { + topTalon.setControl(velocityOutput.withVelocity(topVelocity / 60)); + bottomTalon.setControl(velocityOutput.withVelocity(bottomVelocity / 60)); + } + + @Override + public void setSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { + gainsConfig.kP = kP; + gainsConfig.kI = kI; + gainsConfig.kD = kD; + gainsConfig.kS = kS; + gainsConfig.kV = kV; + gainsConfig.kA = kA; + + topTalon.getConfigurator().apply(gainsConfig); + bottomTalon.getConfigurator().apply(gainsConfig); + } + + @Override + public void stop() { + topTalon.setControl(neutralOutput); + bottomTalon.setControl(neutralOutput); + } +}