From 831c680079ffe12995e29f265a6914a529c5e048 Mon Sep 17 00:00:00 2001 From: KonnorR Date: Fri, 10 Jan 2025 20:57:46 -0500 Subject: [PATCH 01/11] Added PigeonViaTalonSRX UNTESTED!! --- .../imu/PigeonViaTalonSRXSwerve.java | 151 ++++++++++++++++++ .../swervelib/parser/json/DeviceJson.java | 12 +- 2 files changed, 154 insertions(+), 9 deletions(-) create mode 100644 src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java diff --git a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java new file mode 100644 index 000000000..4057e712a --- /dev/null +++ b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java @@ -0,0 +1,151 @@ +package swervelib.imu; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.Optional; + +import static edu.wpi.first.units.Units.DegreesPerSecond; + +/** + * SwerveIMU interface for the {@link WPI_PigeonIMU}. + */ +public class PigeonViaTalonSRXSwerve extends SwerveIMU +{ + + + /** + * {@link TalonSRX} TalonSRX the IMU is attached to. + */ + private final TalonSRX talon; + + /** + * {@link WPI_PigeonIMU} IMU device. + */ + private final WPI_PigeonIMU imu; + /** + * Mutable {@link AngularVelocity} for readings. + */ + private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); + /** + * Offset for the {@link WPI_PigeonIMU}. + */ + private Rotation3d offset = new Rotation3d(); + /** + * Inversion for the gyro + */ + private boolean invertedIMU = false; + + /** + * Generate the SwerveIMU for {@link WPI_PigeonIMU}. + * + * @param canid CAN ID for the {@link WPI_PigeonIMU}, does not support CANBus. + */ + public PigeonViaTalonSRXSwerve(int canid) + { + talon = new TalonSRX(canid); + imu = new WPI_PigeonIMU(talon); + offset = new Rotation3d(); + SmartDashboard.putData(imu); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + imu.configFactoryDefault(); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + imu.clearStickyFaults(); + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Set the gyro to invert its default direction + * + * @param invertIMU invert gyro direction + */ + public void setInverted(boolean invertIMU) + { + invertedIMU = invertIMU; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() + { + double[] wxyz = new double[4]; + imu.get6dQuaternion(wxyz); + Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); + return invertedIMU ? reading.unaryMinus() : reading; + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + short[] initial = new short[3]; + imu.getBiasedAccelerometer(initial); + return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); + } + + @Override + public MutAngularVelocity getYawAngularVelocity() + { + return yawVel.mut_setMagnitude(imu.getRate()); + } + + /** + * Get the instantiated {@link WPI_PigeonIMU} IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } +} diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 8cc332132..264167e1c 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -18,15 +18,7 @@ import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.encoders.TalonSRXEncoderSwerve; import swervelib.encoders.ThriftyNovaEncoderSwerve; -import swervelib.imu.ADIS16448Swerve; -import swervelib.imu.ADIS16470Swerve; -import swervelib.imu.ADXRS450Swerve; -import swervelib.imu.AnalogGyroSwerve; -import swervelib.imu.CanandgyroSwerve; -import swervelib.imu.NavXSwerve; -import swervelib.imu.Pigeon2Swerve; -import swervelib.imu.PigeonSwerve; -import swervelib.imu.SwerveIMU; +import swervelib.imu.*; import swervelib.motors.SparkFlexSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve.Type; @@ -154,6 +146,8 @@ public SwerveIMU createIMU() return new NavXSwerve(NavXComType.kMXP_UART); case "pigeon": return new PigeonSwerve(id); + case "pigeon_via_talonsrx" : + return new PigeonViaTalonSRXSwerve(id); case "pigeon2": return new Pigeon2Swerve(id, canbus != null ? canbus : ""); default: From fe4204f50b1632fb3edb4e7086a20bb1df97c19f Mon Sep 17 00:00:00 2001 From: KonnorR Date: Sat, 11 Jan 2025 18:29:25 -0500 Subject: [PATCH 02/11] Update build.gradle --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 61aa57fce..bfd20c1c2 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } java { From f0abb6e68a80e7365c025ecb640b1a1f9a6b081f Mon Sep 17 00:00:00 2001 From: KonnorR Date: Sat, 11 Jan 2025 19:04:21 -0500 Subject: [PATCH 03/11] Added swerveDrive.getPigeonAttachedTalonSRX --- .../subsystems/swervedrive/SwerveSubsystem.java | 1 + src/main/java/swervelib/SwerveDrive.java | 17 ++++++++++++++++- .../swervelib/imu/PigeonViaTalonSRXSwerve.java | 14 ++++++++++++++ 3 files changed, 31 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index e64a63e3a..50bcf3469 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -127,6 +127,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig Constants.MAX_SPEED, new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), Rotation2d.fromDegrees(0))); + } // /** diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index c18e5d1ed..0ecc9f3a2 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -12,6 +12,7 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -62,6 +63,7 @@ import swervelib.encoders.CANCoderSwerve; import swervelib.imu.Pigeon2Swerve; +import swervelib.imu.PigeonViaTalonSRXSwerve; import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; import swervelib.motors.TalonFXSwerve; @@ -189,6 +191,10 @@ public class SwerveDrive * Simulation of the swerve drive. */ private SwerveIMUSimulation simIMU; + /** + * Pigeon attached to Talon class + */ + public final PigeonViaTalonSRXSwerve pigeonViaTalonSRXSwerve; /** * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ @@ -947,7 +953,6 @@ public SwerveModulePosition[] getModulePositions() } return positions; } - /** * Getter for the {@link SwerveIMU}. * @@ -1056,6 +1061,16 @@ public Optional getAccel() } } + + /** + * Gets the {@link TalonSRX} that the WPI_PigeonIMU is attached to. + * + * @return the {@link TalonSRX} created for the WPI_PigeonIMU. + */ + public TalonSRX getPigeonAttachedTalonSRX() { + return pigeonViaTalonSRXSwerve.getTalonSRX(); + } + /** * Sets the drive motors to brake/coast mode. * diff --git a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java index 4057e712a..bf0855d46 100644 --- a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java +++ b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; @@ -50,11 +51,24 @@ public class PigeonViaTalonSRXSwerve extends SwerveIMU public PigeonViaTalonSRXSwerve(int canid) { talon = new TalonSRX(canid); + /** + * + * I want to add something like this but not too sure how to check if the talon was properly created + * + if (talon is not on canbus) { + DriverStation.reportWarning("pigeon_via_talonsrx can id should be the attached talon's can id", true); + } + */ + imu = new WPI_PigeonIMU(talon); offset = new Rotation3d(); SmartDashboard.putData(imu); } + public TalonSRX getTalonSRX() { + return talon; + } + /** * Reset IMU to factory default. */ From d43c8f2c611a4b2aaf59756ae52ed11459aa4f57 Mon Sep 17 00:00:00 2001 From: KonnorR Date: Sun, 12 Jan 2025 19:16:54 -0500 Subject: [PATCH 04/11] Canceled Talon Detection No TalonSRX detection to attempt to see if the user has made a fault. I had an idea like below but don't like it. enableVoltageComp if (!voltageComp) { DriverStation.Error } else { disableVoltageComp } --- .../java/swervelib/imu/PigeonViaTalonSRXSwerve.java | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java index bf0855d46..d1fdcab67 100644 --- a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java +++ b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java @@ -44,22 +44,13 @@ public class PigeonViaTalonSRXSwerve extends SwerveIMU private boolean invertedIMU = false; /** - * Generate the SwerveIMU for {@link WPI_PigeonIMU}. + * Generate the SwerveIMU for {@link WPI_PigeonIMU} attached to a {@link TalonSRX}. * - * @param canid CAN ID for the {@link WPI_PigeonIMU}, does not support CANBus. + * @param canid CAN ID for the {@link TalonSRX} the {@link WPI_PigeonIMU} is attached to, does not support CANBus. */ public PigeonViaTalonSRXSwerve(int canid) { talon = new TalonSRX(canid); - /** - * - * I want to add something like this but not too sure how to check if the talon was properly created - * - if (talon is not on canbus) { - DriverStation.reportWarning("pigeon_via_talonsrx can id should be the attached talon's can id", true); - } - */ - imu = new WPI_PigeonIMU(talon); offset = new Rotation3d(); SmartDashboard.putData(imu); From 9e725a5fd426ad954821ab574bbfa7a957640482 Mon Sep 17 00:00:00 2001 From: KonnorR Date: Sun, 12 Jan 2025 19:32:12 -0500 Subject: [PATCH 05/11] Added the can id Forgot to pull the can id into the class call lol. --- src/main/java/swervelib/SwerveDrive.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 0ecc9f3a2..451623a11 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -70,6 +70,7 @@ import swervelib.parser.Cache; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; +import swervelib.parser.SwerveParser; import swervelib.simulation.SwerveIMUSimulation; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -194,7 +195,7 @@ public class SwerveDrive /** * Pigeon attached to Talon class */ - public final PigeonViaTalonSRXSwerve pigeonViaTalonSRXSwerve; + public final PigeonViaTalonSRXSwerve pigeonViaTalonSRXSwerve = new PigeonViaTalonSRXSwerve(SwerveParser.swerveDriveJson.imu.id); /** * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ From 5e9fb6e1f4d85a815d60e6c1afd37ac3ff51b596 Mon Sep 17 00:00:00 2001 From: KonnorR Date: Thu, 16 Jan 2025 23:16:55 -0500 Subject: [PATCH 06/11] First Actual Code Added some basic elevator and shooter code and for now just a spark max controller. --- src/main/java/swervelib/parser/PIDFConfig.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/swervelib/parser/PIDFConfig.java b/src/main/java/swervelib/parser/PIDFConfig.java index 9da06849c..f6b86b53e 100644 --- a/src/main/java/swervelib/parser/PIDFConfig.java +++ b/src/main/java/swervelib/parser/PIDFConfig.java @@ -1,7 +1,7 @@ package swervelib.parser; import edu.wpi.first.math.controller.PIDController; -import swervelib.parser.deserializer.PIDFRange; +import maniplib.utils.deserializer.PIDFRange; /** * Hold the PIDF and Integral Zone values for a PID. From 280561b17e48e4072597e76039081fba930700cc Mon Sep 17 00:00:00 2001 From: KonnorR Date: Thu, 16 Jan 2025 23:18:55 -0500 Subject: [PATCH 07/11] Revert "First Actual Code" This reverts commit 5e9fb6e1f4d85a815d60e6c1afd37ac3ff51b596. --- src/main/java/swervelib/parser/PIDFConfig.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/swervelib/parser/PIDFConfig.java b/src/main/java/swervelib/parser/PIDFConfig.java index f6b86b53e..9da06849c 100644 --- a/src/main/java/swervelib/parser/PIDFConfig.java +++ b/src/main/java/swervelib/parser/PIDFConfig.java @@ -1,7 +1,7 @@ package swervelib.parser; import edu.wpi.first.math.controller.PIDController; -import maniplib.utils.deserializer.PIDFRange; +import swervelib.parser.deserializer.PIDFRange; /** * Hold the PIDF and Integral Zone values for a PID. From 7e8df72eced523521cb4c4ced0929532a7760e84 Mon Sep 17 00:00:00 2001 From: KonnorR Date: Fri, 17 Jan 2025 18:32:50 -0500 Subject: [PATCH 08/11] Added SparkFlexEncoderSwerve Added roughly by copying the existing SparkMaxEncoderSwerve and making very minor tweaks under the assumption sparkflex and max are similar enough as I have not used them. --- .../encoders/SparkFlexEncoderSwerve.java | 170 ++++++++++++++++++ .../swervelib/parser/json/DeviceJson.java | 15 +- 2 files changed, 176 insertions(+), 9 deletions(-) create mode 100644 src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java diff --git a/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java new file mode 100644 index 000000000..b6257f132 --- /dev/null +++ b/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java @@ -0,0 +1,170 @@ +package swervelib.encoders; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SwerveMotor; + +import java.util.function.Supplier; + +/** + * SparkMax absolute encoder, attached through the data port. + */ +public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. + */ + public SparkAbsoluteEncoder encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring; + /** + * An {@link Alert} for if there is a failure configuring the encoder offset. + */ + private Alert offsetFailure; + /** + * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance. + */ + private SwerveMotor sparkFlex; + + /** + * Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex} + * motor. + * + * @param motor Motor to create the encoder from. + * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. + */ + public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor) + { + failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Absolute Encoder", + AlertType.kWarning); + offsetFailure = new Alert( + "Encoders", + "Failure to set Absolute Encoder Offset", + AlertType.kWarning); + if (motor.getMotor() instanceof SparkFlex) + { + sparkFlex = motor; + encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder(); + motor.setAbsoluteEncoder(this); + motor.configureIntegratedEncoder(conversionFactor); + } else + { + throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + } + } + + /** + * Run the configuration until it succeeds or times out. + * + * @param config Lambda supplier returning the error state. + */ + private void configureSparkMax(Supplier config) + { + for (int i = 0; i < maximumRetries; i++) + { + if (config.get() == REVLibError.kOk) + { + return; + } + } + failureConfiguring.set(true); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + if (sparkFlex instanceof SparkMaxSwerve) + { + SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig(); + cfg.absoluteEncoder.inverted(inverted); + ((SparkMaxSwerve) sparkFlex).updateConfig(cfg); + } + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return encoder.getPosition(); + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + if (sparkFlex instanceof SparkMaxSwerve) + { + SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig(); + cfg.absoluteEncoder.zeroOffset(offset); + ((SparkMaxSwerve) sparkFlex).updateConfig(cfg); + return true; + } + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + return encoder.getVelocity(); + } +} diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 264167e1c..65f4b5eb9 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -9,15 +9,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; -import swervelib.encoders.AnalogAbsoluteEncoderSwerve; -import swervelib.encoders.CANCoderSwerve; -import swervelib.encoders.CanAndMagSwerve; -import swervelib.encoders.PWMDutyCycleEncoderSwerve; -import swervelib.encoders.SparkMaxAnalogEncoderSwerve; -import swervelib.encoders.SparkMaxEncoderSwerve; -import swervelib.encoders.SwerveAbsoluteEncoder; -import swervelib.encoders.TalonSRXEncoderSwerve; -import swervelib.encoders.ThriftyNovaEncoderSwerve; +import swervelib.encoders.*; import swervelib.imu.*; import swervelib.motors.SparkFlexSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve; @@ -73,6 +65,11 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) return new SparkMaxAnalogEncoderSwerve(motor, 3.3); case "sparkmax_analog5v": return new SparkMaxAnalogEncoderSwerve(motor, 5); + case "sparkflex_integrated": + case "sparkflex_attached": + case "sparkflex_canandmag": + case "sparkflex_canandcoder": + return new SparkFlexEncoderSwerve(motor, 360); case "canandcoder_can": case "canandmag_can": return new CanAndMagSwerve(id); From 73f61734f00ccbe1d71b99bae0acce2c9c22db80 Mon Sep 17 00:00:00 2001 From: KonnorR Date: Mon, 20 Jan 2025 18:38:01 -0500 Subject: [PATCH 09/11] Reverted gePigeonAttachedTalonSRX --- src/main/java/swervelib/SwerveDrive.java | 14 -------------- .../swervelib/imu/PigeonViaTalonSRXSwerve.java | 4 ---- 2 files changed, 18 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 72732d5b1..3a1a55a3d 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -188,10 +188,6 @@ public class SwerveDrive * Simulation of the swerve drive. */ private SwerveIMUSimulation simIMU; - /** - * Pigeon attached to Talon class - */ - public final PigeonViaTalonSRXSwerve pigeonViaTalonSRXSwerve = new PigeonViaTalonSRXSwerve(SwerveParser.swerveDriveJson.imu.id); /** * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ @@ -1059,16 +1055,6 @@ public Optional getAccel() } } - - /** - * Gets the {@link TalonSRX} that the WPI_PigeonIMU is attached to. - * - * @return the {@link TalonSRX} created for the WPI_PigeonIMU. - */ - public TalonSRX getPigeonAttachedTalonSRX() { - return pigeonViaTalonSRXSwerve.getTalonSRX(); - } - /** * Sets the drive motors to brake/coast mode. * diff --git a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java index d1fdcab67..641511a5e 100644 --- a/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java +++ b/src/main/java/swervelib/imu/PigeonViaTalonSRXSwerve.java @@ -56,10 +56,6 @@ public PigeonViaTalonSRXSwerve(int canid) SmartDashboard.putData(imu); } - public TalonSRX getTalonSRX() { - return talon; - } - /** * Reset IMU to factory default. */ From 69c3d93f0c4800bb968e6eaa72d9ba6970bce990 Mon Sep 17 00:00:00 2001 From: KonnorR Date: Mon, 20 Jan 2025 23:20:06 -0500 Subject: [PATCH 10/11] Reformat --- src/main/java/swervelib/SwerveDrive.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 3a1a55a3d..5740a560a 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -11,7 +11,6 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -59,14 +58,12 @@ import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; import swervelib.encoders.CANCoderSwerve; import swervelib.imu.Pigeon2Swerve; -import swervelib.imu.PigeonViaTalonSRXSwerve; import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; import swervelib.motors.TalonFXSwerve; import swervelib.parser.Cache; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; -import swervelib.parser.SwerveParser; import swervelib.simulation.SwerveIMUSimulation; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -947,6 +944,7 @@ public SwerveModulePosition[] getModulePositions() } return positions; } + /** * Getter for the {@link SwerveIMU}. * From 466a6cfa878dfebb236e7edf9eea20fa13edf56b Mon Sep 17 00:00:00 2001 From: KonnorR Date: Tue, 21 Jan 2025 19:14:55 -0500 Subject: [PATCH 11/11] Fixed incomplete SparkFlex implementation --- .../swervedrive/SwerveSubsystem.java | 1 - .../encoders/SparkFlexEncoderSwerve.java | 31 +++++++++---------- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index b8d2fec06..f08e74cf9 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -127,7 +127,6 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig Constants.MAX_SPEED, new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), Rotation2d.fromDegrees(0))); - } /** diff --git a/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java index b6257f132..58cd7f4e8 100644 --- a/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkFlexEncoderSwerve.java @@ -4,23 +4,22 @@ import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import swervelib.motors.SparkMaxBrushedMotorSwerve; -import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SparkFlexSwerve; import swervelib.motors.SwerveMotor; import java.util.function.Supplier; /** - * SparkMax absolute encoder, attached through the data port. + * SparkFlex absolute encoder, attached through the data port. */ public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder { /** - * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. + * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkFlex. */ public SparkAbsoluteEncoder encoder; /** @@ -32,7 +31,7 @@ public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder */ private Alert offsetFailure; /** - * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance. + * {@link SparkFlexSwerve} instance. */ private SwerveMotor sparkFlex; @@ -47,7 +46,7 @@ public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor) { failureConfiguring = new Alert( "Encoders", - "Failure configuring SparkMax Absolute Encoder", + "Failure configuring SparkFlex Absolute Encoder", AlertType.kWarning); offsetFailure = new Alert( "Encoders", @@ -61,7 +60,7 @@ public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor) motor.configureIntegratedEncoder(conversionFactor); } else { - throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); + throw new RuntimeException("Motor given to instantiate SparkFlexEncoder is not a CANSparkFlex"); } } @@ -70,7 +69,7 @@ public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor) * * @param config Lambda supplier returning the error state. */ - private void configureSparkMax(Supplier config) + private void configureSparkFlex(Supplier config) { for (int i = 0; i < maximumRetries; i++) { @@ -108,11 +107,11 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - if (sparkFlex instanceof SparkMaxSwerve) + if (sparkFlex instanceof SparkFlexSwerve) { - SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig(); + SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig(); cfg.absoluteEncoder.inverted(inverted); - ((SparkMaxSwerve) sparkFlex).updateConfig(cfg); + ((SparkFlexSwerve) sparkFlex).updateConfig(cfg); } } @@ -139,7 +138,7 @@ public Object getAbsoluteEncoder() } /** - * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. + * Sets the Absolute Encoder Offset inside of the SparkFlex's Memory. * * @param offset the offset the Absolute Encoder uses as the zero point. * @return if setting Absolute Encoder Offset was successful or not. @@ -147,11 +146,11 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - if (sparkFlex instanceof SparkMaxSwerve) + if (sparkFlex instanceof SparkFlexSwerve) { - SparkMaxConfig cfg = ((SparkMaxSwerve) sparkFlex).getConfig(); + SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig(); cfg.absoluteEncoder.zeroOffset(offset); - ((SparkMaxSwerve) sparkFlex).updateConfig(cfg); + ((SparkFlexSwerve) sparkFlex).updateConfig(cfg); return true; } return false;