diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 6bebedc2..d19fc867 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -21,7 +21,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.DEVBOT; + private static RobotType robotType = RobotType.SIMBOT; public static final boolean tuningMode = true; public static RobotType getRobot() { diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index 6ae269cf..1279058f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -79,7 +79,7 @@ public static class OdometryTimestampInputs { /** Active drive mode. */ private DriveMode currentDriveMode = DriveMode.TELEOP; - private double characterizationVolts = 0.0; + private double characterizationInput = 0.0; private boolean modulesOrienting = false; private final Timer coastTimer = new Timer(); private boolean brakeModeEnabled = true; @@ -245,7 +245,7 @@ public void periodic() { case CHARACTERIZATION -> { // run characterization for (Module module : modules) { - module.runCharacterization(characterizationVolts); + module.runCharacterization(characterizationInput); } } default -> {} @@ -343,7 +343,7 @@ public boolean isAutoAimGoalCompleted() { /** Runs forwards at the commanded voltage. */ public void runCharacterizationVolts(double volts) { currentDriveMode = DriveMode.CHARACTERIZATION; - characterizationVolts = volts; + characterizationInput = volts; } /** Disables the characterization mode. */ diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java index ba2a6497..7b86dc28 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java @@ -20,15 +20,6 @@ /** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { - // For Kraken - public static class KrakenDriveConstants { - public static final boolean useTorqueCurrentFOC = false; - public static final boolean useMotionMagic = false; - public static final double motionMagicCruiseVelocity = 0.0; - public static final double motionMagicAcceleration = 0.0; - } - - // Drive Constants public static DriveConfig driveConfig = switch (Constants.getRobot()) { case SIMBOT, COMPBOT -> 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 d27c0dd0..2e3deddd 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import lombok.Getter; +import org.littletonrobotics.frc2024.util.Alert; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; @@ -31,62 +32,83 @@ public class Module { new LoggedTunableNumber("Drive/Module/TurnkP", moduleConstants.turnkP()); private static final LoggedTunableNumber turnkD = new LoggedTunableNumber("Drive/Module/TurnkD", moduleConstants.turnkD()); + private static final String[] moduleNames = new String[] {"FL", "FR", "BL", "BR"}; private final int index; private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private SimpleMotorFeedforward driveFF = + private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0); - @Getter private SwerveModuleState setpointState = new SwerveModuleState(); + // Alerts + private final Alert driveMotorDisconnected; + private final Alert turnMotorDisconnected; + public Module(ModuleIO io, int index) { this.io = io; this.index = index; + + driveMotorDisconnected = + new Alert( + "Drive", moduleNames[index] + " drive motor disconnected!", Alert.AlertType.WARNING); + turnMotorDisconnected = + new Alert( + "Drive", moduleNames[index] + " turn motor disconnected!", Alert.AlertType.WARNING); } /** Called while blocking odometry thread */ public void updateInputs() { io.updateInputs(inputs); - Logger.processInputs("Drive/Module" + index, inputs); + Logger.processInputs("Drive/" + moduleNames[index] + "_Module", inputs); - // Update FF and controllers + // Update ff and controllers LoggedTunableNumber.ifChanged( hashCode(), - () -> { - driveFF = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0); - io.setDriveFF(drivekS.get(), drivekV.get(), 0); - }, + () -> ff = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0), drivekS, drivekV); LoggedTunableNumber.ifChanged( hashCode(), () -> io.setDrivePID(drivekP.get(), 0, drivekD.get()), drivekP, drivekD); LoggedTunableNumber.ifChanged( hashCode(), () -> io.setTurnPID(turnkP.get(), 0, turnkD.get()), turnkP, turnkD); + + // Display warnings + driveMotorDisconnected.set(!inputs.driveMotorConnected); + turnMotorDisconnected.set(!inputs.turnMotorConnected); } + /** Runs to {@link SwerveModuleState} */ public void runSetpoint(SwerveModuleState setpoint) { setpointState = setpoint; - io.setDriveVelocitySetpoint( + io.runDriveVelocitySetpoint( setpoint.speedMetersPerSecond / driveConfig.wheelRadius(), - driveFF.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius())); - io.setTurnPositionSetpoint(setpoint.angle.getRadians()); + ff.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius())); + io.runTurnPositionSetpoint(setpoint.angle.getRadians()); } - public void runCharacterization(double volts) { - io.setTurnPositionSetpoint(0.0); - io.setDriveVoltage(volts); + /** Runs characterization volts or amps depending on using voltage or current control. */ + public void runCharacterization(double input) { + io.runTurnPositionSetpoint(0.0); + if (inputs.hasCurrentControl) { + io.runDriveCurrent(input); + } else { + io.runDriveVolts(input); + } } + /** Sets brake mode to {@code enabled}. */ public void setBrakeMode(boolean enabled) { io.setDriveBrakeMode(enabled); io.setTurnBrakeMode(enabled); } + /** Stops motors. */ public void stop() { io.stop(); } + /** Get all latest {@link SwerveModulePosition}'s from last cycle. */ public SwerveModulePosition[] getModulePositions() { int minOdometryPositions = Math.min(inputs.odometryDrivePositionsMeters.length, inputs.odometryTurnPositions.length); @@ -99,26 +121,32 @@ public SwerveModulePosition[] getModulePositions() { return positions; } + /** Get turn angle of module as {@link Rotation2d}. */ public Rotation2d getAngle() { return inputs.turnAbsolutePosition; } + /** Get position of wheel in meters. */ public double getPositionMeters() { return inputs.drivePositionRad * driveConfig.wheelRadius(); } + /** Get velocity of wheel in m/s. */ public double getVelocityMetersPerSec() { return inputs.driveVelocityRadPerSec * driveConfig.wheelRadius(); } + /** Get current {@link SwerveModulePosition} of module. */ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } + /** Get current {@link SwerveModuleState} of module. */ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } + /** Get velocity of drive wheel for characterization */ public double getCharacterizationVelocity() { return inputs.driveVelocityRadPerSec; } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java index ecd7f27a..1a7c2abf 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIO.java @@ -13,6 +13,10 @@ public interface ModuleIO { @AutoLog class ModuleIOInputs { + public boolean driveMotorConnected = true; + public boolean turnMotorConnected = true; + public boolean hasCurrentControl = false; + public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; @@ -34,16 +38,22 @@ class ModuleIOInputs { default void updateInputs(ModuleIOInputs inputs) {} /** Run drive motor at volts */ - default void setDriveVoltage(double volts) {} + default void runDriveVolts(double volts) {} /** Run turn motor at volts */ - default void setTurnVoltage(double volts) {} + default void runTurnVolts(double volts) {} + + /** Run drive motor at current */ + default void runDriveCurrent(double current) {} - /** Set drive velocity setpoint */ - default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) {} + /** Run turn motor at current */ + default void runTurnCurrent(double current) {} - /** Set turn position setpoint */ - default void setTurnPositionSetpoint(double angleRads) {} + /** Run to drive velocity setpoint with feedforward */ + default void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) {} + + /** Run to turn position setpoint */ + default void runTurnPositionSetpoint(double angleRads) {} /** Configure drive PID */ default void setDrivePID(double kP, double kI, double kD) {} @@ -51,9 +61,6 @@ default void setDrivePID(double kP, double kI, double kD) {} /** Configure turn PID */ default void setTurnPID(double kP, double kI, double kD) {} - /** Configure drive feedforward for TorqueCurrentFOC */ - default void setDriveFF(double kS, double kV, double kA) {} - /** Enable or disable brake mode on the drive motor. */ default void setDriveBrakeMode(boolean enable) {} 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 f313ad6e..c750bc94 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java @@ -46,15 +46,25 @@ public class ModuleIOKrakenFOC implements ModuleIO { private final StatusSignal turnSupplyCurrent; private final StatusSignal turnTorqueCurrent; - // Queues + // Odometry Queues private final Queue drivePositionQueue; private final Queue turnPositionQueue; - private static final int shouldResetCounts = 100; - private int resetCounter = shouldResetCounts; - - private Slot0Configs driveFeedbackConfig = new Slot0Configs(); - private Slot0Configs turnFeedbackConfig = new Slot0Configs(); + // Controller Configs + private final Slot0Configs driveFeedbackConfig = new Slot0Configs(); + private final Slot0Configs turnFeedbackConfig = new Slot0Configs(); + + // Control + private final VoltageOut driveVoltage = new VoltageOut(0).withUpdateFreqHz(0); + private final VoltageOut turnVoltage = new VoltageOut(0).withUpdateFreqHz(0); + private final TorqueCurrentFOC driveCurrent = new TorqueCurrentFOC(0).withUpdateFreqHz(0); + private final TorqueCurrentFOC turnCurrent = new TorqueCurrentFOC(0).withUpdateFreqHz(0); + private final VelocityTorqueCurrentFOC driveVelocityControl = + new VelocityTorqueCurrentFOC(0).withUpdateFreqHz(0); + private final PositionTorqueCurrentFOC turnPositionControl = + new PositionTorqueCurrentFOC(0).withUpdateFreqHz(0); + private final NeutralOut driveBrake = new NeutralOut().withUpdateFreqHz(0); + private final NeutralOut turnBrake = new NeutralOut().withUpdateFreqHz(0); public ModuleIOKrakenFOC(ModuleConfig config) { // Init controllers and encoders from config constants @@ -80,19 +90,11 @@ public ModuleIOKrakenFOC(ModuleConfig config) { ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - // If in motoControl mode, set reference points in rotations convert from radians - // Affects getPosition() and getVelocity() + // Conversions affect getPosition()/setPosition() and getVelocity() driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - // Config Motion Magic - if (KrakenDriveConstants.useMotionMagic) { - turnConfig.MotionMagic.MotionMagicCruiseVelocity = - KrakenDriveConstants.motionMagicCruiseVelocity; - turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; - } - // Apply configs for (int i = 0; i < 4; i++) { boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; @@ -139,29 +141,26 @@ public ModuleIOKrakenFOC(ModuleConfig config) { PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + + // Reset turn position to absolute encoder position + turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); } @Override public void updateInputs(ModuleIOInputs inputs) { - // Reset position of encoder to absolute position every shouldResetCount cycles - // Make sure turnMotor is not moving too fast - if (++resetCounter >= shouldResetCounts - && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { - turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); - resetCounter = 0; - } - - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveSupplyCurrent, - driveTorqueCurrent, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnSupplyCurrent, - turnTorqueCurrent); + inputs.hasCurrentControl = true; + inputs.driveMotorConnected = + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent) + == StatusCode.OK; + inputs.turnMotorConnected = + BaseStatusSignal.refreshAll( + turnPosition, turnVelocity, turnAppliedVolts, turnSupplyCurrent, turnTorqueCurrent) + == StatusCode.OK; inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); @@ -188,44 +187,36 @@ public void updateInputs(ModuleIOInputs inputs) { } @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + public void runDriveVolts(double volts) { + driveTalon.setControl(driveVoltage.withOutput(volts)); } @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + public void runTurnVolts(double volts) { + turnTalon.setControl(turnVoltage.withOutput(volts)); } @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); - } else { - driveTalon.setControl( - new VelocityVoltage(velocityRotationsPerSec) - .withFeedForward(ffVolts) - .withEnableFOC(true)); - } + public void runDriveCurrent(double current) { + driveTalon.setControl(driveCurrent.withOutput(current)); } @Override - public void setTurnPositionSetpoint(double angleRads) { - double angleRotations = Units.radiansToRotations(angleRads); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); - } else { - turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); - } - } else { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicVoltage(angleRotations).withEnableFOC(true)); - } else { - turnTalon.setControl(new PositionVoltage(angleRotations).withEnableFOC(true)); - } - } + public void runTurnCurrent(double current) { + turnTalon.setControl(turnCurrent.withOutput(current)); + } + + @Override + public void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) { + driveTalon.setControl( + driveVelocityControl + .withVelocity(Units.radiansToRotations(velocityRadsPerSec)) + .withFeedForward(feedForward)); + } + + @Override + public void runTurnPositionSetpoint(double angleRads) { + turnTalon.setControl(turnPositionControl.withPosition(Units.radiansToRotations(angleRads))); } @Override @@ -244,14 +235,6 @@ public void setTurnPID(double kP, double kI, double kD) { turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); } - @Override - public void setDriveFF(double kS, double kV, double kA) { - driveFeedbackConfig.kS = kS; - driveFeedbackConfig.kV = kV; - driveFeedbackConfig.kA = kA; - driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); - } - @Override public void setDriveBrakeMode(boolean enable) { driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); @@ -264,7 +247,7 @@ public void setTurnBrakeMode(boolean enable) { @Override public void stop() { - driveTalon.setControl(new NeutralOut()); - turnTalon.setControl(new NeutralOut()); + driveTalon.setControl(driveBrake); + turnTalon.setControl(turnBrake); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java index a21d893d..606372f6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -57,26 +57,26 @@ public void updateInputs(ModuleIOInputs inputs) { new Rotation2d[] {Rotation2d.fromRadians(turnSim.getAngularPositionRad())}; } - public void setDriveVoltage(double volts) { + public void runDriveVolts(double volts) { driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); driveSim.setInputVoltage(driveAppliedVolts); } - public void setTurnVoltage(double volts) { + public void runTurnVolts(double volts) { turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); turnSim.setInputVoltage(turnAppliedVolts); } @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - setDriveVoltage( + public void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) { + runDriveVolts( driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) - + ffVolts); + + feedForward); } @Override - public void setTurnPositionSetpoint(double angleRads) { - setTurnVoltage(turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); + public void runTurnPositionSetpoint(double angleRads) { + runTurnVolts(turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); } @Override 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 586ee20c..6ae50e71 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java @@ -131,28 +131,28 @@ public void updateInputs(ModuleIOInputs inputs) { } @Override - public void setDriveVoltage(double volts) { + public void runDriveVolts(double volts) { driveMotor.setVoltage(volts); } @Override - public void setTurnVoltage(double volts) { + public void runTurnVolts(double volts) { turnMotor.setVoltage(volts); } @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + public void runDriveVelocitySetpoint(double velocityRadsPerSec, double feedForward) { double feedback = driveController.calculate( Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / moduleConstants.driveReduction(), velocityRadsPerSec); - setDriveVoltage(feedback + ffVolts); + runDriveVolts(feedback + feedForward); } @Override - public void setTurnPositionSetpoint(double angleRads) { - setTurnVoltage(turnController.calculate(absoluteEncoderValue.get().getRadians(), angleRads)); + public void runTurnPositionSetpoint(double angleRads) { + runTurnVolts(turnController.calculate(absoluteEncoderValue.get().getRadians(), angleRads)); } @Override