From 681b4f85588abaaa94365e5a17d8df471e74481d Mon Sep 17 00:00:00 2001 From: Jay Date: Mon, 2 Dec 2024 08:19:20 +0000 Subject: [PATCH] update to match 2025 beta changes/deprecations --- build.gradle | 2 +- .../flywheels/FlywheelsIOTalonFX.java | 54 +++++++++---------- .../rollers/GenericRollersIOTalonFX.java | 12 +++-- .../frc/robot/subsystems/swerve/Drive.java | 7 ++- .../subsystems/swerve/GyroIOPigeon2.java | 6 ++- .../subsystems/swerve/ModuleIOTalonFX.java | 24 +++++---- .../swerve/controllers/TeleopController.java | 10 ++++ 7 files changed, 69 insertions(+), 46 deletions(-) diff --git a/build.gradle b/build.gradle index 9b06747..17af8af 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/src/main/java/frc/robot/subsystems/flywheels/FlywheelsIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheels/FlywheelsIOTalonFX.java index 8df6da6..75b8044 100644 --- a/src/main/java/frc/robot/subsystems/flywheels/FlywheelsIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheels/FlywheelsIOTalonFX.java @@ -9,23 +9,29 @@ import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; public class FlywheelsIOTalonFX implements FlywheelsIO { private final TalonFX topTalon; private final TalonFX bottomTalon; - private final StatusSignal topPositionRads; - private final StatusSignal topVelocityRPM; - private final StatusSignal topAppliedVolts; - private final StatusSignal topSupplyCurrent; - private final StatusSignal topTemp; - private final StatusSignal bottomPositionRads; - private final StatusSignal bottomVelocityRPM; - private final StatusSignal bottomAppliedVolts; - private final StatusSignal bottomSupplyCurrent; - private final StatusSignal bottomTemp; + private final StatusSignal topPosition; + private final StatusSignal topVelocity; + private final StatusSignal topAppliedVolts; + private final StatusSignal topSupplyCurrent; + private final StatusSignal topTemp; + private final StatusSignal bottomPosition; + private final StatusSignal bottomVelocity; + private final StatusSignal bottomAppliedVolts; + private final StatusSignal bottomSupplyCurrent; + private final StatusSignal bottomTemp; private final Slot0Configs gainsConfig = new Slot0Configs(); private final VelocityVoltage velocityOutput = new VelocityVoltage(0).withUpdateFreqHz(0); @@ -39,6 +45,7 @@ public FlywheelsIOTalonFX() { config.CurrentLimits.SupplyCurrentLimit = 60.0; // FIXME config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = FLYWHEEL_CONFIG.reduction(); gainsConfig.kP = GAINS.kP(); @@ -53,17 +60,14 @@ public FlywheelsIOTalonFX() { topTalon.getConfigurator().apply(gainsConfig); bottomTalon.getConfigurator().apply(gainsConfig); - topTalon.setInverted(true); - bottomTalon.setInverted(true); // FIXME - - topPositionRads = topTalon.getPosition(); - topVelocityRPM = topTalon.getVelocity(); + topPosition = topTalon.getPosition(); + topVelocity = topTalon.getVelocity(); topAppliedVolts = topTalon.getMotorVoltage(); topSupplyCurrent = topTalon.getSupplyCurrent(); topTemp = topTalon.getDeviceTemp(); - bottomPositionRads = bottomTalon.getPosition(); - bottomVelocityRPM = bottomTalon.getVelocity(); + bottomPosition = bottomTalon.getPosition(); + bottomVelocity = bottomTalon.getVelocity(); bottomAppliedVolts = bottomTalon.getMotorVoltage(); bottomSupplyCurrent = bottomTalon.getSupplyCurrent(); bottomTemp = bottomTalon.getDeviceTemp(); @@ -73,25 +77,21 @@ public FlywheelsIOTalonFX() { public void updateInputs(FlywheelsIOInputs inputs) { inputs.topMotorConnected = BaseStatusSignal.refreshAll( - topPositionRads, topVelocityRPM, topAppliedVolts, topSupplyCurrent, topTemp) + topPosition, topVelocity, topAppliedVolts, topSupplyCurrent, topTemp) .isOK(); inputs.bottomMotorConnected = BaseStatusSignal.refreshAll( - bottomPositionRads, - bottomVelocityRPM, - bottomAppliedVolts, - bottomSupplyCurrent, - bottomTemp) + bottomPosition, bottomVelocity, bottomAppliedVolts, bottomSupplyCurrent, bottomTemp) .isOK(); - inputs.topPositionRads = Units.rotationsToRadians(topPositionRads.getValueAsDouble()); - inputs.topVelocityRPM = topVelocityRPM.getValueAsDouble() * 60.0; + inputs.topPositionRads = Units.rotationsToRadians(topPosition.getValueAsDouble()); + inputs.topVelocityRPM = topVelocity.getValueAsDouble() * 60.0; inputs.topAppliedVolts = topAppliedVolts.getValueAsDouble(); inputs.topSupplyCurrent = topSupplyCurrent.getValueAsDouble(); inputs.topTempCelcius = topTemp.getValueAsDouble(); - inputs.bottomPositionRads = Units.rotationsToRadians(bottomPositionRads.getValueAsDouble()); - inputs.bottomVelocityRPM = bottomVelocityRPM.getValueAsDouble() * 60.0; + inputs.bottomPositionRads = Units.rotationsToRadians(bottomPosition.getValueAsDouble()); + inputs.bottomVelocityRPM = bottomVelocity.getValueAsDouble() * 60.0; inputs.bottomAppliedVolts = bottomAppliedVolts.getValueAsDouble(); inputs.bottomSupplyCurrent = bottomSupplyCurrent.getValueAsDouble(); inputs.bottomTempCelcius = bottomTemp.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/rollers/GenericRollersIOTalonFX.java b/src/main/java/frc/robot/subsystems/rollers/GenericRollersIOTalonFX.java index e689460..c3386b1 100644 --- a/src/main/java/frc/robot/subsystems/rollers/GenericRollersIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/rollers/GenericRollersIOTalonFX.java @@ -9,14 +9,18 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; public abstract class GenericRollersIOTalonFX implements GenericRollersIO { private final TalonFX talon; - private final StatusSignal position; - private final StatusSignal velocity; - private final StatusSignal appliedVolts; - private final StatusSignal supplyCurrent; + private final StatusSignal position; + private final StatusSignal velocity; + private final StatusSignal appliedVolts; + private final StatusSignal supplyCurrent; private final VoltageOut voltageOutput = new VoltageOut(0).withUpdateFreqHz(0); private final NeutralOut neutralOutput = new NeutralOut(); diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index 03bf797..5b70ba3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -74,6 +74,9 @@ public void periodic() { /* use kinematics to get desired module states */ ChassisSpeeds discretizedSpeeds = ChassisSpeeds.discretize(targetSpeeds, Constants.PERIODIC_LOOP_SEC); + /* ChassisSpeeds discretizedSpeeds = targetSpeeds; // FIXME + discretizedSpeeds.discretize(Constants.PERIODIC_LOOP_SEC); */ + SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(discretizedSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( moduleTargetStates, DRIVE_CONFIG.maxLinearVelocity()); @@ -81,8 +84,8 @@ public void periodic() { SwerveModuleState[] optimizedTargetStates = new SwerveModuleState[4]; for (int i = 0; i < modules.length; i++) { - optimizedTargetStates[i] = - SwerveModuleState.optimize(moduleTargetStates[i], modules[i].getSteerHeading()); + optimizedTargetStates[i] = moduleTargetStates[i]; + optimizedTargetStates[i].optimize(modules[i].getSteerHeading()); modules[i].runToSetpoint(optimizedTargetStates[i]); } diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java index 84e7442..3300221 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java @@ -6,11 +6,13 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon; - private final StatusSignal yaw; - private final StatusSignal yawVelocity; + private final StatusSignal yaw; + private final StatusSignal yawVelocity; public GyroIOPigeon2() { pigeon = new Pigeon2(DriveConstants.GYRO_ID); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index a8f945c..503e86d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -14,6 +14,10 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; import frc.robot.subsystems.swerve.DriveConstants.Gains; import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig; import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; @@ -24,18 +28,18 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFX steerTalon; private final CANcoder encoder; - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveSupplyCurrent; - private final StatusSignal driveStatorCurrent; + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveStatorCurrent; private final Supplier steerAbsolutePosition; - private final StatusSignal steerPosition; - private final StatusSignal steerVelocity; - private final StatusSignal steerAppliedVolts; - private final StatusSignal steerSupplyCurrent; - private final StatusSignal steerStatorCurrent; + private final StatusSignal steerPosition; + private final StatusSignal steerVelocity; + private final StatusSignal steerAppliedVolts; + private final StatusSignal steerSupplyCurrent; + private final StatusSignal steerStatorCurrent; private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java index 1756c05..e619fb7 100644 --- a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java +++ b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java @@ -35,6 +35,16 @@ public ChassisSpeeds update(Rotation2d yaw) { linearVelocity.getY() * DRIVE_CONFIG.maxLinearVelocity(), omega * DRIVE_CONFIG.maxAngularVelocity(), yaw); + + /* ChassisSpeeds speeds = + new ChassisSpeeds( + linearVelocity.getX() * DRIVE_CONFIG.maxLinearVelocity(), + linearVelocity.getY() * DRIVE_CONFIG.maxLinearVelocity(), + omega * DRIVE_CONFIG.maxAngularVelocity()); + // eventually run off of pose estimation? + speeds.toRobotRelativeSpeeds(yaw); + + return speeds; */ } public Translation2d calculateLinearVelocity(double x, double y) {