From 86595fd770e79bcfd19054ac01257ab30891b28d Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Thu, 24 Oct 2024 10:06:21 -0500 Subject: [PATCH] Reformatted code, updated javadocs. Signed-off-by: thenetworkgrinch --- src/main/java/frc/robot/Constants.java | 3 +- src/main/java/frc/robot/Robot.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 40 +++---- .../drivebase/AbsoluteDriveAdv.java | 2 +- .../swervedrive/SwerveSubsystem.java | 61 +++++----- .../robot/subsystems/swervedrive/Vision.java | 11 +- src/main/java/swervelib/SwerveDrive.java | 109 ++++++++++-------- src/main/java/swervelib/SwerveModule.java | 21 ++-- .../java/swervelib/imu/CanandgyroSwerve.java | 27 ++--- src/main/java/swervelib/imu/IMUVelocity.java | 71 ++++++------ src/main/java/swervelib/imu/NavXSwerve.java | 13 ++- .../java/swervelib/imu/Pigeon2Swerve.java | 34 +++--- src/main/java/swervelib/imu/PigeonSwerve.java | 18 +-- .../math/IMULinearMovingAverageFilter.java | 47 ++++---- .../motors/SparkMaxBrushedMotorSwerve.java | 2 +- .../java/swervelib/motors/SparkMaxSwerve.java | 2 +- .../swervelib/parser/json/ModuleJson.java | 2 - 17 files changed, 241 insertions(+), 229 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3bcfa386..0c89bbfc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,7 +5,6 @@ package frc.robot; import com.pathplanner.lib.util.PIDConstants; - import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import swervelib.math.Matter; @@ -25,7 +24,7 @@ public final class Constants public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag public static final double MAX_SPEED = Units.feetToMeters(14.5); - // Maximum speed of the robot in meters per second, used to limit acceleration. + // Maximum speed of the robot in meters per second, used to limit acceleration. public static final class AutonConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 52a18c40..4750235d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,14 +4,10 @@ package frc.robot; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import java.io.File; -import java.io.IOException; -import swervelib.parser.SwerveParser; /** * The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as @@ -125,7 +121,8 @@ public void teleopInit() if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); - } else { + } else + { CommandScheduler.getInstance().cancelAll(); } m_robotContainer.setDriveMode(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 56b25cea..2e6fb1d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,11 +8,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -31,10 +29,10 @@ public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed - final CommandXboxController driverXbox = new CommandXboxController(0); + final CommandXboxController driverXbox = new CommandXboxController(0); // The robot's subsystems and commands are defined here... - private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), - "swerve/neo")); + private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), + "swerve/neo")); // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive @@ -43,16 +41,16 @@ public class RobotContainer // buttons are quick rotation positions to different ways to face // WARNING: default buttons are on the same buttons as the ones defined in configureBindings AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, - () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), - OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), - OperatorConstants.LEFT_X_DEADBAND), - () -> -MathUtil.applyDeadband(driverXbox.getRightX(), - OperatorConstants.RIGHT_X_DEADBAND), - driverXbox.getHID()::getYButtonPressed, - driverXbox.getHID()::getAButtonPressed, - driverXbox.getHID()::getXButtonPressed, - driverXbox.getHID()::getBButtonPressed); + () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), + OperatorConstants.LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), + OperatorConstants.LEFT_X_DEADBAND), + () -> -MathUtil.applyDeadband(driverXbox.getRightX(), + OperatorConstants.RIGHT_X_DEADBAND), + driverXbox.getHID()::getYButtonPressed, + driverXbox.getHID()::getAButtonPressed, + driverXbox.getHID()::getXButtonPressed, + driverXbox.getHID()::getBButtonPressed); // Applies deadbands and inverts controls because joysticks // are back-right positive while robot @@ -98,7 +96,8 @@ public RobotContainer() */ private void configureBindings() { - if (DriverStation.isTest()) { + if (DriverStation.isTest()) + { driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand()); driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2)); @@ -107,13 +106,14 @@ private void configureBindings() driverXbox.leftBumper().onTrue(Commands.none()); driverXbox.rightBumper().onTrue(Commands.none()); drivebase.setDefaultCommand( - !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim); - } else { + !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim); + } else + { driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); driverXbox.b().whileTrue( Commands.deferredProxy(() -> drivebase.driveToPose( - new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) + new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) )); driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); driverXbox.start().whileTrue(Commands.none()); @@ -121,7 +121,7 @@ private void configureBindings() driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); driverXbox.rightBumper().onTrue(Commands.none()); drivebase.setDefaultCommand( - !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim); + !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim); } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 6f370a44..a92904f0 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -27,7 +27,7 @@ public class AbsoluteDriveAdv extends Command private final DoubleSupplier vX, vY; private final DoubleSupplier headingAdjust; private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; - private boolean resetHeading = false; + private boolean resetHeading = false; /** * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 7a109ced..14c29616 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -4,20 +4,12 @@ package frc.robot.subsystems.swervedrive; -import java.io.File; -import java.util.Arrays; -import java.util.function.DoubleSupplier; - -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -32,18 +24,21 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; import frc.robot.Constants.AutonConstants; +import java.io.File; +import java.util.Arrays; +import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; import swervelib.math.SwerveMath; -import swervelib.parser.PIDFConfig; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; @@ -53,10 +48,6 @@ public class SwerveSubsystem extends SubsystemBase { - /** - * PhotonVision class to keep an accurate odometry. - */ - private Vision vision; /** * Swerve drive object. */ @@ -69,6 +60,10 @@ public class SwerveSubsystem extends SubsystemBase * Enable vision odometry updates while driving. */ private final boolean visionDriveTest = false; + /** + * PhotonVision class to keep an accurate odometry. + */ + private Vision vision; /** * Initialize {@link SwerveDrive} with the directory provided. @@ -104,8 +99,11 @@ public SwerveSubsystem(File directory) } swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. - swerveDrive.setAngularVelocityCompensation(true, true, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. - swerveDrive.setModuleEncoderAutoSynchronize(false, 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. + swerveDrive.setAngularVelocityCompensation(true, + true, + 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. + swerveDrive.setModuleEncoderAutoSynchronize(false, + 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible if (visionDriveTest) { @@ -367,23 +365,26 @@ public Command sysIdAngleMotorCommand() * * @return a Command that centers the modules of the SwerveDrive subsystem */ - public Command centerModulesCommand() { + public Command centerModulesCommand() + { return run(() -> Arrays.asList(swerveDrive.getModules()) - .forEach(it -> it.setAngle(0.0))); + .forEach(it -> it.setAngle(0.0))); } /** * Returns a Command that drives the swerve drive to a specific distance at a given speed. * - * @param distanceInMeters the distance to drive in meters + * @param distanceInMeters the distance to drive in meters * @param speedInMetersPerSecond the speed at which to drive in meters per second * @return a Command that drives the swerve drive to a specific distance at a given speed */ - public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { + public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) + { return Commands.deferredProxy( () -> Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this) - .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > distanceInMeters) - ); + .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > + distanceInMeters) + ); } /** @@ -391,18 +392,22 @@ public Command driveToDistanceCommand(double distanceInMeters, double speedInMet * * @param maximumSpeedInMetersPerSecond the maximum speed to set for the swerve drive in meters per second */ - public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) { - swerveDrive.setMaximumSpeed(maximumSpeedInMetersPerSecond, false, swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); + public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) + { + swerveDrive.setMaximumSpeed(maximumSpeedInMetersPerSecond, + false, + swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage); } /** * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. * - * @param kS the static gain of the feedforward - * @param kV the velocity gain of the feedforward - * @param kA the acceleration gain of the feedforward + * @param kS the static gain of the feedforward + * @param kV the velocity gain of the feedforward + * @param kA the acceleration gain of the feedforward */ - public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { + public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) + { swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 17d41b4c..ae8f67b3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -17,9 +17,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.robot.Robot; import java.awt.Desktop; -import java.io.IOException; -import java.net.URI; -import java.net.URISyntaxException; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -52,6 +49,10 @@ public class Vision */ public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( AprilTagFields.k2024Crescendo); + /** + * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. + */ + private final double maximumAmbiguity = 0.25; /** * Photon Vision Simulation */ @@ -64,10 +65,6 @@ public class Vision * Current pose from the pose estimator using wheel odometry. */ private Supplier currentPose; - /** - * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. - */ - private final double maximumAmbiguity = 0.25; /** * Field from {@link swervelib.SwerveDrive#field} */ diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 415f0bb8..7d83e5d0 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -104,18 +104,19 @@ public class SwerveDrive */ public boolean autonomousChassisVelocityCorrection = false; /** - * Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} + * Correct for skew that scales with angular velocity in + * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} */ - public boolean angularVelocityCorrection = false; - /** - * Correct for skew that scales with angular velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} - * during auto. + public boolean angularVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in + * {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto. */ - public boolean autonomousAngularVelocityCorrection = false; + public boolean autonomousAngularVelocityCorrection = false; /** * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15). */ - public double angularVelocityCoefficient = 0; + public double angularVelocityCoefficient = 0; /** * Whether to correct heading when driving translationally. Set to true to enable. */ @@ -133,10 +134,10 @@ public class SwerveDrive */ private SwerveIMU imu; /** - * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in + * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. */ - private IMUVelocity imuVelocity; + private IMUVelocity imuVelocity; /** * Simulation of the swerve drive. */ @@ -586,11 +587,12 @@ public double getMaximumAngularVelocity() /** * Set the module states (azimuth and velocity) directly. * - * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param desiredStates A list of SwerveModuleStates to send to the modules. * @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. */ - private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, boolean isOpenLoop) + private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, + boolean isOpenLoop) { // Desaturates wheel speeds if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) @@ -612,10 +614,10 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds } /** - * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. - * Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates, - * ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, - * double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) + * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. Does not allow for usage of + * desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds desiredChassisSpeed, double + * attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double + * attainableMaxRotationalVelocityRadiansPerSecond) * * @param desiredStates A list of SwerveModuleStates to send to the modules. * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. @@ -640,7 +642,9 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection); + chassisSpeeds = movementOptimizations(chassisSpeeds, + autonomousChassisVelocityCorrection, + autonomousAngularVelocityCorrection); SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; @@ -869,13 +873,15 @@ public void setMotorIdleMode(boolean brake) } /** - * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. - * @param enabled Enable state + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state * @param deadband Deadband in degrees, default is 3 degrees. */ public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband) { - for(SwerveModule swerveModule : swerveModules) + for (SwerveModule swerveModule : swerveModules) { swerveModule.setEncoderAutoSynchronize(enabled, deadband); } @@ -1259,7 +1265,7 @@ public void setCosineCompensator(boolean enabled) */ public void setChassisDiscretization(boolean enable, double dtSeconds) { - if(!SwerveDriveTelemetry.isSimulation) + if (!SwerveDriveTelemetry.isSimulation) { chassisVelocityCorrection = enable; discretizationdtSeconds = dtSeconds; @@ -1267,17 +1273,18 @@ public void setChassisDiscretization(boolean enable, double dtSeconds) } /** - * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop + * and/or auto * - * @param useInTeleop Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop. - * @param useInAuto Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto. - * @param dtSeconds The duration of the timestep the speeds should be applied for. + * @param useInTeleop Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop. + * @param useInAuto Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto. + * @param dtSeconds The duration of the timestep the speeds should be applied for. */ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds) { - if(!SwerveDriveTelemetry.isSimulation) + if (!SwerveDriveTelemetry.isSimulation) { chassisVelocityCorrection = useInTeleop; autonomousChassisVelocityCorrection = useInAuto; @@ -1286,22 +1293,22 @@ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, dou } /** - * Enables angular velocity skew correction in teleop and/or autonomous - * and sets the angular velocity coefficient for both modes + * Enables angular velocity skew correction in teleop and/or autonomous and sets the angular velocity coefficient for + * both modes * * @param useInTeleop Enables angular velocity correction in teleop. * @param useInAuto Enables angular velocity correction in autonomous. - * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. - * Start with a value of 0.1, test in teleop. - * When enabling for the first time if the skew is significantly worse try inverting the value. - * Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick. - * Change the value until you are visually happy with the skew. - * Ensure your tune works with different translational and rotational magnitudes. - * If this reduces skew in teleop, it may improve auto. + * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. Start with a + * value of 0.1, test in teleop. When enabling for the first time if the skew is + * significantly worse try inverting the value. Tune by moving in a straight line while + * rotating. Testing is best done with angular velocity controls on the right stick. + * Change the value until you are visually happy with the skew. Ensure your tune works + * with different translational and rotational magnitudes. If this reduces skew in teleop, + * it may improve auto. */ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff) { - if(!SwerveDriveTelemetry.isSimulation) + if (!SwerveDriveTelemetry.isSimulation) { imuVelocity = IMUVelocity.createIMUVelocity(imu); angularVelocityCorrection = useInTeleop; @@ -1313,20 +1320,21 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut /** * Correct for skew that worsens as angular velocity increases * - * @param velocity The chassis speeds to set the robot to achieve. + * @param velocity The chassis speeds to set the robot to achieve. * @return {@link ChassisSpeeds} of the robot after angular velocity skew correction. */ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity) { var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient); - if(angularVelocity.getRadians() != 0.0){ - velocity = ChassisSpeeds.fromRobotRelativeSpeeds( - velocity.vxMetersPerSecond, - velocity.vyMetersPerSecond, - velocity.omegaRadiansPerSecond, - getOdometryHeading()); - velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); - } + if (angularVelocity.getRadians() != 0.0) + { + velocity = ChassisSpeeds.fromRobotRelativeSpeeds( + velocity.vxMetersPerSecond, + velocity.vyMetersPerSecond, + velocity.omegaRadiansPerSecond, + getOdometryHeading()); + velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); + } return velocity; } @@ -1336,12 +1344,13 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity) * @param velocity The chassis speeds to set the robot to achieve. * @param uesChassisDiscretize Correct chassis velocity using 254's correction. * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew. - * @return The chassis speeds after optimizations. + * @return The chassis speeds after optimizations. */ - private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection) + private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, + boolean useAngularVelocitySkewCorrection) { - if(useAngularVelocitySkewCorrection) + if (useAngularVelocitySkewCorrection) { velocity = angularVelocitySkewCorrection(velocity); } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 2cdbf449..8b5dec10 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -95,7 +95,7 @@ public class SwerveModule /** * Anti-Jitter AKA auto-centering disabled. */ - private boolean antiJitterEnabled = true; + private boolean antiJitterEnabled = true; /** * Last swerve module state applied. */ @@ -111,15 +111,15 @@ public class SwerveModule /** * Encoder synchronization queued. */ - private boolean synchronizeEncoderQueued = false; + private boolean synchronizeEncoderQueued = false; /** * Encoder, Absolute encoder synchronization enabled. */ - private boolean synchronizeEncoderEnabled = false; + private boolean synchronizeEncoderEnabled = false; /** * Encoder synchronization deadband in degrees. */ - private double synchronizeEncoderDeadband = 3; + private double synchronizeEncoderDeadband = 3; /** @@ -257,8 +257,10 @@ public void queueSynchronizeEncoders() } /** - * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. - * @param enabled Enable state + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state * @param deadband Deadband in degrees, default is 3 degrees. */ public void setEncoderAutoSynchronize(boolean enabled, double deadband) @@ -268,7 +270,9 @@ public void setEncoderAutoSynchronize(boolean enabled, double deadband) } /** - * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * * @param enabled Enable state */ public void setEncoderAutoSynchronize(boolean enabled) @@ -386,7 +390,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled) { double absoluteEncoderPosition = getAbsolutePosition(); - if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) { + if (Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) + { angleMotor.setPosition(absoluteEncoderPosition); } angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); diff --git a/src/main/java/swervelib/imu/CanandgyroSwerve.java b/src/main/java/swervelib/imu/CanandgyroSwerve.java index 1dc09956..aed3a146 100644 --- a/src/main/java/swervelib/imu/CanandgyroSwerve.java +++ b/src/main/java/swervelib/imu/CanandgyroSwerve.java @@ -1,17 +1,12 @@ package swervelib.imu; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.Pigeon2Configurator; -import com.ctre.phoenix6.hardware.Pigeon2; import com.reduxrobotics.sensors.canandgyro.Canandgyro; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; /** - * SwerveIMU interface for the Boron Candandgyro by Redux Robotics + * SwerveIMU interface for the Boron {@link Canandgyro} by Redux Robotics */ public class CanandgyroSwerve extends SwerveIMU { @@ -19,24 +14,24 @@ public class CanandgyroSwerve extends SwerveIMU /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; + public static double STATUS_TIMEOUT_SECONDS = 0.04; /** - * Boron Canandgyro by Redux Robotics. + * Boron {@link Canandgyro} by Redux Robotics. */ - Canandgyro imu; + private final Canandgyro imu; /** - * Offset for the Boron Canandgyro. + * Offset for the Boron {@link Canandgyro}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * Generate the SwerveIMU for {@link Canandgyro}. * - * @param canid CAN ID for the Boron Canandgyro + * @param canid CAN ID for the Boron {@link Canandgyro} */ public CanandgyroSwerve(int canid) { @@ -44,7 +39,7 @@ public CanandgyroSwerve(int canid) } /** - * Reset IMU to factory default. + * Reset {@link Canandgyro} to factory default. */ @Override public void factoryDefault() @@ -53,7 +48,7 @@ public void factoryDefault() } /** - * Clear sticky faults on IMU. + * Clear sticky faults on {@link Canandgyro}. */ @Override public void clearStickyFaults() @@ -128,7 +123,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated {@link Canandgyro} IMU object. * * @return IMU object. */ diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java index fba47a34..c1c0f775 100644 --- a/src/main/java/swervelib/imu/IMUVelocity.java +++ b/src/main/java/swervelib/imu/IMUVelocity.java @@ -8,48 +8,48 @@ /** * Generic IMU Velocity filter. */ -public class IMUVelocity { +public class IMUVelocity +{ + /** * Swerve IMU. */ - private final SwerveIMU gyro; + private final SwerveIMU gyro; /** - * Linear filter used to calculate velocity, we use a custom filter class - * to prevent unwanted operations. + * Linear filter used to calculate velocity, we use a custom filter class to prevent unwanted operations. */ private final IMULinearMovingAverageFilter velocityFilter; /** * WPILib {@link Notifier} to keep IMU velocity up to date. */ - private final Notifier notifier; + private final Notifier notifier; /** * Prevents calculation when no previous measurement exists. */ - private boolean firstCycle = true; + private boolean firstCycle = true; /** * Tracks the previous loop's recorded time. */ - private double timestamp = 0.0; + private double timestamp = 0.0; /** - * Tracks the previous loop's position as a Rotation2d. + * Tracks the previous loop's position as a Rotation2d. */ - private Rotation2d position = new Rotation2d(); + private Rotation2d position = new Rotation2d(); /** * The calculated velocity of the robot based on averaged IMU measurements. */ - private double velocity = 0.0; + private double velocity = 0.0; /** * Constructor for the IMU Velocity. * - * @param gyro The SwerveIMU gyro. + * @param gyro The SwerveIMU gyro. * @param periodSeconds The rate to collect measurements from the gyro, in the form (1/number of samples per second), - * make sure this does not exceed the update rate of your IMU. + * make sure this does not exceed the update rate of your IMU. * @param averagingTaps The number of samples to used for the moving average linear filter. Higher values will not - * allow the system to update to changes in velocity, lower values may create a less smooth signal. Expected taps - * will probably be ~2-8, with the goal of having the lowest smooth value. - * + * allow the system to update to changes in velocity, lower values may create a less smooth + * signal. Expected taps will probably be ~2-8, with the goal of having the lowest smooth value. */ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps) { @@ -61,51 +61,51 @@ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps) } /** - * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible - * but will not exceed 100hz and will use 5 taps (supported IMUs are listed in swervelib's IMU folder). - * Other gyroscopes will default to 50hz and 5 taps. For custom rates please use the IMUVelocity constructor. + * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible but will not exceed 100hz and + * will use 5 taps (supported IMUs are listed in swervelib's IMU folder). Other gyroscopes will default to 50hz and 5 + * taps. For custom rates please use the IMUVelocity constructor. * * @param gyro The SwerveIMU gyro. * @return {@link IMUVelocity} for the given gyro with adjusted period readings for velocity. */ public static IMUVelocity createIMUVelocity(SwerveIMU gyro) { - double desiredNotifierPeriod = 1.0/50.0; + double desiredNotifierPeriod = 1.0 / 50.0; // ADIS16448_IMU ~200HZ: // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277 if (gyro instanceof ADIS16448Swerve) { - desiredNotifierPeriod = 1.0/100.0; + desiredNotifierPeriod = 1.0 / 100.0; } // ADIS16470_IMU 200HZ // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345 else if (gyro instanceof ADIS16470Swerve) { - desiredNotifierPeriod = 1.0/100.0; + desiredNotifierPeriod = 1.0 / 100.0; } // ADXRS450_Gyro 2000HZ? // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31 else if (gyro instanceof ADXRS450Swerve) { - desiredNotifierPeriod = 1.0/100.0; + desiredNotifierPeriod = 1.0 / 100.0; } // NAX (AHRS): 60HZ // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61 else if (gyro instanceof NavXSwerve) { - desiredNotifierPeriod = 1.0/60.0; + desiredNotifierPeriod = 1.0 / 60.0; } // Pigeon2 100HZ // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf else if (gyro instanceof Pigeon2Swerve) { - desiredNotifierPeriod = 1.0/100.0; + desiredNotifierPeriod = 1.0 / 100.0; } // Pigeon 100HZ // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf else if (gyro instanceof PigeonSwerve) { - desiredNotifierPeriod = 1.0/100.0; + desiredNotifierPeriod = 1.0 / 100.0; } return new IMUVelocity(gyro, desiredNotifierPeriod, 5); } @@ -113,15 +113,17 @@ else if (gyro instanceof PigeonSwerve) /** * Update the robot's rotational velocity based on the current gyro position. */ - private void update() + private void update() { - double newTimestamp = HALUtil.getFPGATime(); - Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); + double newTimestamp = HALUtil.getFPGATime(); + Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); - synchronized (this) { - if (!firstCycle) { + synchronized (this) + { + if (!firstCycle) + { velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp)); - } + } firstCycle = false; timestamp = newTimestamp; position = newPosition; @@ -129,12 +131,13 @@ private void update() } /** - * Get the robot's angular velocity based on averaged meaasurements from the IMU. - * Velocity is multiplied by 1e+6 (1,000,000) because the timestamps are in microseconds. + * Get the robot's angular velocity based on averaged meaasurements from the IMU. Velocity is multiplied by 1e+6 + * (1,000,000) because the timestamps are in microseconds. * * @return robot's angular velocity in rads/s as a double. */ - public synchronized double getVelocity() { + public synchronized double getVelocity() + { velocity = velocityFilter.calculate(); return velocity * 1e+6; } diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 1adb6e19..2d6454ef 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -11,7 +11,7 @@ import swervelib.telemetry.Alert; /** - * Communicates with the NavX as the IMU. + * Communicates with the NavX({@link AHRS}) as the IMU. */ public class NavXSwerve extends SwerveIMU { @@ -34,7 +34,7 @@ public class NavXSwerve extends SwerveIMU private Alert navXError; /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port Serial Port to connect to. */ @@ -57,7 +57,7 @@ public NavXSwerve(SerialPort.Port port) } /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port SPI Port to connect to. */ @@ -79,7 +79,7 @@ public NavXSwerve(SPI.Port port) } /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port I2C Port to connect to. */ @@ -101,7 +101,8 @@ public NavXSwerve(I2C.Port port) } /** - * Reset IMU to factory default. + * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be + * too slow. */ @Override public void factoryDefault() @@ -188,7 +189,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated NavX({@link AHRS}) IMU object. * * @return IMU object. */ diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 22160d86..574da147 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -10,7 +10,7 @@ import java.util.Optional; /** - * SwerveIMU interface for the Pigeon2 + * SwerveIMU interface for the {@link Pigeon2} */ public class Pigeon2Swerve extends SwerveIMU { @@ -18,29 +18,29 @@ public class Pigeon2Swerve extends SwerveIMU /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; + public static double STATUS_TIMEOUT_SECONDS = 0.04; /** - * Pigeon2 IMU device. + * {@link Pigeon2} IMU device. */ - Pigeon2 imu; + private final Pigeon2 imu; /** - * Offset for the Pigeon 2. + * Offset for the {@link Pigeon2}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** - * Pigeon2 configurator. + * {@link Pigeon2} configurator. */ - private Pigeon2Configurator cfg; + private Pigeon2Configurator cfg; /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link Pigeon2}. * - * @param canid CAN ID for the pigeon - * @param canbus CAN Bus name the pigeon resides on. + * @param canid CAN ID for the {@link Pigeon2} + * @param canbus CAN Bus name the {@link Pigeon2} resides on. */ public Pigeon2Swerve(int canid, String canbus) { @@ -50,9 +50,9 @@ public Pigeon2Swerve(int canid, String canbus) } /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link Pigeon2}. * - * @param canid CAN ID for the pigeon + * @param canid CAN ID for the {@link Pigeon2} */ public Pigeon2Swerve(int canid) { @@ -60,7 +60,7 @@ public Pigeon2Swerve(int canid) } /** - * Reset IMU to factory default. + * Reset {@link Pigeon2} to factory default. */ @Override public void factoryDefault() @@ -72,7 +72,7 @@ public void factoryDefault() } /** - * Clear sticky faults on IMU. + * Clear sticky faults on {@link Pigeon2}. */ @Override public void clearStickyFaults() @@ -153,7 +153,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated {@link Pigeon2} object. * * @return IMU object. */ diff --git a/src/main/java/swervelib/imu/PigeonSwerve.java b/src/main/java/swervelib/imu/PigeonSwerve.java index 9515b0d1..e2dfafe0 100644 --- a/src/main/java/swervelib/imu/PigeonSwerve.java +++ b/src/main/java/swervelib/imu/PigeonSwerve.java @@ -8,28 +8,28 @@ import java.util.Optional; /** - * SwerveIMU interface for the Pigeon. + * SwerveIMU interface for the {@link WPI_PigeonIMU}. */ public class PigeonSwerve extends SwerveIMU { /** - * Pigeon v1 IMU device. + * {@link WPI_PigeonIMU} IMU device. */ - WPI_PigeonIMU imu; + private final WPI_PigeonIMU imu; /** - * Offset for the Pigeon. + * Offset for the {@link WPI_PigeonIMU}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link WPI_PigeonIMU}. * - * @param canid CAN ID for the pigeon, does not support CANBus. + * @param canid CAN ID for the {@link WPI_PigeonIMU}, does not support CANBus. */ public PigeonSwerve(int canid) { @@ -126,7 +126,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated {@link WPI_PigeonIMU} IMU object. * * @return IMU object. */ diff --git a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java index f2a008a7..09509b75 100644 --- a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java +++ b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java @@ -3,10 +3,10 @@ import edu.wpi.first.util.DoubleCircularBuffer; /** - * A linear filter that does not calculate() each time a value is added to - * the DoubleCircularBuffer. + * A linear filter that does not calculate() each time a value is added to the DoubleCircularBuffer. */ -public class IMULinearMovingAverageFilter { +public class IMULinearMovingAverageFilter +{ /** * Circular buffer storing the current IMU readings @@ -16,39 +16,42 @@ public class IMULinearMovingAverageFilter { * Gain on each reading. */ private final double m_inputGain; - - /** - * Construct a linear moving average fitler - * @param bufferLength The number of values to average across - */ + + /** + * Construct a linear moving average fitler + * + * @param bufferLength The number of values to average across + */ public IMULinearMovingAverageFilter(int bufferLength) { m_inputs = new DoubleCircularBuffer(bufferLength); m_inputGain = 1.0 / bufferLength; } - /** - * Add a value to the DoubleCircularBuffer - * @param input Value to add - */ + /** + * Add a value to the DoubleCircularBuffer + * + * @param input Value to add + */ public void addValue(double input) { m_inputs.addFirst(input); } - /** - * Calculate the average of the samples in the buffer - * @return The average of the values in the buffer - */ + /** + * Calculate the average of the samples in the buffer + * + * @return The average of the values in the buffer + */ public double calculate() { double returnVal = 0.0; - for(int i = 0; i < m_inputs.size(); i++) - { - returnVal += m_inputs.get(i) * m_inputGain; - } - - return returnVal; + for (int i = 0; i < m_inputs.size(); i++) + { + returnVal += m_inputs.get(i) * m_inputGain; + } + + return returnVal; } } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index d8fb2a8a..f3ba23af 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -17,7 +17,7 @@ import swervelib.telemetry.Alert; /** - * Brushed motor control with SparkMax. + * Brushed motor control with {@link CANSparkMax}. */ public class SparkMaxBrushedMotorSwerve extends SwerveMotor { diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 0f014173..0e930ad9 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -252,7 +252,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50); } // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use - else if(absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) + else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) { configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200); } diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index a2e5a6d8..2fa83c89 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -1,6 +1,5 @@ package swervelib.parser.json; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import edu.wpi.first.math.util.Units; @@ -12,7 +11,6 @@ import swervelib.parser.json.modules.BoolMotorJson; import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.parser.json.modules.LocationJson; -import swervelib.telemetry.Alert; /** * {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data.