Elevator subsystem copied

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());
         }
       }
     }

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;
+  }
+}

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) {}
+}

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() {}
+}

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<Double> topPositionRads;
+  private final StatusSignal<Double> topVelocityRPM;
+  private final StatusSignal<Double> topAppliedVolts;
+  private final StatusSignal<Double> topSupplyCurrent;
+  private final StatusSignal<Double> topTemp;
+  private final StatusSignal<Double> bottomPositionRads;
+  private final StatusSignal<Double> bottomVelocityRPM;
+  private final StatusSignal<Double> bottomAppliedVolts;
+  private final StatusSignal<Double> bottomSupplyCurrent;
+  private final StatusSignal<Double> 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);
+  }
+}