From cc0615c740552300b18a5ca23029c55504f6743e Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Wed, 18 Dec 2024 15:05:56 -0600 Subject: [PATCH] Removed unfinished portion. Reformatted library. Signed-off-by: thenetworkgrinch --- .../java/swervelib/SwerveInputStream.java | 95 +++++++------ src/main/java/swervelib/SwerveModule.java | 2 - .../swervelib/encoders/CANCoderSwerve.java | 10 +- .../swervelib/motors/SparkFlexSwerve.java | 17 --- .../motors/SparkMaxBrushedMotorSwerve.java | 15 -- .../java/swervelib/motors/SparkMaxSwerve.java | 15 -- .../java/swervelib/motors/SwerveMotor.java | 8 -- .../java/swervelib/motors/TalonFXSwerve.java | 11 -- .../java/swervelib/motors/TalonSRXSwerve.java | 6 - .../telemetry/SwerveDriveTelemetry.java | 132 +++++++++--------- 10 files changed, 121 insertions(+), 190 deletions(-) diff --git a/src/main/java/swervelib/SwerveInputStream.java b/src/main/java/swervelib/SwerveInputStream.java index 4a8748da..86a62b37 100644 --- a/src/main/java/swervelib/SwerveInputStream.java +++ b/src/main/java/swervelib/SwerveInputStream.java @@ -88,53 +88,6 @@ public class SwerveInputStream implements Supplier private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY; - /** - * Drive modes to keep track of. - */ - enum SwerveInputMode - { - /** - * Translation only mode, does not allow for rotation and maintains current heading. - */ - TRANSLATION_ONLY, - /** - * Output based off angular velocity - */ - ANGULAR_VELOCITY, - /** - * Output based off of heading. - */ - HEADING, - /** - * Output based off of targeting. - */ - AIM - } - - /** - * Copy the {@link SwerveInputStream} object. - * - * @return Clone of current {@link SwerveInputStream} - */ - public SwerveInputStream copy() - { - SwerveInputStream newStream = new SwerveInputStream(swerveDrive, controllerTranslationX, controllerTranslationY); - newStream.controllerOmega = controllerOmega; - newStream.controllerHeadingX = controllerHeadingX; - newStream.controllerHeadingY = controllerHeadingY; - newStream.axisDeadband = axisDeadband; - newStream.translationAxisScale = translationAxisScale; - newStream.omegaAxisScale = omegaAxisScale; - newStream.aimTarget = aimTarget; - newStream.headingEnabled = headingEnabled; - newStream.aimEnabled = aimEnabled; - newStream.currentMode = currentMode; - newStream.translationOnlyEnabled = translationOnlyEnabled; - newStream.lockedHeading = lockedHeading; - newStream.swerveController = swerveController; - return newStream; - } - /** * Create a {@link SwerveInputStream} for an easy way to generate {@link ChassisSpeeds} from a driver controller. * @@ -193,6 +146,30 @@ public static SwerveInputStream of(SwerveDrive drive, DoubleSupplier x, DoubleSu return new SwerveInputStream(drive, x, y); } + /** + * Copy the {@link SwerveInputStream} object. + * + * @return Clone of current {@link SwerveInputStream} + */ + public SwerveInputStream copy() + { + SwerveInputStream newStream = new SwerveInputStream(swerveDrive, controllerTranslationX, controllerTranslationY); + newStream.controllerOmega = controllerOmega; + newStream.controllerHeadingX = controllerHeadingX; + newStream.controllerHeadingY = controllerHeadingY; + newStream.axisDeadband = axisDeadband; + newStream.translationAxisScale = translationAxisScale; + newStream.omegaAxisScale = omegaAxisScale; + newStream.aimTarget = aimTarget; + newStream.headingEnabled = headingEnabled; + newStream.aimEnabled = aimEnabled; + newStream.currentMode = currentMode; + newStream.translationOnlyEnabled = translationOnlyEnabled; + newStream.lockedHeading = lockedHeading; + newStream.swerveController = swerveController; + return newStream; + } + /** * Add a rotation axis for Angular Velocity control * @@ -255,7 +232,6 @@ public SwerveInputStream scaleRotation(double scaleRotation) return this; } - /** * Output {@link ChassisSpeeds} based on heading while the supplier is True. * @@ -573,4 +549,27 @@ public ChassisSpeeds get() return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); } + + /** + * Drive modes to keep track of. + */ + enum SwerveInputMode + { + /** + * Translation only mode, does not allow for rotation and maintains current heading. + */ + TRANSLATION_ONLY, + /** + * Output based off angular velocity + */ + ANGULAR_VELOCITY, + /** + * Output based off of heading. + */ + HEADING, + /** + * Output based off of targeting. + */ + AIM + } } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 5272aa0d..ed6f48f8 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -197,7 +197,6 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat // Config angle motor/controller angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); - angleMotor.configureConversionFactor(moduleConfiguration.conversionFactors); angleMotor.configurePIDF(moduleConfiguration.anglePIDF); angleMotor.configurePIDWrapping(0, 360); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); @@ -211,7 +210,6 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat // Config drive motor/controller driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive.factor); - driveMotor.configureConversionFactor(moduleConfiguration.conversionFactors); driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); driveMotor.setInverted(moduleConfiguration.driveMotorInverted); driveMotor.setMotorBrake(true); diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 3789dc3b..7dfe6a37 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -27,11 +27,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(10).in(Seconds); - /** - * CANCoder with WPILib sendable and support. - */ - public CANcoder encoder; + public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(10).in(Seconds); /** * An {@link Alert} for if the CANCoder magnet field is less than ideal. */ @@ -60,6 +56,10 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder * Angular velocity of the {@link CANcoder}. */ private final StatusSignal velocity; + /** + * CANCoder with WPILib sendable and support. + */ + public CANcoder encoder; /** * {@link CANcoder} Configurator objet for this class. */ diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index e63624c5..5b5002f0 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -11,7 +11,6 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; @@ -25,7 +24,6 @@ import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -74,15 +72,6 @@ public class SparkFlexSwerve extends SwerveMotor * Configuration object for {@link SparkFlex} motor. */ private SparkFlexConfig cfg = new SparkFlexConfig(); - /** - * After the first post-module config update there will be an error thrown to alert to a possible issue. - */ - private boolean startupInitialized = false; - /** - * Module Conversion factors to use. - */ - private ConversionFactorsJson moduleConversionFactors; - /** @@ -364,12 +353,6 @@ public void configureIntegratedEncoder(double positionConversionFactor) } - @Override - public void configureConversionFactor(ConversionFactorsJson factorsJson) - { - this.moduleConversionFactors = factorsJson; - } - /** * Configure the PIDF values for the closed loop controller. * diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 0b9bc384..d447600a 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -26,7 +26,6 @@ import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -79,14 +78,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor * Configuration object for {@link SparkMax} motor. */ private SparkMaxConfig cfg = new SparkMaxConfig(); - /** - * Module Conversion factors to use. - */ - private ConversionFactorsJson moduleConversionFactors; - /** - * After the first post-module config update there will be an error thrown to alert to a possible issue. - */ - private boolean startupInitialized = false; /** * Initialize the swerve motor. @@ -440,12 +431,6 @@ public void configureIntegratedEncoder(double positionConversionFactor) } } - @Override - public void configureConversionFactor(ConversionFactorsJson factorsJson) - { - this.moduleConversionFactors = factorsJson; - } - /** * Configure the PIDF values for the closed loop controller. * diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 8d8ef2b3..aae5cbf7 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -23,7 +23,6 @@ import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -64,14 +63,6 @@ public class SparkMaxSwerve extends SwerveMotor * Configuration object for {@link SparkMax} motor. */ private SparkMaxConfig cfg = new SparkMaxConfig(); - /** - * Module Conversion factors to use. - */ - private ConversionFactorsJson moduleConversionFactors; - /** - * After the first post-module config update there will be an error thrown to alert to a possible issue. - */ - private boolean startupInitialized = false; /** @@ -366,12 +357,6 @@ public void configureIntegratedEncoder(double positionConversionFactor) } - @Override - public void configureConversionFactor(ConversionFactorsJson factorsJson) - { - this.moduleConversionFactors = factorsJson; - } - /** * Configure the PIDF values for the closed loop controller. * diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java index cd8834b9..06940c6b 100644 --- a/src/main/java/swervelib/motors/SwerveMotor.java +++ b/src/main/java/swervelib/motors/SwerveMotor.java @@ -3,7 +3,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -import swervelib.parser.json.modules.ConversionFactorsJson; /** * Swerve motor abstraction which defines a standard interface for motors within a swerve module. @@ -51,13 +50,6 @@ public abstract class SwerveMotor */ public abstract void configureIntegratedEncoder(double positionConversionFactor); - /** - * Configure the motor controller conversion factors based off given ConversionFactors. - * - * @param factorsJson {@link ConversionFactorsJson} from {@link swervelib.parser.SwerveParser}. - */ - public abstract void configureConversionFactor(ConversionFactorsJson factorsJson); - /** * Configure the PIDF values for the closed loop controller. 0 is disabled or off. * diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 87dba396..7d3b98a6 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -52,10 +51,6 @@ public class TalonFXSwerve extends SwerveMotor * Conversion factor for the motor. */ private double conversionFactor; - /** - * Module Conversion factors to use. - */ - private ConversionFactorsJson moduleConversionFactors; /** * Current TalonFX configuration. */ @@ -196,12 +191,6 @@ public void configureIntegratedEncoder(double positionConversionFactor) cfg.apply(configuration); } - @Override - public void configureConversionFactor(ConversionFactorsJson factorsJson) - { - this.moduleConversionFactors = factorsJson; - } - /** * Configure the PIDF values for the closed loop controller. 0 is disabled or off. * diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 0ce8ab92..e1dba2e3 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -143,12 +143,6 @@ public void configureIntegratedEncoder(double positionConversionFactor) configureCANStatusFrames(250); } - @Override - public void configureConversionFactor(ConversionFactorsJson factorsJson) - { - this.moduleConversionFactors = factorsJson; - } - /** * Set the CAN status frames. * diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index fa010f1c..8bd59642 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -168,119 +168,125 @@ public class SwerveDriveTelemetry /** * Wheel locations array publisher for NT4. */ - private static final DoubleArrayPublisher wheelLocationsArrayPublisher = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getDoubleArrayTopic( - "swerve/wheelLocation") - .publish(); + private static final DoubleArrayPublisher wheelLocationsArrayPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleArrayTopic( + "swerve/wheelLocation") + .publish(); /** * Max speed publisher for NT4. */ - private static final DoublePublisher maxSpeedPublisher = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getDoubleTopic( - "swerve/maxSpeed") - .publish(); + private static final DoublePublisher maxSpeedPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleTopic( + "swerve/maxSpeed") + .publish(); /** * Rotation unit for NT4. */ - private static final StringPublisher rotationUnitPublisher = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getStringTopic( - "swerve/rotationUnit") - .publish(); + private static final StringPublisher rotationUnitPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getStringTopic( + "swerve/rotationUnit") + .publish(); /** * Chassis width publisher */ - private static final DoublePublisher sizeLeftRightPublisher = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getDoubleTopic( - "swerve/sizeLeftRight") - .publish(); + private static final DoublePublisher sizeLeftRightPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleTopic( + "swerve/sizeLeftRight") + .publish(); /** * Chassis Length publisher. */ - private static final DoublePublisher sizeFrontBackPublisher = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getDoubleTopic( - "swerve/sizeFrontBack") - .publish(); + private static final DoublePublisher sizeFrontBackPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleTopic( + "swerve/sizeFrontBack") + .publish(); /** * Chassis direction widget publisher. */ - private static final StringPublisher forwardDirectionPublisher = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getStringTopic( - "swerve/forwardDirection") - .publish(); + private static final StringPublisher forwardDirectionPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getStringTopic( + "swerve/forwardDirection") + .publish(); /** * Odometry cycle time, updated whenever {@link SwerveDrive#updateOdometry()} is called. */ private static final DoublePublisher odomCycleTime - = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getDoubleTopic( - "swerve/odomCycleMS") - .publish(); + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleTopic( + "swerve/odomCycleMS") + .publish(); /** * Control cycle time, updated whenever * {@link swervelib.SwerveModule#setDesiredState(SwerveModuleState, boolean, double)} is called for the last module. */ private static final DoublePublisher ctrlCycleTime - = NetworkTableInstance.getDefault() - .getTable( - "SmartDashboard") - .getDoubleTopic( - "swerve/controlCycleMS") - .publish(); + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleTopic( + "swerve/controlCycleMS") + .publish(); /** * Odometry timer to track cycle times. */ - private static final Timer odomTimer = new Timer(); + private static final Timer odomTimer = new Timer(); /** * Control timer to track cycle times. */ - private static final Timer ctrlTimer = new Timer(); + private static final Timer ctrlTimer = new Timer(); /** * Measured swerve module states object. */ public static SwerveModuleState[] measuredStatesObj - = new SwerveModuleState[4]; + = new SwerveModuleState[4]; /** * Desired swerve module states object */ public static SwerveModuleState[] desiredStatesObj - = new SwerveModuleState[4]; + = new SwerveModuleState[4]; /** * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the * chassis speeds properties. */ - public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds(); + public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds(); /** * Describes the desired forward, sideways and angular velocity of the robot. */ - public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds(); + public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds(); /** * The robot's current rotation based on odometry or gyro readings */ - public static Rotation2d robotRotationObj = new Rotation2d(); + public static Rotation2d robotRotationObj = new Rotation2d(); /** * The current telemetry verbosity level. */ public static TelemetryVerbosity verbosity - = TelemetryVerbosity.MACHINE; + = TelemetryVerbosity.MACHINE; /** * State of simulation of the Robot, used to optimize retrieval. */ public static boolean isSimulation - = RobotBase.isSimulation(); + = RobotBase.isSimulation(); /** * The number of swerve modules */ @@ -300,7 +306,7 @@ public class SwerveDriveTelemetry /** * The robot's current rotation based on odometry or gyro readings */ - public static double robotRotation = 0; + public static double robotRotation = 0; /** * The maximum achievable speed of the modules, used to adjust the size of the vectors. */ @@ -308,7 +314,7 @@ public class SwerveDriveTelemetry /** * The units of the module rotations and robot rotation */ - public static String rotationUnit = "degrees"; + public static String rotationUnit = "degrees"; /** * The distance between the left and right modules. */ @@ -321,7 +327,7 @@ public class SwerveDriveTelemetry * The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to * align with odometry data or match videos. 'up', 'right', 'down' or 'left' */ - public static String forwardDirection = "up"; + public static String forwardDirection = "up"; /** * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the * chassis speeds properties. @@ -331,15 +337,15 @@ public class SwerveDriveTelemetry * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the * chassis speeds properties. */ - public static double[] measuredChassisSpeeds = new double[3]; + public static double[] measuredChassisSpeeds = new double[3]; /** * Describes the desired forward, sideways and angular velocity of the robot. */ - public static double[] desiredChassisSpeeds = new double[3]; + public static double[] desiredChassisSpeeds = new double[3]; /** * Update the telemetry settings that infrequently change. */ - public static boolean updateSettings = true; + public static boolean updateSettings = true; /** * Start the ctrl timer to measure cycle time, independent of periodic loops.