diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 6b379e2..6514c16 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -20,7 +20,6 @@ import frc.robot.subsystems.swerve.SwerveDriveSubsystem; import frc.robot.subsystems.transport.TransportSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.utils.RaiderUtils; public class Autos { private final SwerveDriveSubsystem driveSubsystem; @@ -133,11 +132,13 @@ public Command ampSpeakerCloseAmpTwoPieceAuto() { } public Command mobilitySourceSideAuto() { - return Commands.sequence(autoStart(), followPathCommand("MobilitySourceSide", true, driveSubsystem)); + return Commands.sequence( + autoStart(), followPathCommand("MobilitySourceSide", true, driveSubsystem)); } public Command mobilityAmpSideAuto() { - return Commands.sequence(autoStart(), followPathCommand("MobilityAmpSide", true, driveSubsystem)); + return Commands.sequence( + autoStart(), followPathCommand("MobilityAmpSide", true, driveSubsystem)); } private Command centerSpeakerCloseMidNote(boolean firstPath) { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ac153af..a1d6002 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -114,18 +114,27 @@ private SwerveConstants() {} public static final double STEER_GEAR_REDUCTION = 150.0 / 7.0; public static final double DRIVE_CURRENT_LIMIT = 60.0; - public static final int STEER_STALL_CURRENT_LIMIT = 45; - public static final int STEER_FREE_CURRENT_LIMIT = 25; + public static final double STEER_CURRENT_LIMIT = 35.0; // 0.47 public static final TunablePIDGains DRIVE_VELOCITY_PID_GAINS = new TunablePIDGains("/gains/drive", 0.2, 0.0, 0.0, MiscConstants.TUNING_MODE); - + // Meters per rotation + private static final double DRIVE_CONVERSION = + (Math.PI * WHEEL_DIAMETER_METERS) / DRIVE_GEAR_REDUCTION; + // Volts/(Meter/Second) to Volts/(Rotation/Second) public static final TunableFFGains DRIVE_VELOCITY_FF_GAINS = - new TunableFFGains("/gains/drive", 0.064022, 1.9843, 0.9255, MiscConstants.TUNING_MODE); + new TunableFFGains( + "/gains/drive", + 0.064022 * DRIVE_CONVERSION, + 1.9843 * DRIVE_CONVERSION, + 0.9255 * DRIVE_CONVERSION, + MiscConstants.TUNING_MODE); public static final TunablePIDGains STEER_POSITION_PID_GAINS = - new TunablePIDGains("/gains/steer", 1.0, 0.0, 0.1, MiscConstants.TUNING_MODE); + new TunablePIDGains("/gains/steer", 7.207, 0.0, 0.002, MiscConstants.TUNING_MODE); + public static final TunableFFGains STEER_VELOCITY_FF_GAINS = + new TunableFFGains("/gains/steer", 0.0, 0.0, 0.0, MiscConstants.TUNING_MODE); // Left right distance between center of wheels public static final double TRACKWIDTH_METERS = Units.inchesToMeters(22.75); @@ -154,9 +163,9 @@ private SwerveConstants() {} DRIVE_GEAR_REDUCTION, STEER_GEAR_REDUCTION, DRIVE_CURRENT_LIMIT, - STEER_FREE_CURRENT_LIMIT, - STEER_STALL_CURRENT_LIMIT, + STEER_CURRENT_LIMIT, WHEEL_DIAMETER_METERS, + MAX_VELOCITY_METERS_SECOND, ODOMETRY_FREQUENCY, DRIVE_VELOCITY_PID_GAINS, DRIVE_VELOCITY_FF_GAINS, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a69824b..1adc6d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -93,8 +93,7 @@ private void configureAutos() { autoCommand.addOption( "Center Speaker Two Piece Close Mid", autos.centerSpeakerCloseMidTwoPieceAuto()); autoCommand.addOption( - "Center Speaker Three Piece Close Amp Mid", - autos.centerSpeakerCloseAmpMidThreePieceAuto()); + "Center Speaker Three Piece Close Amp Mid", autos.centerSpeakerCloseAmpMidThreePieceAuto()); autoCommand.addOption( "Center Speaker Three Piece Close Source Mid", autos.centerSpeakerCloseSourceMidThreePieceAuto()); @@ -230,9 +229,7 @@ private void configureDriving() { TunableDouble maxMaxAngularSpeedPercent = new TunableDouble("/speed/maxAngular", 0.6, true); DoubleSupplier maxTranslationalSpeedSuppler = - () -> - maxTranslationSpeedPercent.get() - * SwerveConstants.MAX_VELOCITY_METERS_SECOND; + () -> maxTranslationSpeedPercent.get() * SwerveConstants.MAX_VELOCITY_METERS_SECOND; DoubleSupplier maxAngularSpeedSupplier = () -> maxMaxAngularSpeedPercent.get() * SwerveConstants.MAX_ANGULAR_VELOCITY_RADIANS_SECOND; diff --git a/src/main/java/frc/robot/commands/drive/auto/FollowPathCommand.java b/src/main/java/frc/robot/commands/drive/auto/FollowPathCommand.java index 76050be..b8688ec 100644 --- a/src/main/java/frc/robot/commands/drive/auto/FollowPathCommand.java +++ b/src/main/java/frc/robot/commands/drive/auto/FollowPathCommand.java @@ -41,12 +41,16 @@ public class FollowPathCommand extends Command { private final Timer timer = new Timer(); private ChoreoTrajectory currentPath; - public FollowPathCommand(ChoreoTrajectory path, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) { + public FollowPathCommand( + ChoreoTrajectory path, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) { this(path, resetOdometry, true, driveSubsystem); } public FollowPathCommand( - ChoreoTrajectory path, boolean resetOdometry, boolean shouldFlipIfRed, SwerveDriveSubsystem driveSubsystem) { + ChoreoTrajectory path, + boolean resetOdometry, + boolean shouldFlipIfRed, + SwerveDriveSubsystem driveSubsystem) { this( () -> { if (shouldFlipIfRed && (RaiderUtils.shouldFlip())) { @@ -59,7 +63,9 @@ public FollowPathCommand( } public FollowPathCommand( - Supplier pathSupplier, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) { + Supplier pathSupplier, + boolean resetOdometry, + SwerveDriveSubsystem driveSubsystem) { this.pathSupplier = pathSupplier; this.resetOdometry = resetOdometry; this.driveSubsystem = driveSubsystem; diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index be4f67e..4ee1d10 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -19,7 +19,10 @@ public class ClimberSubsystem extends SubsystemBase { private final TelemetryTalonFX climberMotor = new TelemetryTalonFX( - CLIMBER_MOTOR_ID, "/climber/motor", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE); + CLIMBER_MOTOR_ID, + "/climber/motor", + MiscConstants.CANIVORE_NAME, + MiscConstants.TUNING_MODE); private final EventTelemetryEntry eventEntry = new EventTelemetryEntry("/climber/events"); private final DoubleTelemetryEntry voltageReqEntry = new DoubleTelemetryEntry("/climber/voltageReq", MiscConstants.TUNING_MODE); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java index e33e1b6..a9ebbcd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java @@ -98,7 +98,8 @@ enum DriveMode { private final Function> cameraPoseDataSupplier; private final TelemetryPigeon2 pigeon = - new TelemetryPigeon2(PIGEON_ID, "/drive/gyro", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE); + new TelemetryPigeon2( + PIGEON_ID, "/drive/gyro", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE); private StatusSignal yawSignal; @@ -113,11 +114,15 @@ enum DriveMode { null, this)); + private final SysIdRoutine steerPositionSysId = + new SysIdRoutine( + new SysIdRoutine.Config( + null, null, null, (state) -> SignalLogger.writeString("State", state.toString())), + new SysIdRoutine.Mechanism( + (Measure voltage) -> setRawVolts(0.0, voltage.in(Volts)), null, this)); + private final Alert pigeonConfigurationAlert = new Alert("Pigeon failed to initialize", Alert.AlertType.ERROR); - private final BooleanTelemetryEntry allModulesAtAbsoluteZeroEntry = - new BooleanTelemetryEntry("/drive/allModulesAtAbsoluteZero", true); - private final DoubleTelemetryEntry gyroEntry = new DoubleTelemetryEntry("/drive/gyroRadians", true); @@ -327,20 +332,6 @@ public double[] getActualDriveVoltages() { return voltages; } - public void setAllModulesToAbsolute() { - for (SwerveModule module : modules) { - module.resetSteerToAbsolute(); - } - } - - private boolean allModulesAtAbsolute() { - boolean allSet = true; - for (SwerveModule module : modules) { - allSet &= module.isSetToAbsolute(); - } - return allSet; - } - /** * Should only be used for characterization * @@ -376,14 +367,22 @@ public SwerveModulePosition[] getModulePositions() { return actualPositions; } - public Command quasistaticSysIDCommand(SysIdRoutine.Direction direction) { + public Command driveQuasistaticSysIDCommand(SysIdRoutine.Direction direction) { return driveVelocitySysId.quasistatic(direction).beforeStarting(SignalLogger::start); } - public Command dynamicSysIDCommand(SysIdRoutine.Direction direction) { + public Command driveDynamicSysIDCommand(SysIdRoutine.Direction direction) { return driveVelocitySysId.dynamic(direction).beforeStarting(SignalLogger::start); } + public Command steerQuasistaticSysIDCommand(SysIdRoutine.Direction direction) { + return steerPositionSysId.quasistatic(direction).beforeStarting(SignalLogger::start); + } + + public Command steerDynamicSysIDCommand(SysIdRoutine.Direction direction) { + return steerPositionSysId.dynamic(direction).beforeStarting(SignalLogger::start); + } + @Override public void periodic() { switch (driveMode) { @@ -401,7 +400,7 @@ public void periodic() { } case CHARACTERIZATION -> { for (SwerveModule module : modules) { - module.setCharacterizationVoltage(rawDriveVolts); + module.setDriveCharacterizationVoltage(rawDriveVolts); } } } @@ -418,7 +417,6 @@ public void periodic() { } private void logValues() { - allModulesAtAbsoluteZeroEntry.append(allModulesAtAbsolute()); gyroEntry.append(getGyroRotation().getRadians()); chassisSpeedsEntry.append(getCurrentChassisSpeeds()); actualSwerveStatesEntry.append(getActualStates()); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index e259964..34222cd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -4,30 +4,25 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.*; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants.MiscConstants; -import frc.robot.Constants.SwerveConstants; import frc.robot.telemetry.tunable.gains.TunableFFGains; import frc.robot.telemetry.tunable.gains.TunablePIDGains; import frc.robot.telemetry.types.*; import frc.robot.telemetry.wrappers.TelemetryCANCoder; -import frc.robot.telemetry.wrappers.TelemetryCANSparkMax; import frc.robot.telemetry.wrappers.TelemetryTalonFX; import frc.robot.utils.*; import frc.robot.utils.Alert.AlertType; @@ -51,39 +46,27 @@ private enum SwerveModuleControlMode { private final int instanceId; - private final DoubleTelemetryEntry driveVelocitySetpointEntry, - steerPositionGoalEntry, - feedForwardOutputEntry; - private final BooleanTelemetryEntry activeSteerEntry, setToAbsoluteEntry, openLoopEntry; + private final DoubleTelemetryEntry driveVelocitySetpointEntry, steerPositionGoalEntry; + private final BooleanTelemetryEntry activeSteerEntry, openLoopEntry; private final IntegerTelemetryEntry controlModeEntry; private final EventTelemetryEntry moduleEventEntry; - private final Alert notSetToAbsoluteAlert, - steerEncoderFaultAlert, - steerMotorFaultAlert, - driveMotorFaultAlert; + private final Alert steerEncoderFaultAlert, steerMotorFaultAlert, driveMotorFaultAlert; private final TelemetryTalonFX driveMotor; - private final TelemetryCANSparkMax steerMotor; + private final TelemetryTalonFX steerMotor; private final TelemetryCANCoder absoluteSteerEncoder; - private RelativeEncoder steerRelativeEncoder; private StatusSignal absoluteSteerPositionSignal; private StatusSignal drivePositionSignal, driveVelocitySignal; - private final double drivePositionConversion, - driveVelocityConversion, - steerPositionConversion, - steerVelocityConversion; + private StatusSignal steerPositionSignal, steerVelocitySignal; + private final double driveConversion, steerConversion; + + private final double maxDriveVelocityMetersPerSecond; private final TunablePIDGains driveVelocityPIDGains; private final TunableFFGains driveVelocityFFGains; private final TunablePIDGains steerPositionPIDGains; - private SimpleMotorFeedforward driveMotorFF; - private SparkPIDController steerController; - - private boolean setToAbsolute = false; - private double lastMoveTime = 0.0; - private double lastAbsoluteResetTime = 0.0; /** Constructs a new Swerve Module using the given config */ public SwerveModule(SwerveModuleConfiguration config, boolean tuningMode) { @@ -94,17 +77,13 @@ public SwerveModule(SwerveModuleConfiguration config, boolean tuningMode) { driveVelocitySetpointEntry = new DoubleTelemetryEntry(tableName + "driveVelocitySetpoint", true); openLoopEntry = new BooleanTelemetryEntry(tableName + "openLoop", tuningMode); - feedForwardOutputEntry = new DoubleTelemetryEntry(tableName + "feedforwardOutput", tuningMode); steerPositionGoalEntry = new DoubleTelemetryEntry(tableName + "steerPositionGoal", true); activeSteerEntry = new BooleanTelemetryEntry(tableName + "activeSteer", tuningMode); - setToAbsoluteEntry = new BooleanTelemetryEntry(tableName + "setToAbsolute", true); controlModeEntry = new IntegerTelemetryEntry(tableName + "controlMode", false); moduleEventEntry = new EventTelemetryEntry(tableName + "events"); // Initialize all alerts String alertPrefix = "Module " + instanceId + ": "; - notSetToAbsoluteAlert = - new Alert(alertPrefix + "Steer is not reset to absolute position", AlertType.ERROR); steerEncoderFaultAlert = new Alert(alertPrefix + "Steer encoder had a fault initializing", AlertType.ERROR); steerMotorFaultAlert = @@ -112,19 +91,18 @@ public SwerveModule(SwerveModuleConfiguration config, boolean tuningMode) { driveMotorFaultAlert = new Alert(alertPrefix + "Drive motor had a fault initializing", AlertType.ERROR); - drivePositionConversion = + driveConversion = (config.sharedConfiguration().wheelDiameterMeters() * Math.PI) / (config.sharedConfiguration().driveGearRatio()); - driveVelocityConversion = drivePositionConversion; - steerPositionConversion = (Math.PI * 2) / (config.sharedConfiguration().steerGearRatio()); - steerVelocityConversion = steerPositionConversion / 60.0; + // Steer uses CANCoder which is one to one + steerConversion = (Math.PI * 2); + + maxDriveVelocityMetersPerSecond = config.sharedConfiguration().freeSpeedMetersPerSecond(); driveVelocityPIDGains = config.sharedConfiguration().driveVelocityPIDGains(); driveVelocityFFGains = config.sharedConfiguration().driveVelocityFFGains(); steerPositionPIDGains = config.sharedConfiguration().steerPositionPIDGains(); - driveMotorFF = driveVelocityFFGains.createFeedforward(); - // Drive motor driveMotor = new TelemetryTalonFX( @@ -148,13 +126,12 @@ public SwerveModule(SwerveModuleConfiguration config, boolean tuningMode) { // Steer motor steerMotor = - new TelemetryCANSparkMax( + new TelemetryTalonFX( config.steerMotorID(), - CANSparkMax.MotorType.kBrushless, tableName + "steerMotor", + config.sharedConfiguration().canBus(), tuningMode); configSteerMotor(config); - resetSteerToAbsolute(0.25); } private void configDriveMotor(SwerveModuleConfiguration config) { @@ -213,117 +190,73 @@ private void configDriveMotor(SwerveModuleConfiguration config) { faultRecorder.getFaultString()); driveMotorFaultAlert.set(faultRecorder.hasFault()); - driveMotor.setLoggingPositionConversionFactor(drivePositionConversion); - driveMotor.setLoggingVelocityConversionFactor(driveVelocityConversion); + driveMotor.setLoggingPositionConversionFactor(driveConversion); + driveMotor.setLoggingVelocityConversionFactor(driveConversion); // Clear reset as this is on startup driveMotor.hasResetOccurred(); } private void configSteerMotor(SwerveModuleConfiguration config) { - steerRelativeEncoder = steerMotor.getEncoder(); - steerController = steerMotor.getPIDController(); + TalonFXConfiguration motorConfiguration = new TalonFXConfiguration(); + motorConfiguration.CurrentLimits.SupplyCurrentLimit = + config.sharedConfiguration().steerCurrentLimit(); + motorConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; + motorConfiguration.MotorOutput.Inverted = + config.steerMotorInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + motorConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; + motorConfiguration.Feedback.FeedbackRemoteSensorID = config.steerEncoderID(); + motorConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + motorConfiguration.Feedback.RotorToSensorRatio = config.sharedConfiguration().steerGearRatio(); + motorConfiguration.ClosedLoopGeneral.ContinuousWrap = true; + + config.sharedConfiguration().steerPositionPIDGains().setSlot(motorConfiguration.Slot0); + // Came from the CTRE Swerve Module example + motorConfiguration.MotionMagic.MotionMagicCruiseVelocity = + 100.0 / config.sharedConfiguration().steerGearRatio(); + motorConfiguration.MotionMagic.MotionMagicAcceleration = + motorConfiguration.MotionMagic.MotionMagicCruiseVelocity / 0.100; + motorConfiguration.MotionMagic.MotionMagicExpo_kV = + 0.12 * config.sharedConfiguration().steerGearRatio(); + motorConfiguration.MotionMagic.MotionMagicExpo_kA = 0.1; + + steerPositionSignal = steerMotor.getPosition(); + steerVelocitySignal = steerMotor.getVelocity(); StringFaultRecorder faultRecorder = new StringFaultRecorder(); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerMotor.setCANTimeout(250), - () -> true, - faultRecorder.run("CAN timeout"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - steerMotor::restoreFactoryDefaults, - () -> true, - faultRecorder.run("Factory default"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> - steerMotor.setSmartCurrentLimit( - config.sharedConfiguration().steerStallCurrentLimit(), - config.sharedConfiguration().steerFreeCurrentLimit()), - () -> true, - faultRecorder.run("Current limit"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecord( + ConfigurationUtils.applyCheckRecordCTRE( + () -> steerMotor.getConfigurator().apply(motorConfiguration), () -> { - steerMotor.setInverted(config.steerMotorInverted()); + TalonFXConfiguration appliedConfig = new TalonFXConfiguration(); + steerMotor.getConfigurator().refresh(appliedConfig); + return ConfigEquality.isTalonConfigurationEqual(motorConfiguration, appliedConfig); }, - () -> steerMotor.getInverted() == config.steerMotorInverted(), - faultRecorder.run("Invert"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerController.setP(steerPositionPIDGains.p.get()), - () -> ConfigurationUtils.fpEqual(steerController.getP(), steerPositionPIDGains.p.get()), - faultRecorder.run("P gain"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerController.setI(steerPositionPIDGains.i.get()), - () -> ConfigurationUtils.fpEqual(steerController.getI(), steerPositionPIDGains.i.get()), - faultRecorder.run("I gain"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerController.setD(steerPositionPIDGains.d.get()), - () -> ConfigurationUtils.fpEqual(steerController.getD(), steerPositionPIDGains.d.get()), - faultRecorder.run("D gain"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerController.setPositionPIDWrappingEnabled(true), - () -> steerController.getPositionPIDWrappingEnabled(), - faultRecorder.run("PID wrapping"), + faultRecorder.run("Motor configuration"), MiscConstants.CONFIGURATION_ATTEMPTS); - ConfigurationUtils.applyCheckRecordRev( - () -> steerController.setPositionPIDWrappingMinInput(-Math.PI), + ConfigurationUtils.applyCheckRecordCTRE( () -> - ConfigurationUtils.fpEqual(steerController.getPositionPIDWrappingMinInput(), -Math.PI), - faultRecorder.run("PID wrapping min input"), - MiscConstants.CONFIGURATION_ATTEMPTS); - ConfigurationUtils.applyCheckRecordRev( - () -> steerController.setPositionPIDWrappingMaxInput(Math.PI), - () -> ConfigurationUtils.fpEqual(steerController.getPositionPIDWrappingMaxInput(), Math.PI), - faultRecorder.run("PID wrapping max input"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerRelativeEncoder.setPositionConversionFactor(steerPositionConversion), + steerPositionSignal.setUpdateFrequency( + config.sharedConfiguration().odometryFrequency()), () -> - ConfigurationUtils.fpEqual( - steerRelativeEncoder.getPositionConversionFactor(), steerPositionConversion), - faultRecorder.run("Position conversion factor"), + steerPositionSignal.getAppliedUpdateFrequency() + == config.sharedConfiguration().odometryFrequency(), + faultRecorder.run("Position signal update frequency"), MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerRelativeEncoder.setVelocityConversionFactor(steerVelocityConversion), + ConfigurationUtils.applyCheckRecordCTRE( () -> - ConfigurationUtils.fpEqual( - steerRelativeEncoder.getVelocityConversionFactor(), steerVelocityConversion), - faultRecorder.run("Velocity conversion factor"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - () -> steerMotor.setIdleMode(IdleMode.kBrake), - () -> steerMotor.getIdleMode() == IdleMode.kBrake, - faultRecorder.run("Idle mode"), - MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( + steerVelocitySignal.setUpdateFrequency( + config.sharedConfiguration().odometryFrequency()), () -> - steerMotor.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, 1000 / SwerveConstants.ODOMETRY_FREQUENCY), - () -> true, - faultRecorder.run("Status 2 frame period"), + steerVelocitySignal.getAppliedUpdateFrequency() + == config.sharedConfiguration().odometryFrequency(), + faultRecorder.run("Velocity signal update frequency"), MiscConstants.CONFIGURATION_ATTEMPTS); - - ConfigurationUtils.applyCheckRecordRev( - steerMotor::burnFlashWithDelay, + ConfigurationUtils.applyCheckRecordCTRE( + steerMotor::optimizeBusUtilization, () -> true, - faultRecorder.run("Burn flash"), + faultRecorder.run("Optimize bus utilization"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.postDeviceConfig( @@ -332,6 +265,9 @@ private void configSteerMotor(SwerveModuleConfiguration config) { "Steer motor module " + instanceId, faultRecorder.getFaultString()); steerMotorFaultAlert.set(faultRecorder.hasFault()); + + steerMotor.setLoggingPositionConversionFactor(steerConversion); + steerMotor.setLoggingVelocityConversionFactor(steerConversion); } private void configSteerEncoder(SwerveModuleConfiguration config) { @@ -378,10 +314,8 @@ private void configSteerEncoder(SwerveModuleConfiguration config) { private void checkForSteerMotorReset() { // Steer motor lost power - if (RobotBase.isReal() && steerMotor.getFault(CANSparkBase.FaultID.kHasReset)) { + if (RobotBase.isReal() && steerMotor.hasResetOccurred()) { reportError("Steer motor reset occurred"); - setToAbsolute = false; - resetSteerToAbsolute(); } } @@ -391,59 +325,25 @@ private void checkForDriveMotorReset() { } } - /** - * Resets the integrated encoder on the Steer motor to the absolute position of the CANCoder. Trys - * only once, and if it fails, it will not try again until - */ - public void resetSteerToAbsolute() { - resetSteerToAbsolute(0.0); - } - - /** - * Resets the integrated encoder on the Steer motor to the absolute position of the CANCoder - * - * @param timeout The timeout in seconds to wait. - */ - public void resetSteerToAbsolute(double timeout) { - double absolutePosition = - Units.rotationsToRadians(absoluteSteerPositionSignal.waitForUpdate(timeout).getValue()); - if (absoluteSteerPositionSignal.getStatus().isOK()) { - REVLibError settingPositionError = steerRelativeEncoder.setPosition(absolutePosition); - if (RaiderUtils.isRevOk(settingPositionError)) { - setToAbsolute = true; - lastAbsoluteResetTime = Timer.getFPGATimestamp(); - moduleEventEntry.append("Reset steer motor encoder to position: " + absolutePosition); - } else { - reportError("Failed to set SparkMax to absolute position: " + settingPositionError); - } - } else { - reportError("Failed to get absolute position: " + absoluteSteerPositionSignal.getStatus()); - } - } - - public boolean isSetToAbsolute() { - return setToAbsolute; - } - - private double getSteerAngleRadiansNoWrap() { - return steerRelativeEncoder.getPosition(); - } - /** * @return the rotation of the wheel */ private Rotation2d getSteerAngle() { - return Rotation2d.fromRadians(MathUtil.angleModulus(getSteerAngleRadiansNoWrap())); + StatusSignal.refreshAll(steerPositionSignal, steerVelocitySignal); + return Rotation2d.fromRadians( + MathUtil.angleModulus( + StatusSignal.getLatencyCompensatedValue(steerPositionSignal, steerVelocitySignal) + * steerConversion)); } private double getDriveVelocityMetersPerSecond() { - return driveVelocitySignal.refresh().getValue() * driveVelocityConversion; + return driveVelocitySignal.refresh().getValue() * driveConversion; } private double getDriveMotorPositionMeters() { StatusSignal.refreshAll(drivePositionSignal, driveVelocitySignal); return StatusSignal.getLatencyCompensatedValue(drivePositionSignal, driveVelocitySignal) - * drivePositionConversion; + * driveConversion; } /** @@ -479,70 +379,67 @@ public void setDesiredState(SwerveModuleState state, boolean activeSteer, boolea controlModeEntry.append(SwerveModuleControlMode.NORMAL.logValue); - if (state.speedMetersPerSecond != 0.0 || activeSteer) { - lastMoveTime = Timer.getFPGATimestamp(); - } - state = SwerveModuleState.optimize(state, getSteerAngle()); + // Account for steer error to reduce skew + double steerErrorRadians = state.angle.getRadians() - getSteerAngle().getRadians(); + double cosineScalar = Math.cos(steerErrorRadians); + if (cosineScalar < 0.0) { + cosineScalar = 0.0; + } + state.speedMetersPerSecond *= cosineScalar; + setDriveReference(state.speedMetersPerSecond, openLoop); setSteerReference(state.angle.getRadians(), activeSteer); - - if (shouldResetToAbsolute()) { - resetSteerToAbsolute(); - } } - private boolean shouldResetToAbsolute() { - double currentTime = Timer.getFPGATimestamp(); - // If we have not reset in 5 seconds, been still for 1.5 seconds and our steer - // velocity is less than half a degree per second (could happen if we are being - // pushed), reset to absolute - return DriverStation.isDisabled() - && currentTime - lastAbsoluteResetTime > 5.0 - && currentTime - lastMoveTime > 1.5 - && Math.abs(Units.rotationsToRadians(absoluteSteerEncoder.getVelocity().getValue())) - < Units.degreesToRadians(0.5); - } + private final VoltageOut driveVoltageOut = new VoltageOut(0.0); - public void setCharacterizationVoltage(double voltage) { + public void setDriveCharacterizationVoltage(double voltage) { controlModeEntry.append(SwerveModuleControlMode.CHARACTERIZATION.logValue); - driveMotor.setControl(new VoltageOut(voltage)); + driveMotor.setControl(driveVoltageOut.withOutput(voltage)); setSteerReference(0.0, true); } + private final VoltageOut steerVoltageOut = new VoltageOut(0.0); + public void setRawVoltage(double driveVolts, double steerVolts) { controlModeEntry.append(SwerveModuleControlMode.RAW_VOLTAGE.logValue); - driveMotor.setControl(new VoltageOut(driveVolts)); - steerMotor.setVoltage(steerVolts); + driveMotor.setControl(driveVoltageOut.withOutput(driveVolts)); + steerMotor.setControl(steerVoltageOut.withOutput(steerVolts)); } + private final MotionMagicExpoVoltage steerMotionMagicExpoVoltage = + new MotionMagicExpoVoltage(0.0).withUpdateFreqHz(0.0); + private void setSteerReference(double targetAngleRadians, boolean activeSteer) { activeSteerEntry.append(activeSteer); steerPositionGoalEntry.append(targetAngleRadians); if (activeSteer) { - steerController.setReference(targetAngleRadians, CANSparkMax.ControlType.kPosition); + steerMotor.setControl( + steerMotionMagicExpoVoltage.withPosition(Units.radiansToRotations(targetAngleRadians))); } else { steerMotor.setVoltage(0.0); } } + private final VelocityVoltage driveVelocityVoltage = + new VelocityVoltage(0.0).withUpdateFreqHz(0.0); + private void setDriveReference(double targetVelocityMetersPerSecond, boolean openLoop) { driveVelocitySetpointEntry.append(targetVelocityMetersPerSecond); openLoopEntry.append(openLoop); - double feedforwardValueVoltage = driveMotorFF.calculate(targetVelocityMetersPerSecond); - feedForwardOutputEntry.append(feedforwardValueVoltage); - if (openLoop) { - driveMotor.setControl(new VoltageOut(feedforwardValueVoltage)); + driveMotor.setControl( + driveVoltageOut.withOutput( + targetVelocityMetersPerSecond / maxDriveVelocityMetersPerSecond * 12.0)); } else { driveMotor.setControl( - new VelocityVoltage(targetVelocityMetersPerSecond / driveVelocityConversion) - .withFeedForward(feedforwardValueVoltage)); + driveVelocityVoltage.withVelocity(targetVelocityMetersPerSecond / driveConversion)); } } @@ -550,16 +447,16 @@ private void checkAndUpdateGains() { if (driveVelocityPIDGains.hasChanged() || driveVelocityFFGains.hasChanged()) { Slot0Configs newSlotConfig = new Slot0Configs(); driveVelocityPIDGains.setSlot(newSlotConfig); + driveVelocityFFGains.setSlot(newSlotConfig); driveMotor.getConfigurator().apply(newSlotConfig); - driveMotorFF = driveVelocityFFGains.createFeedforward(); moduleEventEntry.append("Updated drive gains due to value change"); } if (steerPositionPIDGains.hasChanged()) { - steerController.setP(steerPositionPIDGains.p.get()); - steerController.setD(steerPositionPIDGains.d.get()); - steerController.setI(steerPositionPIDGains.i.get()); + Slot0Configs newSlotConfig = new Slot0Configs(); + steerPositionPIDGains.setSlot(newSlotConfig); + steerMotor.getConfigurator().apply(newSlotConfig); moduleEventEntry.append("Updated steer gains due to value change"); } @@ -580,8 +477,5 @@ public void logValues() { driveMotor.logValues(); steerMotor.logValues(); absoluteSteerEncoder.logValues(); - - notSetToAbsoluteAlert.set(!setToAbsolute); - setToAbsoluteEntry.append(setToAbsolute); } } diff --git a/src/main/java/frc/robot/telemetry/tunable/gains/TunableFFGains.java b/src/main/java/frc/robot/telemetry/tunable/gains/TunableFFGains.java index fd55737..a68c129 100644 --- a/src/main/java/frc/robot/telemetry/tunable/gains/TunableFFGains.java +++ b/src/main/java/frc/robot/telemetry/tunable/gains/TunableFFGains.java @@ -1,5 +1,6 @@ package frc.robot.telemetry.tunable.gains; +import com.ctre.phoenix6.configs.Slot0Configs; import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class TunableFFGains { @@ -22,6 +23,12 @@ public TunableFFGains( this.aFF = new TunableDouble(networkName + "aFF", aFF, tuningMode); } + public void setSlot(Slot0Configs slot) { + slot.kS = sFF.get(); + slot.kV = vFF.get(); + slot.kA = aFF.get(); + } + public boolean hasChanged() { return sFF.hasChanged() || vFF.hasChanged() || aFF.hasChanged(); } diff --git a/src/main/java/frc/robot/utils/SwerveModuleConfiguration.java b/src/main/java/frc/robot/utils/SwerveModuleConfiguration.java index 5263b28..3d03f02 100644 --- a/src/main/java/frc/robot/utils/SwerveModuleConfiguration.java +++ b/src/main/java/frc/robot/utils/SwerveModuleConfiguration.java @@ -18,9 +18,9 @@ public record SharedSwerveModuleConfiguration( double driveGearRatio, double steerGearRatio, double driveCurrentLimit, - int steerFreeCurrentLimit, - int steerStallCurrentLimit, + double steerCurrentLimit, double wheelDiameterMeters, + double freeSpeedMetersPerSecond, int odometryFrequency, TunablePIDGains driveVelocityPIDGains, TunableFFGains driveVelocityFFGains,