From f5dd2ddfe8368cf73a73d18cc06e71386beb946f Mon Sep 17 00:00:00 2001 From: suryatho Date: Tue, 13 Feb 2024 23:28:58 -0500 Subject: [PATCH] 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