From f5dd2ddfe8368cf73a73d18cc06e71386beb946f Mon Sep 17 00:00:00 2001 From: suryatho Date: Tue, 13 Feb 2024 23:28:58 -0500 Subject: [PATCH 1/8] Set turn motor position to absolute position at start up Use pre-defined members for control rather than creating new control request each cycle Use feedforward input from module always Run drive and turn with amps Make every control request one shot Make alerts for drive and turn disconnected Rename some methods in ModuleIO --- .../littletonrobotics/frc2024/Constants.java | 2 +- .../frc2024/subsystems/drive/Drive.java | 6 +- .../subsystems/drive/DriveConstants.java | 9 -- .../frc2024/subsystems/drive/Module.java | 56 ++++++-- .../frc2024/subsystems/drive/ModuleIO.java | 25 ++-- .../subsystems/drive/ModuleIOKrakenFOC.java | 129 ++++++++---------- .../frc2024/subsystems/drive/ModuleIOSim.java | 14 +- .../subsystems/drive/ModuleIOSparkMax.java | 12 +- 8 files changed, 131 insertions(+), 122 deletions(-) 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 From 976a7c3f3c73928423f757dd232dcbeb67359974 Mon Sep 17 00:00:00 2001 From: suryatho Date: Wed, 14 Feb 2024 06:44:45 -0500 Subject: [PATCH 2/8] Use odometry timestamps for filtering --- .../frc2024/subsystems/drive/Drive.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) 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 1279058f..27d9a8c9 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -95,7 +95,7 @@ public static class OdometryTimestampInputs { new SwerveModuleState(), new SwerveModuleState() }); - private SwerveSetpointGenerator setpointGenerator; + private final SwerveSetpointGenerator setpointGenerator; private final TeleopDriveController teleopDriveController; private TrajectoryController trajectoryController = null; @@ -150,7 +150,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 @@ -160,10 +159,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) @@ -172,8 +172,8 @@ public void periodic() { 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) { + if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 2.0 + || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 2.0) { includeMeasurement = false; break; } @@ -185,9 +185,9 @@ public void periodic() { .addOdometryObservation( new RobotState.OdometryObservation( wheelPositions, yaw, odometryTimestampInputs.timestamps[i])); + lastTime = odometryTimestampInputs.timestamps[i]; } } - lastTime = Timer.getFPGATimestamp(); // update current velocities use gyro when possible ChassisSpeeds robotRelativeVelocity = getSpeeds(); From b3980c03263a3ddad75d171b584d7cd6ad202887 Mon Sep 17 00:00:00 2001 From: suryatho Date: Wed, 14 Feb 2024 22:57:36 -0500 Subject: [PATCH 3/8] Use isOK() method --- .../frc2024/subsystems/drive/Module.java | 2 +- .../frc2024/subsystems/drive/ModuleIOKrakenFOC.java | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) 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 2e3deddd..b41e9d40 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java @@ -73,7 +73,7 @@ public void updateInputs() { LoggedTunableNumber.ifChanged( hashCode(), () -> io.setTurnPID(turnkP.get(), 0, turnkD.get()), turnkP, turnkD); - // Display warnings + // Display alerts driveMotorDisconnected.set(!inputs.driveMotorConnected); turnMotorDisconnected.set(!inputs.turnMotorConnected); } 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 c750bc94..32742835 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOKrakenFOC.java @@ -63,8 +63,8 @@ public class ModuleIOKrakenFOC implements ModuleIO { 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); + 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 @@ -156,11 +156,11 @@ public void updateInputs(ModuleIOInputs inputs) { driveAppliedVolts, driveSupplyCurrent, driveTorqueCurrent) - == StatusCode.OK; + .isOK(); inputs.turnMotorConnected = BaseStatusSignal.refreshAll( turnPosition, turnVelocity, turnAppliedVolts, turnSupplyCurrent, turnTorqueCurrent) - == StatusCode.OK; + .isOK(); inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); @@ -247,7 +247,7 @@ public void setTurnBrakeMode(boolean enable) { @Override public void stop() { - driveTalon.setControl(driveBrake); - turnTalon.setControl(turnBrake); + driveTalon.setControl(driveNeutral); + turnTalon.setControl(turnNeutral); } } From 2f4ab73d604378a8cee7c6d2effbd42ee673281f Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 20:29:09 -0500 Subject: [PATCH 4/8] Fix --- .../littletonrobotics/frc2024/Constants.java | 2 +- .../frc2024/subsystems/drive/Drive.java | 32 +++++++------------ .../drive/controllers/AutoAimController.java | 3 +- 3 files changed, 14 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index d19fc867..6bebedc2 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.SIMBOT; + private static RobotType robotType = RobotType.DEVBOT; 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 27d9a8c9..5c4a1296 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -73,6 +73,7 @@ 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; @@ -171,9 +172,9 @@ public void periodic() { double omega = wheelPositions.positions[j].angle.minus(lastPositions.positions[j].angle).getRadians() / dt; - - if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 2.0 - || Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 2.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; } @@ -200,24 +201,15 @@ public void periodic() { // 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(); + if (brakeModeEnabled) { + if (coastTimer.hasElapsed(coastDisableTime.get())) { + setBrakeMode(false); + coastTimer.stop(); + coastTimer.reset(); + } else { + coastTimer.start(); + } } - return; - } else { - // Brake mode - setBrakeMode(true); } // Run drive based on current mode diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java index c62387c1..8ca1b0ed 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/AutoAimController.java @@ -22,8 +22,7 @@ public class AutoAimController { new LoggedTunableNumber("AutoAim/kD", DriveConstants.headingControllerConstants.kD()); private static final LoggedTunableNumber tolerance = new LoggedTunableNumber("AutoAim/ToleranceDegrees", 4.0); - - private PIDController headingController; + private final PIDController headingController; public AutoAimController() { headingController = new PIDController(0, 0, 0, 0.02); From 9afd7eb431f02169f03521f84a1b7786493bb97d Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 20:51:20 -0500 Subject: [PATCH 5/8] Fix coast timer --- .../frc2024/subsystems/drive/Drive.java | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) 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 5c4a1296..50d84eca 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -27,6 +27,7 @@ import org.littletonrobotics.frc2024.subsystems.drive.controllers.TeleopDriveController; import org.littletonrobotics.frc2024.subsystems.drive.controllers.TrajectoryController; import org.littletonrobotics.frc2024.subsystems.drive.trajectory.HolonomicTrajectory; +import org.littletonrobotics.frc2024.util.EqualsUtil; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.frc2024.util.swerve.ModuleLimits; @@ -38,11 +39,8 @@ @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); public enum DriveMode { /** Driving with input from driver joysticks. (Default) */ @@ -109,6 +107,7 @@ 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); + setBrakeMode(true); setpointGenerator = SwerveSetpointGenerator.builder() @@ -201,15 +200,19 @@ public void periodic() { // Disabled, stop modules and coast if (DriverStation.isDisabled()) { Arrays.stream(modules).forEach(Module::stop); - if (brakeModeEnabled) { - if (coastTimer.hasElapsed(coastDisableTime.get())) { - setBrakeMode(false); - coastTimer.stop(); - coastTimer.reset(); - } else { - coastTimer.start(); - } + if (EqualsUtil.epsilonEquals( + Math.hypot( + robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond), + 0.0, + 0.05)) { + coastTimer.start(); + } else if (coastTimer.hasElapsed(coastWaitTime.get())) { + coastTimer.stop(); + coastTimer.reset(); + setBrakeMode(false); } + } else { + setBrakeMode(true); } // Run drive based on current mode @@ -354,8 +357,10 @@ public double getCharacterizationVelocity() { /** Set brake mode enabled */ 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) { From 143c1949e31174ad976161a20186a57a5cc8c4f8 Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 23:39:05 -0500 Subject: [PATCH 6/8] Coast mode logic working --- .../frc2024/subsystems/drive/Drive.java | 42 +++++++++---------- .../frc2024/subsystems/drive/ModuleIOSim.java | 9 +++- 2 files changed, 28 insertions(+), 23 deletions(-) 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 50d84eca..b40acf16 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -20,6 +20,7 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.stream.IntStream; +import lombok.Getter; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.drive.controllers.AutoAimController; @@ -27,7 +28,6 @@ import org.littletonrobotics.frc2024.subsystems.drive.controllers.TeleopDriveController; import org.littletonrobotics.frc2024.subsystems.drive.controllers.TrajectoryController; import org.littletonrobotics.frc2024.subsystems.drive.trajectory.HolonomicTrajectory; -import org.littletonrobotics.frc2024.util.EqualsUtil; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.frc2024.util.swerve.ModuleLimits; @@ -41,6 +41,8 @@ public class Drive extends SubsystemBase { 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) */ @@ -80,8 +82,10 @@ public static class OdometryTimestampInputs { private double characterizationInput = 0.0; private boolean modulesOrienting = false; - private final Timer coastTimer = new Timer(); - private boolean brakeModeEnabled = true; + private final Timer lastMovementTimer = new Timer(); + + @Getter(onMethod_ = @AutoLogOutput(key = "Drive/BrakeModeEnabled")) + boolean brakeModeEnabled = true; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; @@ -107,6 +111,7 @@ 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 = @@ -179,6 +184,7 @@ public void periodic() { } } } + // If delta isn't too large we can include the measurement. if (includeMeasurement) { lastPositions = wheelPositions; RobotState.getInstance() @@ -189,7 +195,7 @@ public void periodic() { } } - // update current velocities use gyro when possible + // Update current velocities use gyro when possible ChassisSpeeds robotRelativeVelocity = getSpeeds(); robotRelativeVelocity.omegaRadiansPerSecond = gyroInputs.connected @@ -197,22 +203,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 (EqualsUtil.epsilonEquals( - Math.hypot( - robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond), - 0.0, - 0.05)) { - coastTimer.start(); - } else if (coastTimer.hasElapsed(coastWaitTime.get())) { - coastTimer.stop(); - coastTimer.reset(); - setBrakeMode(false); - } - } else { - 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 @@ -355,7 +355,7 @@ 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)); 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 606372f6..5c51f37b 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; public class ModuleIOSim implements ModuleIO { @@ -36,6 +37,10 @@ public ModuleIOSim(ModuleConfig config) { @Override public void updateInputs(ModuleIOInputs inputs) { + if (DriverStation.isDisabled()) { + stop(); + } + driveSim.update(0.02); turnSim.update(0.02); @@ -91,7 +96,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); } } From d1d8a31acd8f3eeb67ba276c1f4f75e44d3907bc Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 16 Feb 2024 23:27:04 -0500 Subject: [PATCH 7/8] Use AutoLogOutput annotation on field directly --- .../littletonrobotics/frc2024/subsystems/drive/Drive.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 b40acf16..bcfa1055 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -64,7 +64,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 = @@ -84,8 +83,9 @@ public static class OdometryTimestampInputs { private boolean modulesOrienting = false; private final Timer lastMovementTimer = new Timer(); - @Getter(onMethod_ = @AutoLogOutput(key = "Drive/BrakeModeEnabled")) - boolean brakeModeEnabled = true; + @Getter + @AutoLogOutput(key = "Drive/BrakeModeEnabled") + private boolean brakeModeEnabled = true; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; From d798a83f819c95cbdfd7a1d2aa70659bc8e21464 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Fri, 16 Feb 2024 23:33:37 -0500 Subject: [PATCH 8/8] Fix formatting --- .../littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 08aa3ff9..1044afb6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -42,7 +42,7 @@ public void updateInputs(ModuleIOInputs inputs) { if (DriverStation.isDisabled()) { stop(); } - + driveSim.update(Constants.loopPeriodSecs); turnSim.update(Constants.loopPeriodSecs);