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 f5e2ef6d..9070d242 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -21,6 +21,7 @@ import java.util.concurrent.locks.ReentrantLock; import java.util.function.Supplier; import java.util.stream.IntStream; +import lombok.Getter; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.Constants; import org.littletonrobotics.frc2024.RobotState; @@ -40,11 +41,10 @@ @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { - private static final LoggedTunableNumber coastSpeedLimit = - new LoggedTunableNumber( - "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6); - private static final LoggedTunableNumber coastDisableTime = - new LoggedTunableNumber("Drive/CoastDisableTimeSeconds", 0.5); + private static final LoggedTunableNumber coastWaitTime = + new LoggedTunableNumber("Drive/CoastWaitTimeSeconds", 0.5); + private static final LoggedTunableNumber coastMetersPerSecThreshold = + new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", 0.05); public enum DriveMode { /** Driving with input from driver joysticks. (Default) */ @@ -66,7 +66,6 @@ public static class OdometryTimestampInputs { } public static final Lock odometryLock = new ReentrantLock(); - // TODO: DO THIS BETTER! public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); private final OdometryTimestampInputsAutoLogged odometryTimestampInputs = @@ -75,15 +74,19 @@ public static class OdometryTimestampInputs { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; + // Store previous positions and time for filtering odometry data private SwerveDriveWheelPositions lastPositions = null; private double lastTime = 0.0; /** 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 final Timer lastMovementTimer = new Timer(); + + @Getter + @AutoLogOutput(key = "Drive/BrakeModeEnabled") private boolean brakeModeEnabled = true; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); @@ -97,7 +100,7 @@ public static class OdometryTimestampInputs { new SwerveModuleState(), new SwerveModuleState() }); - private SwerveSetpointGenerator setpointGenerator; + private final SwerveSetpointGenerator setpointGenerator; private final TeleopDriveController teleopDriveController; private TrajectoryController trajectoryController = null; @@ -110,6 +113,8 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) modules[1] = new Module(fr, 1); modules[2] = new Module(bl, 2); modules[3] = new Module(br, 3); + lastMovementTimer.start(); + setBrakeMode(true); setpointGenerator = SwerveSetpointGenerator.builder() @@ -152,7 +157,6 @@ public void periodic() { } // Pass odometry data to robot state for (int i = 0; i < minOdometryUpdates; i++) { - boolean includeMeasurement = true; int odometryIndex = i; Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null; // Get all four swerve module positions at that odometry update @@ -162,10 +166,11 @@ public void periodic() { Arrays.stream(modules) .map(module -> module.getModulePositions()[odometryIndex]) .toArray(SwerveModulePosition[]::new)); - // Filtering + // Filtering based on delta wheel positions + boolean includeMeasurement = true; if (lastPositions != null) { - double dt = Timer.getFPGATimestamp() - lastTime; - for (int j = 0; j < 4; j++) { + double dt = odometryTimestampInputs.timestamps[i] - lastTime; + for (int j = 0; j < modules.length; j++) { double velocity = (wheelPositions.positions[j].distanceMeters - lastPositions.positions[j].distanceMeters) @@ -173,25 +178,26 @@ public void periodic() { double omega = wheelPositions.positions[j].angle.minus(lastPositions.positions[j].angle).getRadians() / dt; - - if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 100.0 - || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 100.0) { + // Check if delta is too large + if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 5.0 + || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 5.0) { includeMeasurement = false; break; } } } + // If delta isn't too large we can include the measurement. if (includeMeasurement) { lastPositions = wheelPositions; RobotState.getInstance() .addOdometryObservation( new RobotState.OdometryObservation( wheelPositions, yaw, odometryTimestampInputs.timestamps[i])); + lastTime = odometryTimestampInputs.timestamps[i]; } } - lastTime = Timer.getFPGATimestamp(); - // update current velocities use gyro when possible + // Update current velocities use gyro when possible ChassisSpeeds robotRelativeVelocity = getSpeeds(); robotRelativeVelocity.omegaRadiansPerSecond = gyroInputs.connected @@ -199,27 +205,16 @@ public void periodic() { : robotRelativeVelocity.omegaRadiansPerSecond; RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); - // Disabled, stop modules and coast - if (DriverStation.isDisabled()) { - Arrays.stream(modules).forEach(Module::stop); - if (Math.hypot( - robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) - <= coastSpeedLimit.get() - && brakeModeEnabled) { - setBrakeMode(false); - coastTimer.stop(); - coastTimer.reset(); - } else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) { - setBrakeMode(false); - coastTimer.stop(); - coastTimer.reset(); - } else { - coastTimer.start(); - } - return; - } else { - // Brake mode - setBrakeMode(true); + // Update brake mode + // Reset movement timer if moved + if (Arrays.stream(modules) + .anyMatch(module -> module.getVelocityMetersPerSec() > coastMetersPerSecThreshold.get())) { + lastMovementTimer.reset(); + } + if (DriverStation.isEnabled()) { + setBrakeMode(true); // Always in brake mode during teleop + } else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) { + setBrakeMode(false); } // Run drive based on current mode @@ -247,7 +242,7 @@ public void periodic() { case CHARACTERIZATION -> { // run characterization for (Module module : modules) { - module.runCharacterization(characterizationVolts); + module.runCharacterization(characterizationInput); } } default -> {} @@ -345,7 +340,7 @@ public boolean atHeadingGoal() { /** Runs forwards at the commanded voltage. */ public void runCharacterizationVolts(double volts) { currentDriveMode = DriveMode.CHARACTERIZATION; - characterizationVolts = volts; + characterizationInput = volts; } /** Disables the characterization mode. */ @@ -362,10 +357,12 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } - /** Set brake mode enabled */ + /** Set brake mode to {@code enabled} doesn't change brake mode if already set */ public void setBrakeMode(boolean enabled) { + if (brakeModeEnabled != enabled) { + Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); + } brakeModeEnabled = enabled; - Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } public Command orientModules(Rotation2d orientation) { 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 b4e9a614..27ed86d7 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..b41e9d40 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 alerts + 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..32742835 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 driveNeutral = new NeutralOut().withUpdateFreqHz(0); + private final NeutralOut turnNeutral = 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) + .isOK(); + inputs.turnMotorConnected = + BaseStatusSignal.refreshAll( + turnPosition, turnVelocity, turnAppliedVolts, turnSupplyCurrent, turnTorqueCurrent) + .isOK(); 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(driveNeutral); + turnTalon.setControl(turnNeutral); } } 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 727b6b69..1044afb6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import org.littletonrobotics.frc2024.Constants; @@ -38,6 +39,10 @@ public ModuleIOSim(ModuleConfig config) { @Override public void updateInputs(ModuleIOInputs inputs) { + if (DriverStation.isDisabled()) { + stop(); + } + driveSim.update(Constants.loopPeriodSecs); turnSim.update(Constants.loopPeriodSecs); @@ -59,26 +64,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 @@ -93,7 +98,7 @@ public void setTurnPID(double kP, double kI, double kD) { @Override public void stop() { - driveSim.setInputVoltage(0.0); - turnSim.setInputVoltage(0.0); + runDriveVolts(0.0); + runTurnVolts(0.0); } } 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