From 0d9b0f50ba1afaf65d5b584e8c9c2b245af672a4 Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 9 Nov 2024 05:00:08 +0000 Subject: [PATCH 01/21] update build.yml --- .github/workflows/build.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ba0370f..f2e4d07 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,8 +1,6 @@ name: Build -on: - push: - pull_request: +on: [push, pull_request] jobs: build: @@ -11,7 +9,7 @@ jobs: container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - name: Checkout repository - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Grant execute permission run: chmod +x gradlew - name: Build robot code From 87a9405ea582b2769a020cd61133fa2461de443a Mon Sep 17 00:00:00 2001 From: Jay Date: Sat, 9 Nov 2024 05:05:07 +0000 Subject: [PATCH 02/21] update gitignore --- .gitignore | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index e6b4bef..8b8cb31 100644 --- a/.gitignore +++ b/.gitignore @@ -158,6 +158,8 @@ gradle-app.setting .settings/ bin/ +.factorypath + # IntelliJ *.iml *.ipr @@ -173,6 +175,3 @@ simgui*.json # Version file src/main/java/frc/robot/BuildConstants.java - - -.factorypath From 9021088cd79307b07334181f4912b1fb6dd1874f Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 19 Nov 2024 01:20:55 +0000 Subject: [PATCH 03/21] discretize chassis speeds --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/subsystems/swerve/Drive.java | 8 +++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6fb7de7..2e5b4a8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,6 +15,8 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public static double PERIODIC_LOOP_SEC = 0.02; + public static RobotType ROBOT_TYPE = RobotType.COMP; public static Mode getRobotMode() { diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index f9b3af6..1c06dd4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -10,6 +10,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -65,7 +67,11 @@ public void periodic() { } // run modules - SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(targetSpeeds); + + /* use kinematics to get desired module states */ + ChassisSpeeds discretizedSpeeds = + ChassisSpeeds.discretize(targetSpeeds, Constants.PERIODIC_LOOP_SEC); + SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(discretizedSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( moduleTargetStates, DRIVE_CONFIG.maxLinearVelocity()); From 0e88ac86309adbc0b3bd930c456e38675db19085 Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 19 Nov 2024 08:10:54 +0000 Subject: [PATCH 04/21] begin teleopcontroller for swerve --- .../swerve/controllers/TeleopController.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java diff --git a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java new file mode 100644 index 0000000..f5218af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.swerve.controllers; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class TeleopController { + + private double controllerX = 0; + private double controllerY = 0; + private double controllerOmega = 0; + + public void acceptJoystickInput(double controllerX, double controllerY, double controllerOmega) { + this.controllerX = controllerX; + this.controllerY = controllerY; + this.controllerOmega = controllerOmega; + } + + public ChassisSpeeds update() { + + } +} From 530e6cf7188ccee082c58bf18b4781c75408cb0a Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 19 Nov 2024 09:20:01 +0000 Subject: [PATCH 05/21] teleop drive controller, vector based deadbanding --- .factorypath | 4 -- .../frc/robot/subsystems/swerve/Drive.java | 43 +++++-------------- .../swerve/controllers/TeleopController.java | 39 +++++++++++++++-- 3 files changed, 47 insertions(+), 39 deletions(-) delete mode 100644 .factorypath diff --git a/.factorypath b/.factorypath deleted file mode 100644 index 9a645c6..0000000 --- a/.factorypath +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index 1c06dd4..8e3c1d6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -3,7 +3,6 @@ import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; import static frc.robot.subsystems.swerve.DriveConstants.KINEMATICS; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -11,7 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; - +import frc.robot.subsystems.swerve.controllers.TeleopController; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -33,9 +32,10 @@ public enum DriveModes { @AutoLogOutput(key = "Swerve/YawOffset") private Rotation2d gyroYawOffset = new Rotation2d(0); - private ChassisSpeeds teleopTargetSpeeds = new ChassisSpeeds(); private ChassisSpeeds targetSpeeds = new ChassisSpeeds(); + private final TeleopController teleopController; + public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { this.gyroIO = gyroIO; @@ -43,6 +43,8 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) modules[1] = new Module(fr, 1); modules[2] = new Module(bl, 2); modules[3] = new Module(br, 3); + + teleopController = new TeleopController(); } @Override @@ -61,7 +63,7 @@ public void periodic() { switch (driveMode) { case TELEOP -> { - targetSpeeds = teleopTargetSpeeds; + targetSpeeds = teleopController.update(arbitraryYaw); } case TRAJECTORY -> {} } @@ -89,35 +91,12 @@ public void periodic() { } public void driveTeleopController(double xAxis, double yAxis, double omega) { - if (driveMode != DriveModes.TELEOP) { // auto align override? - driveMode = DriveModes.TELEOP; + if (DriverStation.isTeleopEnabled()) { + if (driveMode != DriveModes.TELEOP) { + driveMode = DriveModes.TELEOP; + } + teleopController.acceptJoystickInput(xAxis, yAxis, omega); } - - // NWU convention - double theta = Math.atan2(xAxis, yAxis); - double radiusDeadband = - MathUtil.applyDeadband(Math.sqrt((xAxis * xAxis) + (yAxis + yAxis)), 0.07); - double radiusExp = Math.copySign(Math.pow(radiusDeadband, 1.5), radiusDeadband); - - double xVelocity = radiusExp * Math.sin(theta) * DRIVE_CONFIG.maxLinearVelocity(); - double yVelocity = radiusExp * Math.cos(theta) * DRIVE_CONFIG.maxLinearVelocity(); - - /*double xVelocity = - MathUtil.applyDeadband(Math.copySign(xAxis * xAxis, xAxis), 0.07) - * Swerve.DRIVE_CONFIG.maxLinearVelocity(); - double yVelocity = - MathUtil.applyDeadband(Math.copySign(yAxis * yAxis, yAxis), 0.07) - * Swerve.DRIVE_CONFIG.maxLinearVelocity();*/ - double radianVelocity = - MathUtil.applyDeadband(Math.copySign(omega * omega, omega), 0.07) - * DRIVE_CONFIG.maxAngularVelocity(); - - this.teleopTargetSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(xVelocity, yVelocity, radianVelocity, arbitraryYaw); - - Logger.recordOutput("Swerve/Teleop/xVelocity", xVelocity); - Logger.recordOutput("Swerve/Teleop/yVelocity", yVelocity); - Logger.recordOutput("Swerve/Teleop/radianVelocity", radianVelocity); } public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) { 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 f5218af..2c4e3a9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java +++ b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java @@ -1,20 +1,53 @@ package frc.robot.subsystems.swerve.controllers; +import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; public class TeleopController { - + private double controllerX = 0; private double controllerY = 0; private double controllerOmega = 0; + /* accept driver input from joysticks */ public void acceptJoystickInput(double controllerX, double controllerY, double controllerOmega) { - this.controllerX = controllerX; + this.controllerX = controllerX; this.controllerY = controllerY; this.controllerOmega = controllerOmega; } - public ChassisSpeeds update() { + /* update controller with current desired state */ + public ChassisSpeeds update(Rotation2d yaw) { + Translation2d linearVelocity = calculateLinearVelocity(controllerX, controllerY); + + double omega = MathUtil.applyDeadband(controllerOmega, 0.1); + omega = Math.copySign(Math.pow(omega, 1.5), omega); + + // eventaully run off of pose estimation? + return ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * DRIVE_CONFIG.maxLinearVelocity(), + linearVelocity.getY() * DRIVE_CONFIG.maxLinearVelocity(), + omega * DRIVE_CONFIG.maxAngularVelocity(), + yaw); + } + + public Translation2d calculateLinearVelocity(double x, double y) { + // apply deadband, raise magnitude to exponent + double magnitude = MathUtil.applyDeadband(Math.hypot(x, y), 0.1); + magnitude = Math.pow(magnitude, 1.5); + + Rotation2d theta = new Rotation2d(x, y); + Translation2d linearVelocity = + new Pose2d(new Translation2d(), theta) + .transformBy(new Transform2d(magnitude, 0, new Rotation2d())) + .getTranslation(); + return linearVelocity; } } From 3a011fc1f13897e7341a91d1ac2430ea3713aae4 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 20 Nov 2024 01:59:48 +0000 Subject: [PATCH 06/21] prepare for tuning & testing --- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++-------- .../robot/subsystems/swerve/DriveConstants.java | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e160785..2c98522 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,14 +95,14 @@ public RobotContainer() { private void configureBindings() { // -----Driver Controls----- - /*swerve.setDefaultCommand( - swerve - .run( - () -> { - swerve.driveTeleopController( - -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); - }) - .withName("Drive Teleop"));*/ + swerve.setDefaultCommand( + swerve + .run( + () -> { + swerve.driveTeleopController( + -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); + }) + .withName("Drive Teleop")); // -----Intake Controls----- driverA.x().whileTrue(rollers.setTargetCommand(RollerState.INTAKE)); diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index 6d527a2..5a4e28a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -48,14 +48,14 @@ public class DriveConstants { case COMP -> new ModuleConfig[] { new ModuleConfig(5, 6, 1, new Rotation2d(0), true, false), new ModuleConfig(7, 8, 2, new Rotation2d(0), true, true), - new ModuleConfig(9, 10, 3, new Rotation2d(0), true, false), - new ModuleConfig(11, 12, 4, new Rotation2d(0), true, true) + new ModuleConfig(11, 12, 3, new Rotation2d(0), true, false), + new ModuleConfig(9, 10, 4, new Rotation2d(0), true, true) }; case DEV -> new ModuleConfig[] { - new ModuleConfig(2, 1, 27, new Rotation2d(1.954), true, false), - new ModuleConfig(13, 12, 26, new Rotation2d(1.465), true, true), - new ModuleConfig(4, 3, 24, new Rotation2d(2.612), true, false), - new ModuleConfig(11, 10, 25, new Rotation2d(-2.563), true, true) + new ModuleConfig(2, 1, 27, new Rotation2d(0), true, false), + new ModuleConfig(13, 12, 26, new Rotation2d(0), true, true), + new ModuleConfig(4, 3, 24, new Rotation2d(0), true, false), + new ModuleConfig(11, 10, 25, new Rotation2d(0), true, true) }; case SIM -> new ModuleConfig[] { new ModuleConfig(0, 0, 0, new Rotation2d(0), true, false), From c891e2e93e2d4ffa86e142e2ed3aac93c361c7e9 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 20 Nov 2024 03:14:31 +0000 Subject: [PATCH 07/21] log module supply currents --- .../frc/robot/subsystems/swerve/ModuleIO.java | 2 ++ .../subsystems/swerve/ModuleIOTalonFX.java | 20 +++++++++++++++---- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 79dbba7..33ffdc8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -13,11 +13,13 @@ class ModuleIOInputs { public double drivePositionRads = 0; public double driveVelocityRadsPerSec = 0; public double driveAppliedVolts = 0; + public double driveSupplyCurrent = 0; public Rotation2d steerAbsolutePostion = new Rotation2d(); public Rotation2d steerPosition = new Rotation2d(); public double steerVelocityRadsPerSec = 0; public double steerAppliedVolts = 0; + public double steerSupplyCurrent = 0; } default void updateInputs(ModuleIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index f2fe723..2961aa3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -25,11 +25,13 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal drivePosition; private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; private final Supplier steerAbsolutePosition; private final StatusSignal steerPosition; private final StatusSignal steerVelocity; private final StatusSignal steerAppliedVolts; + private final StatusSignal steerSupplyCurrent; private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); @@ -85,22 +87,26 @@ public ModuleIOTalonFX(ModuleConfig config) { drivePosition = driveTalon.getPosition(); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); + driveSupplyCurrent = driveTalon.getSupplyCurrent(); steerAbsolutePosition = () -> Rotation2d.fromRotations(encoder.getAbsolutePosition().getValueAsDouble()); - // .minus(config.absoluteEncoderOffset()); steerPosition = steerTalon.getPosition(); steerVelocity = steerTalon.getVelocity(); steerAppliedVolts = steerTalon.getMotorVoltage(); + steerSupplyCurrent = steerTalon.getSupplyCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( 100, drivePosition, driveVelocity, driveAppliedVolts, + driveSupplyCurrent, encoder.getAbsolutePosition(), steerPosition, steerVelocity, - steerAppliedVolts); + steerAppliedVolts, + steerSupplyCurrent); driveTalon.optimizeBusUtilization(); steerTalon.optimizeBusUtilization(); @@ -112,17 +118,23 @@ public ModuleIOTalonFX(ModuleConfig config) { @Override public void updateInputs(ModuleIOInputs inputs) { inputs.driveMotorConnected = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts).isOK(); + BaseStatusSignal.refreshAll( + drivePosition, driveVelocity, driveAppliedVolts, driveSupplyCurrent) + .isOK(); inputs.drivePositionRads = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadsPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrent = driveSupplyCurrent.getValueAsDouble(); inputs.steerMotorConnected = - BaseStatusSignal.refreshAll(steerPosition, steerVelocity, steerAppliedVolts).isOK(); + BaseStatusSignal.refreshAll( + steerPosition, steerVelocity, steerAppliedVolts, steerSupplyCurrent) + .isOK(); inputs.steerAbsolutePostion = steerAbsolutePosition.get(); inputs.steerPosition = Rotation2d.fromRotations(steerPosition.getValueAsDouble()); inputs.steerVelocityRadsPerSec = Units.rotationsToRadians(steerVelocity.getValueAsDouble()); inputs.steerAppliedVolts = steerAppliedVolts.getValueAsDouble(); + inputs.steerSupplyCurrent = steerSupplyCurrent.getValueAsDouble(); } @Override From 9be7b547afd26eb4448bdb22a4a3f393be4f8e8b Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 20 Nov 2024 03:34:27 +0000 Subject: [PATCH 08/21] calibration, placeholder pid --- .../robot/subsystems/swerve/DriveConstants.java | 14 +++++++------- .../robot/subsystems/swerve/ModuleIOTalonFX.java | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index 5a4e28a..d03932b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -42,14 +42,14 @@ public class DriveConstants { public static final int GYRO_ID = 0; - // fl, fr, bl, br + // fl, fr, bl, br; negate offsets public static final ModuleConfig[] MODULE_CONFIGS = switch (getRobotType()) { case COMP -> new ModuleConfig[] { - new ModuleConfig(5, 6, 1, new Rotation2d(0), true, false), - new ModuleConfig(7, 8, 2, new Rotation2d(0), true, true), - new ModuleConfig(11, 12, 3, new Rotation2d(0), true, false), - new ModuleConfig(9, 10, 4, new Rotation2d(0), true, true) + new ModuleConfig(5, 6, 1, new Rotation2d(1.1397), true, false), + new ModuleConfig(7, 8, 2, new Rotation2d(0.8038), true, true), + new ModuleConfig(11, 12, 3, new Rotation2d(1.4327), true, false), + new ModuleConfig(9, 10, 4, new Rotation2d(-1.8208), true, true) }; case DEV -> new ModuleConfig[] { new ModuleConfig(2, 1, 27, new Rotation2d(0), true, false), @@ -71,12 +71,12 @@ public class DriveConstants { 0, // steerkS 0, // steerkV 0, // steerkA - 0, // steerkP + 300, // steerkP 0, // steerkD 0, // drivekS 0, // drivekV 0, // drivekA - 0, // drivekP + 1, // drivekP 0, // drivekD 5.357142857142857, 21.428571428571427, diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index 2961aa3..b887d9d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -46,7 +46,7 @@ public ModuleIOTalonFX(ModuleConfig config) { encoder = new CANcoder(config.encoderID()); // config - encoderConfig.MagnetSensor.MagnetOffset = config.absoluteEncoderOffset().getRotations(); + encoderConfig.MagnetSensor.MagnetOffset = -config.absoluteEncoderOffset().getRotations(); driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.MotorOutput.Inverted = From 365fae01a94389ea58dd7832ca9642a7f7eb0736 Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 22 Nov 2024 00:23:34 +0000 Subject: [PATCH 09/21] log swerve motor stator currents --- .../frc/robot/subsystems/swerve/ModuleIO.java | 2 ++ .../subsystems/swerve/ModuleIOTalonFX.java | 22 ++++++++++++++++--- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 33ffdc8..a97a212 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -14,12 +14,14 @@ class ModuleIOInputs { public double driveVelocityRadsPerSec = 0; public double driveAppliedVolts = 0; public double driveSupplyCurrent = 0; + public double driveStatorCurrent = 0; public Rotation2d steerAbsolutePostion = new Rotation2d(); public Rotation2d steerPosition = new Rotation2d(); public double steerVelocityRadsPerSec = 0; public double steerAppliedVolts = 0; public double steerSupplyCurrent = 0; + public double steerStatorCurrent = 0; } default void updateInputs(ModuleIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index b887d9d..eb39d5c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -26,12 +26,14 @@ public class ModuleIOTalonFX implements ModuleIO { 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 TalonFXConfiguration driveConfig = new TalonFXConfiguration(); private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); @@ -88,6 +90,7 @@ public ModuleIOTalonFX(ModuleConfig config) { driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveStatorCurrent = driveTalon.getStatorCurrent(); steerAbsolutePosition = () -> Rotation2d.fromRotations(encoder.getAbsolutePosition().getValueAsDouble()); @@ -95,6 +98,7 @@ public ModuleIOTalonFX(ModuleConfig config) { steerVelocity = steerTalon.getVelocity(); steerAppliedVolts = steerTalon.getMotorVoltage(); steerSupplyCurrent = steerTalon.getSupplyCurrent(); + steerStatorCurrent = steerTalon.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( 100, @@ -102,11 +106,13 @@ public ModuleIOTalonFX(ModuleConfig config) { driveVelocity, driveAppliedVolts, driveSupplyCurrent, + driveStatorCurrent, encoder.getAbsolutePosition(), steerPosition, steerVelocity, steerAppliedVolts, - steerSupplyCurrent); + steerSupplyCurrent, + steerStatorCurrent); driveTalon.optimizeBusUtilization(); steerTalon.optimizeBusUtilization(); @@ -119,22 +125,32 @@ public ModuleIOTalonFX(ModuleConfig config) { public void updateInputs(ModuleIOInputs inputs) { inputs.driveMotorConnected = BaseStatusSignal.refreshAll( - drivePosition, driveVelocity, driveAppliedVolts, driveSupplyCurrent) + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveStatorCurrent) .isOK(); inputs.drivePositionRads = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadsPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveSupplyCurrent = driveSupplyCurrent.getValueAsDouble(); + inputs.driveStatorCurrent = driveStatorCurrent.getValueAsDouble(); inputs.steerMotorConnected = BaseStatusSignal.refreshAll( - steerPosition, steerVelocity, steerAppliedVolts, steerSupplyCurrent) + steerPosition, + steerVelocity, + steerAppliedVolts, + steerSupplyCurrent, + steerStatorCurrent) .isOK(); inputs.steerAbsolutePostion = steerAbsolutePosition.get(); inputs.steerPosition = Rotation2d.fromRotations(steerPosition.getValueAsDouble()); inputs.steerVelocityRadsPerSec = Units.rotationsToRadians(steerVelocity.getValueAsDouble()); inputs.steerAppliedVolts = steerAppliedVolts.getValueAsDouble(); inputs.steerSupplyCurrent = steerSupplyCurrent.getValueAsDouble(); + inputs.steerStatorCurrent = steerStatorCurrent.getValueAsDouble(); } @Override From f70d72553497ada79d3941cfdf517946dd7690ed Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 22 Nov 2024 10:01:33 +0000 Subject: [PATCH 10/21] placeholder stator current limits --- .../java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index eb39d5c..aaaa0cd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -50,6 +50,12 @@ public ModuleIOTalonFX(ModuleConfig config) { // config encoderConfig.MagnetSensor.MagnetOffset = -config.absoluteEncoderOffset().getRotations(); + driveConfig.CurrentLimits.StatorCurrentLimit = 80; // FIXME + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + steerConfig.CurrentLimits.StatorCurrentLimit = 50; // FIXME + steerConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.MotorOutput.Inverted = config.driveInverted() From 2560a44feddf7f452b956854e8ba60cb34cb806c Mon Sep 17 00:00:00 2001 From: Jay Date: Fri, 22 Nov 2024 10:17:02 +0000 Subject: [PATCH 11/21] modify module gain constants, change steer controls to motion magic --- .../subsystems/swerve/DriveConstants.java | 43 +++++--------- .../frc/robot/subsystems/swerve/ModuleIO.java | 6 +- .../subsystems/swerve/ModuleIOTalonFX.java | 56 +++++++++---------- 3 files changed, 42 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index d03932b..a583f5e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,30 +68,16 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - 0, // steerkS - 0, // steerkV - 0, // steerkA - 300, // steerkP - 0, // steerkD - 0, // drivekS - 0, // drivekV - 0, // drivekA - 1, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 300, 0, 0), + new MotionProfileGains(0, 0, 0), // FIXME + new Gains(0, 0, 0, 1, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); case DEV -> new ModuleConstants( - 0, // steerkS - 0, // steerkV - 0, // steerkA - 11, // steerkP - 0, // steerkD - 0, // drivekS - 0, // drivekV - 0, // drivekA - 1.5, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 11, 0, 0), + new MotionProfileGains(0, 0, 0), + new Gains(0, 0, 0, 1.5, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); @@ -114,22 +100,19 @@ public record ModuleConfig( boolean driveInverted) {} public record ModuleConstants( - double steerkS, - double steerkV, - double steerkA, - double steerkP, - double steerkD, - double drivekS, - double drivekV, - double drivekA, - double drivekP, - double drivekD, + Gains steerGains, + MotionProfileGains steerMotionGains, + Gains driveGains, double driveReduction, double steerReduction, double couplingGearReduction) {} public record TrajectoryFollowerConstants() {} + public record Gains(double kS, double kV, double kA, double kP, double kI, double kD) {} + + public record MotionProfileGains(double cruiseVelocity, double acceleration, double jerk) {} + private enum Mk4iReductions { MK4I_L3((50 / 14) * (16 / 28) * (45 / 15)), STEER(150 / 7); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index a97a212..70f15e9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.swerve.DriveConstants.Gains; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -34,9 +36,9 @@ default void runDriveVelocitySetpoint(double velocityRadsPerSec) {} default void runSteerPositionSetpoint(double angleRads) {} - default void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setDriveGains(Gains gains) {} - default void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index aaaa0cd..a8f945c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,7 +14,9 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.swerve.DriveConstants.Gains; import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import java.util.function.Supplier; public class ModuleIOTalonFX implements ModuleIO { @@ -39,8 +41,9 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); private final CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - private VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); - private PositionVoltage steerPositionControl = new PositionVoltage(0).withUpdateFreqHz(0); + private final VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); + private final MotionMagicVoltage steerPositionControl = + new MotionMagicVoltage(0).withUpdateFreqHz(0); public ModuleIOTalonFX(ModuleConfig config) { driveTalon = new TalonFX(config.driveID()); @@ -72,20 +75,8 @@ public ModuleIOTalonFX(ModuleConfig config) { steerConfig.Feedback.SensorToMechanismRatio = MODULE_CONSTANTS.steerReduction(); steerConfig.ClosedLoopGeneral.ContinuousWrap = true; - setDriveSlot0( - MODULE_CONSTANTS.drivekP(), - 0, - MODULE_CONSTANTS.drivekD(), - MODULE_CONSTANTS.drivekS(), - MODULE_CONSTANTS.drivekV(), - MODULE_CONSTANTS.drivekA()); - setSteerSlot0( - MODULE_CONSTANTS.steerkP(), - 0, - MODULE_CONSTANTS.steerkD(), - MODULE_CONSTANTS.steerkS(), - MODULE_CONSTANTS.steerkV(), - MODULE_CONSTANTS.steerkA()); + setDriveGains(MODULE_CONSTANTS.driveGains()); + setSteerGains(MODULE_CONSTANTS.steerGains(), MODULE_CONSTANTS.steerMotionGains()); driveTalon.getConfigurator().apply(driveConfig); steerTalon.getConfigurator().apply(steerConfig); @@ -171,24 +162,27 @@ public void runSteerPositionSetpoint(double angleRads) { } @Override - public void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - driveConfig.Slot0.kP = kP; - driveConfig.Slot0.kI = kI; - driveConfig.Slot0.kD = kD; - driveConfig.Slot0.kS = kS; - driveConfig.Slot0.kV = kV; - driveConfig.Slot0.kA = kA; + public void setDriveGains(Gains gains) { + driveConfig.Slot0.kP = gains.kP(); + driveConfig.Slot0.kI = gains.kI(); + driveConfig.Slot0.kD = gains.kD(); + driveConfig.Slot0.kS = gains.kS(); + driveConfig.Slot0.kV = gains.kV(); + driveConfig.Slot0.kA = gains.kA(); driveTalon.getConfigurator().apply(driveConfig); } @Override - public void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - steerConfig.Slot0.kP = kP; - steerConfig.Slot0.kI = kI; - steerConfig.Slot0.kD = kD; - steerConfig.Slot0.kS = kS; - steerConfig.Slot0.kV = kV; - steerConfig.Slot0.kA = kA; + public void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) { + steerConfig.Slot0.kP = gains.kP(); + steerConfig.Slot0.kI = gains.kI(); + steerConfig.Slot0.kD = gains.kD(); + steerConfig.Slot0.kS = gains.kS(); + steerConfig.Slot0.kV = gains.kV(); + steerConfig.Slot0.kA = gains.kA(); + steerConfig.MotionMagic.MotionMagicCruiseVelocity = motionProfileGains.cruiseVelocity(); + steerConfig.MotionMagic.MotionMagicAcceleration = motionProfileGains.acceleration(); + steerConfig.MotionMagic.MotionMagicJerk = motionProfileGains.jerk(); steerTalon.getConfigurator().apply(steerConfig); } } From 4ecb826ba3f50c6bac0c4c147c665b319bd98f74 Mon Sep 17 00:00:00 2001 From: Nora Date: Sat, 23 Nov 2024 12:27:49 -0800 Subject: [PATCH 12/21] Revert "modify module gain constants, change steer controls to motion magic" This reverts commit 2560a44feddf7f452b956854e8ba60cb34cb806c. --- .../subsystems/swerve/DriveConstants.java | 43 +++++++++----- .../frc/robot/subsystems/swerve/ModuleIO.java | 6 +- .../subsystems/swerve/ModuleIOTalonFX.java | 56 ++++++++++--------- 3 files changed, 63 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index a583f5e..d03932b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,16 +68,30 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - new Gains(0, 0, 0, 300, 0, 0), - new MotionProfileGains(0, 0, 0), // FIXME - new Gains(0, 0, 0, 1, 0, 0), + 0, // steerkS + 0, // steerkV + 0, // steerkA + 300, // steerkP + 0, // steerkD + 0, // drivekS + 0, // drivekV + 0, // drivekA + 1, // drivekP + 0, // drivekD 5.357142857142857, 21.428571428571427, 3.125); case DEV -> new ModuleConstants( - new Gains(0, 0, 0, 11, 0, 0), - new MotionProfileGains(0, 0, 0), - new Gains(0, 0, 0, 1.5, 0, 0), + 0, // steerkS + 0, // steerkV + 0, // steerkA + 11, // steerkP + 0, // steerkD + 0, // drivekS + 0, // drivekV + 0, // drivekA + 1.5, // drivekP + 0, // drivekD 5.357142857142857, 21.428571428571427, 3.125); @@ -100,19 +114,22 @@ public record ModuleConfig( boolean driveInverted) {} public record ModuleConstants( - Gains steerGains, - MotionProfileGains steerMotionGains, - Gains driveGains, + double steerkS, + double steerkV, + double steerkA, + double steerkP, + double steerkD, + double drivekS, + double drivekV, + double drivekA, + double drivekP, + double drivekD, double driveReduction, double steerReduction, double couplingGearReduction) {} public record TrajectoryFollowerConstants() {} - public record Gains(double kS, double kV, double kA, double kP, double kI, double kD) {} - - public record MotionProfileGains(double cruiseVelocity, double acceleration, double jerk) {} - private enum Mk4iReductions { MK4I_L3((50 / 14) * (16 / 28) * (45 / 15)), STEER(150 / 7); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index 70f15e9..a97a212 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -1,8 +1,6 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.subsystems.swerve.DriveConstants.Gains; -import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -36,9 +34,9 @@ default void runDriveVelocitySetpoint(double velocityRadsPerSec) {} default void runSteerPositionSetpoint(double angleRads) {} - default void setDriveGains(Gains gains) {} + default void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} - default void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) {} + default void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index a8f945c..aaaa0cd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,9 +14,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.swerve.DriveConstants.Gains; import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig; -import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import java.util.function.Supplier; public class ModuleIOTalonFX implements ModuleIO { @@ -41,9 +39,8 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); private final CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - private final VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); - private final MotionMagicVoltage steerPositionControl = - new MotionMagicVoltage(0).withUpdateFreqHz(0); + private VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); + private PositionVoltage steerPositionControl = new PositionVoltage(0).withUpdateFreqHz(0); public ModuleIOTalonFX(ModuleConfig config) { driveTalon = new TalonFX(config.driveID()); @@ -75,8 +72,20 @@ public ModuleIOTalonFX(ModuleConfig config) { steerConfig.Feedback.SensorToMechanismRatio = MODULE_CONSTANTS.steerReduction(); steerConfig.ClosedLoopGeneral.ContinuousWrap = true; - setDriveGains(MODULE_CONSTANTS.driveGains()); - setSteerGains(MODULE_CONSTANTS.steerGains(), MODULE_CONSTANTS.steerMotionGains()); + setDriveSlot0( + MODULE_CONSTANTS.drivekP(), + 0, + MODULE_CONSTANTS.drivekD(), + MODULE_CONSTANTS.drivekS(), + MODULE_CONSTANTS.drivekV(), + MODULE_CONSTANTS.drivekA()); + setSteerSlot0( + MODULE_CONSTANTS.steerkP(), + 0, + MODULE_CONSTANTS.steerkD(), + MODULE_CONSTANTS.steerkS(), + MODULE_CONSTANTS.steerkV(), + MODULE_CONSTANTS.steerkA()); driveTalon.getConfigurator().apply(driveConfig); steerTalon.getConfigurator().apply(steerConfig); @@ -162,27 +171,24 @@ public void runSteerPositionSetpoint(double angleRads) { } @Override - public void setDriveGains(Gains gains) { - driveConfig.Slot0.kP = gains.kP(); - driveConfig.Slot0.kI = gains.kI(); - driveConfig.Slot0.kD = gains.kD(); - driveConfig.Slot0.kS = gains.kS(); - driveConfig.Slot0.kV = gains.kV(); - driveConfig.Slot0.kA = gains.kA(); + public void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { + driveConfig.Slot0.kP = kP; + driveConfig.Slot0.kI = kI; + driveConfig.Slot0.kD = kD; + driveConfig.Slot0.kS = kS; + driveConfig.Slot0.kV = kV; + driveConfig.Slot0.kA = kA; driveTalon.getConfigurator().apply(driveConfig); } @Override - public void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) { - steerConfig.Slot0.kP = gains.kP(); - steerConfig.Slot0.kI = gains.kI(); - steerConfig.Slot0.kD = gains.kD(); - steerConfig.Slot0.kS = gains.kS(); - steerConfig.Slot0.kV = gains.kV(); - steerConfig.Slot0.kA = gains.kA(); - steerConfig.MotionMagic.MotionMagicCruiseVelocity = motionProfileGains.cruiseVelocity(); - steerConfig.MotionMagic.MotionMagicAcceleration = motionProfileGains.acceleration(); - steerConfig.MotionMagic.MotionMagicJerk = motionProfileGains.jerk(); + public void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { + steerConfig.Slot0.kP = kP; + steerConfig.Slot0.kI = kI; + steerConfig.Slot0.kD = kD; + steerConfig.Slot0.kS = kS; + steerConfig.Slot0.kV = kV; + steerConfig.Slot0.kA = kA; steerTalon.getConfigurator().apply(steerConfig); } } From 7b321c3fa9e57fa3cbd5df0d4cdee37585ef041b Mon Sep 17 00:00:00 2001 From: Nora Date: Sat, 23 Nov 2024 16:48:00 -0800 Subject: [PATCH 13/21] Better tuned pid values --- src/main/java/frc/robot/RobotContainer.java | 3 +-- .../frc/robot/subsystems/swerve/DriveConstants.java | 13 ++++++++----- .../java/frc/robot/subsystems/swerve/Module.java | 5 +++++ .../robot/subsystems/swerve/ModuleIOTalonFX.java | 2 +- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c98522..7cfbc82 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,8 +99,7 @@ private void configureBindings() { swerve .run( () -> { - swerve.driveTeleopController( - -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); + swerve.driveTeleopController(-0, -driverA.getLeftX(), -driverA.getRightX()); }) .withName("Drive Teleop")); diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index d03932b..7780fa3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,15 +68,16 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - 0, // steerkS + 0.2, // steerkS 0, // steerkV 0, // steerkA - 300, // steerkP + 98, // steerkP90 + 3, // steer kI 0, // steerkD - 0, // drivekS - 0, // drivekV + 0.3, // drivekS0.3 + 0.63, // drivekV0.67 0, // drivekA - 1, // drivekP + 2, // drivekP 0, // drivekD 5.357142857142857, 21.428571428571427, @@ -86,6 +87,7 @@ public class DriveConstants { 0, // steerkV 0, // steerkA 11, // steerkP + 0, // steer kI 0, // steerkD 0, // drivekS 0, // drivekV @@ -118,6 +120,7 @@ public record ModuleConstants( double steerkV, double steerkA, double steerkP, + double steerkI, double steerkD, double drivekS, double drivekV, diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index fd409fa..44859c6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -44,6 +44,11 @@ public void runToSetpoint(SwerveModuleState targetState) { moduleIO.runDriveVelocitySetpoint(driveVelocityRads); + Logger.recordOutput("Swerve/Module" + index + "/SteerSetpoint", targetState.angle.getRadians()); + Logger.recordOutput( + "Swerve/Module" + index + "/SteerError", + targetState.angle.getRadians() - inputs.steerAbsolutePostion.getRadians()); + Logger.recordOutput("Swerve/Module" + index + "/DriveVelocitySetpoint", driveVelocityRads); Logger.recordOutput("Swerve/Module" + index + "/DriveVelRadsScalar", driveVelocityRads); } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index aaaa0cd..a51c571 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -81,7 +81,7 @@ public ModuleIOTalonFX(ModuleConfig config) { MODULE_CONSTANTS.drivekA()); setSteerSlot0( MODULE_CONSTANTS.steerkP(), - 0, + MODULE_CONSTANTS.steerkI(), MODULE_CONSTANTS.steerkD(), MODULE_CONSTANTS.steerkS(), MODULE_CONSTANTS.steerkV(), From a854c2ab7aa08fc6607afa646db872f523b49fd8 Mon Sep 17 00:00:00 2001 From: Nora Date: Sat, 23 Nov 2024 16:49:54 -0800 Subject: [PATCH 14/21] Forgot to undo testing controls --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cfbc82..2c98522 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,7 +99,8 @@ private void configureBindings() { swerve .run( () -> { - swerve.driveTeleopController(-0, -driverA.getLeftX(), -driverA.getRightX()); + swerve.driveTeleopController( + -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); }) .withName("Drive Teleop")); From 69aed5109789329758a6e6d96b2410724b9a9d4f Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 26 Nov 2024 00:37:35 +0000 Subject: [PATCH 15/21] reintroduce motion magic steer motors, refactor swerve gains --- .../subsystems/swerve/DriveConstants.java | 46 +++++---------- .../frc/robot/subsystems/swerve/ModuleIO.java | 6 +- .../subsystems/swerve/ModuleIOTalonFX.java | 56 +++++++++---------- 3 files changed, 42 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index 7780fa3..a583f5e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,32 +68,16 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - 0.2, // steerkS - 0, // steerkV - 0, // steerkA - 98, // steerkP90 - 3, // steer kI - 0, // steerkD - 0.3, // drivekS0.3 - 0.63, // drivekV0.67 - 0, // drivekA - 2, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 300, 0, 0), + new MotionProfileGains(0, 0, 0), // FIXME + new Gains(0, 0, 0, 1, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); case DEV -> new ModuleConstants( - 0, // steerkS - 0, // steerkV - 0, // steerkA - 11, // steerkP - 0, // steer kI - 0, // steerkD - 0, // drivekS - 0, // drivekV - 0, // drivekA - 1.5, // drivekP - 0, // drivekD + new Gains(0, 0, 0, 11, 0, 0), + new MotionProfileGains(0, 0, 0), + new Gains(0, 0, 0, 1.5, 0, 0), 5.357142857142857, 21.428571428571427, 3.125); @@ -116,23 +100,19 @@ public record ModuleConfig( boolean driveInverted) {} public record ModuleConstants( - double steerkS, - double steerkV, - double steerkA, - double steerkP, - double steerkI, - double steerkD, - double drivekS, - double drivekV, - double drivekA, - double drivekP, - double drivekD, + Gains steerGains, + MotionProfileGains steerMotionGains, + Gains driveGains, double driveReduction, double steerReduction, double couplingGearReduction) {} public record TrajectoryFollowerConstants() {} + public record Gains(double kS, double kV, double kA, double kP, double kI, double kD) {} + + public record MotionProfileGains(double cruiseVelocity, double acceleration, double jerk) {} + private enum Mk4iReductions { MK4I_L3((50 / 14) * (16 / 28) * (45 / 15)), STEER(150 / 7); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java index a97a212..70f15e9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIO.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.swerve.DriveConstants.Gains; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -34,9 +36,9 @@ default void runDriveVelocitySetpoint(double velocityRadsPerSec) {} default void runSteerPositionSetpoint(double angleRads) {} - default void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setDriveGains(Gains gains) {} - default void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) {} + default void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) {} default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index a51c571..a8f945c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,7 +14,9 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.swerve.DriveConstants.Gains; import frc.robot.subsystems.swerve.DriveConstants.ModuleConfig; +import frc.robot.subsystems.swerve.DriveConstants.MotionProfileGains; import java.util.function.Supplier; public class ModuleIOTalonFX implements ModuleIO { @@ -39,8 +41,9 @@ public class ModuleIOTalonFX implements ModuleIO { private final TalonFXConfiguration steerConfig = new TalonFXConfiguration(); private final CANcoderConfiguration encoderConfig = new CANcoderConfiguration(); - private VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); - private PositionVoltage steerPositionControl = new PositionVoltage(0).withUpdateFreqHz(0); + private final VelocityVoltage driveVelocityControl = new VelocityVoltage(0).withUpdateFreqHz(0); + private final MotionMagicVoltage steerPositionControl = + new MotionMagicVoltage(0).withUpdateFreqHz(0); public ModuleIOTalonFX(ModuleConfig config) { driveTalon = new TalonFX(config.driveID()); @@ -72,20 +75,8 @@ public ModuleIOTalonFX(ModuleConfig config) { steerConfig.Feedback.SensorToMechanismRatio = MODULE_CONSTANTS.steerReduction(); steerConfig.ClosedLoopGeneral.ContinuousWrap = true; - setDriveSlot0( - MODULE_CONSTANTS.drivekP(), - 0, - MODULE_CONSTANTS.drivekD(), - MODULE_CONSTANTS.drivekS(), - MODULE_CONSTANTS.drivekV(), - MODULE_CONSTANTS.drivekA()); - setSteerSlot0( - MODULE_CONSTANTS.steerkP(), - MODULE_CONSTANTS.steerkI(), - MODULE_CONSTANTS.steerkD(), - MODULE_CONSTANTS.steerkS(), - MODULE_CONSTANTS.steerkV(), - MODULE_CONSTANTS.steerkA()); + setDriveGains(MODULE_CONSTANTS.driveGains()); + setSteerGains(MODULE_CONSTANTS.steerGains(), MODULE_CONSTANTS.steerMotionGains()); driveTalon.getConfigurator().apply(driveConfig); steerTalon.getConfigurator().apply(steerConfig); @@ -171,24 +162,27 @@ public void runSteerPositionSetpoint(double angleRads) { } @Override - public void setDriveSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - driveConfig.Slot0.kP = kP; - driveConfig.Slot0.kI = kI; - driveConfig.Slot0.kD = kD; - driveConfig.Slot0.kS = kS; - driveConfig.Slot0.kV = kV; - driveConfig.Slot0.kA = kA; + public void setDriveGains(Gains gains) { + driveConfig.Slot0.kP = gains.kP(); + driveConfig.Slot0.kI = gains.kI(); + driveConfig.Slot0.kD = gains.kD(); + driveConfig.Slot0.kS = gains.kS(); + driveConfig.Slot0.kV = gains.kV(); + driveConfig.Slot0.kA = gains.kA(); driveTalon.getConfigurator().apply(driveConfig); } @Override - public void setSteerSlot0(double kP, double kI, double kD, double kS, double kV, double kA) { - steerConfig.Slot0.kP = kP; - steerConfig.Slot0.kI = kI; - steerConfig.Slot0.kD = kD; - steerConfig.Slot0.kS = kS; - steerConfig.Slot0.kV = kV; - steerConfig.Slot0.kA = kA; + public void setSteerGains(Gains gains, MotionProfileGains motionProfileGains) { + steerConfig.Slot0.kP = gains.kP(); + steerConfig.Slot0.kI = gains.kI(); + steerConfig.Slot0.kD = gains.kD(); + steerConfig.Slot0.kS = gains.kS(); + steerConfig.Slot0.kV = gains.kV(); + steerConfig.Slot0.kA = gains.kA(); + steerConfig.MotionMagic.MotionMagicCruiseVelocity = motionProfileGains.cruiseVelocity(); + steerConfig.MotionMagic.MotionMagicAcceleration = motionProfileGains.acceleration(); + steerConfig.MotionMagic.MotionMagicJerk = motionProfileGains.jerk(); steerTalon.getConfigurator().apply(steerConfig); } } From be471afe4dcaf706db897e658ccf2b800d7de8df Mon Sep 17 00:00:00 2001 From: Jay Date: Tue, 26 Nov 2024 01:28:08 +0000 Subject: [PATCH 16/21] tune motion profile, kP, feedforward for steer motors to <0.01 error; placeholder drive gains --- .../java/frc/robot/subsystems/swerve/DriveConstants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index a583f5e..d5ce0da 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -68,9 +68,9 @@ public class DriveConstants { public static final ModuleConstants MODULE_CONSTANTS = switch (getRobotType()) { case COMP, SIM -> new ModuleConstants( - new Gains(0, 0, 0, 300, 0, 0), - new MotionProfileGains(0, 0, 0), // FIXME - new Gains(0, 0, 0, 1, 0, 0), + new Gains(0.25, 2.62, 0, 100, 0, 0), // revisit kP + new MotionProfileGains(4, 64, 640), // revisit all + new Gains(0.3, 0.63, 0, 2, 0, 0), // FIXME placeholder, to do 5.357142857142857, 21.428571428571427, 3.125); From cb2cb8d01e8e33f91648f962a9ff501d6da08fd2 Mon Sep 17 00:00:00 2001 From: Nora Date: Tue, 26 Nov 2024 17:50:29 -0800 Subject: [PATCH 17/21] Turning with triggers implemented --- src/main/java/frc/robot/RobotContainer.java | 5 ++++- src/main/java/frc/robot/subsystems/swerve/Drive.java | 8 ++++++-- src/main/java/frc/robot/subsystems/swerve/Module.java | 1 - .../subsystems/swerve/controllers/TeleopController.java | 2 +- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c98522..fe71830 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -100,7 +100,10 @@ private void configureBindings() { .run( () -> { swerve.driveTeleopController( - -driverA.getLeftY(), -driverA.getLeftX(), -driverA.getRightX()); + -driverA.getLeftY(), + -driverA.getLeftX(), + driverA.getLeftTriggerAxis(), + -driverA.getRightTriggerAxis()); }) .withName("Drive Teleop")); diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index 8e3c1d6..28beeba 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -3,6 +3,7 @@ import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; import static frc.robot.subsystems.swerve.DriveConstants.KINEMATICS; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -90,12 +91,15 @@ public void periodic() { Logger.recordOutput("Swerve/DriveMode", driveMode); } - public void driveTeleopController(double xAxis, double yAxis, double omega) { + public void driveTeleopController( + double xAxis, double yAxis, double leftTrigger, double rightTrigger) { if (DriverStation.isTeleopEnabled()) { if (driveMode != DriveModes.TELEOP) { driveMode = DriveModes.TELEOP; } - teleopController.acceptJoystickInput(xAxis, yAxis, omega); + + double triggers = MathUtil.applyDeadband(leftTrigger + rightTrigger, 0.07); + teleopController.acceptJoystickInput(xAxis, yAxis, triggers); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 44859c6..ce970c4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -48,7 +48,6 @@ public void runToSetpoint(SwerveModuleState targetState) { Logger.recordOutput( "Swerve/Module" + index + "/SteerError", targetState.angle.getRadians() - inputs.steerAbsolutePostion.getRadians()); - Logger.recordOutput("Swerve/Module" + index + "/DriveVelocitySetpoint", driveVelocityRads); Logger.recordOutput("Swerve/Module" + index + "/DriveVelRadsScalar", driveVelocityRads); } 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 2c4e3a9..1756c05 100644 --- a/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java +++ b/src/main/java/frc/robot/subsystems/swerve/controllers/TeleopController.java @@ -27,7 +27,7 @@ public ChassisSpeeds update(Rotation2d yaw) { Translation2d linearVelocity = calculateLinearVelocity(controllerX, controllerY); double omega = MathUtil.applyDeadband(controllerOmega, 0.1); - omega = Math.copySign(Math.pow(omega, 1.5), omega); + omega = Math.copySign(Math.pow(Math.abs(omega), 1.5), omega); // eventaully run off of pose estimation? return ChassisSpeeds.fromFieldRelativeSpeeds( From 97eed50c492d58d27524de47dc7a8dcd994dbe0d Mon Sep 17 00:00:00 2001 From: Nora Date: Tue, 26 Nov 2024 17:54:35 -0800 Subject: [PATCH 18/21] Zeroing --- src/main/java/frc/robot/RobotContainer.java | 9 +++++++++ src/main/java/frc/robot/subsystems/swerve/Drive.java | 4 ++++ 2 files changed, 13 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe71830..c5862c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,6 +136,15 @@ private void configureBindings() { flywheels.setVelocityTarget(VelocityTarget.IDLE); }, flywheels)); + + driverA + .start() + .onTrue( + new InstantCommand( + () -> { + swerve.zero(); + }, + swerve)); } private void configureAutos() {} diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index 28beeba..f625ba9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -108,4 +108,8 @@ public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) { driveMode = DriveModes.TRAJECTORY; } } + + public void zero() { + gyroYawOffset = Rotation2d.fromDegrees(gyroInputs.yawPosition.getDegrees()); + } } From 111f0f9c417616b9a344b5e29b5f23803fe39e32 Mon Sep 17 00:00:00 2001 From: Jay Date: Wed, 27 Nov 2024 02:19:09 +0000 Subject: [PATCH 19/21] clean up --- src/main/java/frc/robot/RobotContainer.java | 44 ++----------------- .../frc/robot/subsystems/swerve/Drive.java | 16 ++++--- 2 files changed, 13 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c5862c9..dbe4fb4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,14 +4,11 @@ package frc.robot; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.Mode; import frc.robot.subsystems.flywheels.Flywheels; -import frc.robot.subsystems.flywheels.Flywheels.VelocityTarget; import frc.robot.subsystems.flywheels.FlywheelsIOTalonFX; import frc.robot.subsystems.rollers.Rollers; -import frc.robot.subsystems.rollers.Rollers.RollerState; import frc.robot.subsystems.rollers.intake.Intake; import frc.robot.subsystems.rollers.intake.IntakeIOTalonFX; import frc.robot.subsystems.swerve.Drive; @@ -102,49 +99,16 @@ private void configureBindings() { swerve.driveTeleopController( -driverA.getLeftY(), -driverA.getLeftX(), - driverA.getLeftTriggerAxis(), - -driverA.getRightTriggerAxis()); + driverA.getLeftTriggerAxis() - driverA.getRightTriggerAxis()); }) .withName("Drive Teleop")); + driverA.start().onTrue(swerve.zeroGyroCommand()); + // -----Intake Controls----- - driverA.x().whileTrue(rollers.setTargetCommand(RollerState.INTAKE)); // -----Flywheel Controls----- - // - driverA - .y() - .onTrue( - new InstantCommand( - () -> { - flywheels.setVelocityTarget(VelocityTarget.SHOOT); - }, - flywheels)); - driverA - .b() - .onTrue( - new InstantCommand( - () -> { - flywheels.setVelocityTarget(VelocityTarget.SLOW); - }, - flywheels)); - driverA - .a() - .onTrue( - new InstantCommand( - () -> { - flywheels.setVelocityTarget(VelocityTarget.IDLE); - }, - flywheels)); - - driverA - .start() - .onTrue( - new InstantCommand( - () -> { - swerve.zero(); - }, - swerve)); + } private void configureAutos() {} diff --git a/src/main/java/frc/robot/subsystems/swerve/Drive.java b/src/main/java/frc/robot/subsystems/swerve/Drive.java index f625ba9..03bf797 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drive.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drive.java @@ -3,12 +3,12 @@ import static frc.robot.subsystems.swerve.DriveConstants.DRIVE_CONFIG; import static frc.robot.subsystems.swerve.DriveConstants.KINEMATICS; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.subsystems.swerve.controllers.TeleopController; @@ -91,15 +91,13 @@ public void periodic() { Logger.recordOutput("Swerve/DriveMode", driveMode); } - public void driveTeleopController( - double xAxis, double yAxis, double leftTrigger, double rightTrigger) { + public void driveTeleopController(double xAxis, double yAxis, double omega) { if (DriverStation.isTeleopEnabled()) { if (driveMode != DriveModes.TELEOP) { driveMode = DriveModes.TELEOP; } - double triggers = MathUtil.applyDeadband(leftTrigger + rightTrigger, 0.07); - teleopController.acceptJoystickInput(xAxis, yAxis, triggers); + teleopController.acceptJoystickInput(xAxis, yAxis, omega); } } @@ -109,7 +107,11 @@ public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) { } } - public void zero() { - gyroYawOffset = Rotation2d.fromDegrees(gyroInputs.yawPosition.getDegrees()); + private void zeroGyro() { + gyroYawOffset = gyroInputs.yawPosition; + } + + public Command zeroGyroCommand() { + return this.runOnce(() -> zeroGyro()); } } From c504d94dbb9f6d53e3c6a4f09c3dd3cd0bfa7ea6 Mon Sep 17 00:00:00 2001 From: Jay Date: Mon, 2 Dec 2024 07:44:37 +0000 Subject: [PATCH 20/21] update gradle, vendor jsons to 2025-beta --- build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 2 +- vendordeps/AdvantageKit.json | 12 +- ...annerLib.json => PathplannerLib-beta.json} | 16 +-- ...json => Phoenix6-frc2025-beta-latest.json} | 116 ++++++++++-------- vendordeps/REVLib.json | 74 ----------- vendordeps/WPILibNewCommands.json | 2 +- vendordeps/photonlib-v2025.0.0-beta-1.json | 97 +++++++++++++++ vendordeps/playingwithfusion2024.json | 71 ----------- 9 files changed, 182 insertions(+), 210 deletions(-) rename vendordeps/{PathplannerLib.json => PathplannerLib-beta.json} (81%) rename vendordeps/{Phoenix6.json => Phoenix6-frc2025-beta-latest.json} (80%) delete mode 100644 vendordeps/REVLib.json create mode 100644 vendordeps/photonlib-v2025.0.0-beta-1.json delete mode 100644 vendordeps/playingwithfusion2024.json diff --git a/build.gradle b/build.gradle index f15821b..9b06747 100755 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index c5c2706..8e975a5 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 5fda88d..498e436 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,33 +1,33 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.2.1", + "version": "4.0.0-alpha-1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.2.1" + "version": "4.0.0-alpha-1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.2.1" + "version": "4.0.0-alpha-1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.2.1" + "version": "4.0.0-alpha-1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.2.1", + "version": "4.0.0-alpha-1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-beta.json similarity index 81% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-beta.json index ebeeb40..e79fe1a 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-beta.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2024.2.8", + "version": "2025.0.0-beta-5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.8" + "version": "2025.0.0-beta-5" } ], "jniDependencies": [], @@ -20,19 +20,19 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", + "version": "2025.0.0-beta-5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64", "osxuniversal", + "linuxx86-64", "linuxathena", "linuxarm32", "linuxarm64" ] } ] -} +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-frc2025-beta-latest.json similarity index 80% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-frc2025-beta-latest.json index 2336e9f..ff0b8c9 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-frc2025-beta-latest.json @@ -1,76 +1,94 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-frc2025-beta-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "24.3.0", - "frcYear": 2024, + "version": "25.0.0-beta-3", + "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.3.0" + "version": "25.0.0-beta-3" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.0.0-beta-3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", + "artifactId": "api-cpp-sim", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.3.0", + "artifactId": "tools-sim", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -104,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +138,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +152,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +166,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,7 +182,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,6 +190,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -173,7 +198,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,6 +206,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -188,7 +214,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,6 +222,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -203,7 +230,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,6 +238,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -218,7 +246,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,21 +254,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -248,7 +262,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,6 +270,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -263,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -271,6 +286,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -278,7 +294,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -286,6 +302,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -293,7 +310,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -301,6 +318,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +326,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +334,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +342,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,9 +350,10 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" } ] -} +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json deleted file mode 100644 index 6bb009c..0000000 --- a/vendordeps/REVLib.json +++ /dev/null @@ -1,74 +0,0 @@ -{ - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2024.2.1", - "frcYear": "2024", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2024.2.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2024.2.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2024.2.1", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2024.2.1", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf389..3718e0a 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/vendordeps/photonlib-v2025.0.0-beta-1.json b/vendordeps/photonlib-v2025.0.0-beta-1.json new file mode 100644 index 0000000..15e6421 --- /dev/null +++ b/vendordeps/photonlib-v2025.0.0-beta-1.json @@ -0,0 +1,97 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "edu.wpi.first.wpilibc", + "artifactId": "wpilibc-cpp", + "version": "2025.1.1-beta-1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-jni", + "version": "v2025.0.0-beta-1", + "skipInvalidPlatforms": true, + "isJar": true, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-1" + } + ] +} diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2024.json deleted file mode 100644 index c166cee..0000000 --- a/vendordeps/playingwithfusion2024.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "playingwithfusion2024.json", - "name": "PlayingWithFusion", - "version": "2024.03.09", - "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", - "frcYear": "2024", - "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json", - "mavenUrls": [ - "https://www.playingwithfusion.com/frc/maven/" - ], - "javaDependencies": [ - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-java", - "version": "2024.03.09" - } - ], - "jniDependencies": [ - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-driver", - "version": "2024.03.09", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxarm32", - "linuxarm64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-cpp", - "version": "2024.03.09", - "headerClassifier": "headers", - "sharedLibrary": false, - "libName": "PlayingWithFusion", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxarm32", - "linuxarm64" - ] - }, - { - "groupId": "com.playingwithfusion.frc", - "artifactId": "PlayingWithFusion-driver", - "version": "2024.03.09", - "headerClassifier": "headers", - "sharedLibrary": true, - "libName": "PlayingWithFusionDriver", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file From 681b4f85588abaaa94365e5a17d8df471e74481d Mon Sep 17 00:00:00 2001 From: Jay Date: Mon, 2 Dec 2024 08:19:20 +0000 Subject: [PATCH 21/21] 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) {